Sphincter-firmware/src/getting_started.ino

/**************************************************************************/
/*!
@file     getting_started.ino
@author   Moritz Walter
@license  GPLv3 (see license.txt)

Demo of SPI configuration tool for Trinamic TMC2130 Motor Drivers

@section  HISTORY

v0.1 - it works
*/
/**************************************************************************/

    ////////////////////
   //
  //    HOW TO USE
 //
////////////////////

// This tool uses hardware SPI
// Just wire the drivers to your SPI pins and define the CS pin below.
// Once flashed, the sketch sends a basic configuration to the driver and makes the motor spin.

#include <SPI.h>
#include <Trinamic_TMC2130.h>

// pin configuration (this is the X-axis on a standard RAMPS 1.4)
#define CS_PIN 7
#define EN_PIN 8 //enable (CFG6)
#define DIR_PIN 9 //direction
#define STEP_PIN 10  //step

Trinamic_TMC2130 myStepper(CS_PIN);

#define PRINT_REG(NAME, regval)                \
    Serial.print(#NAME ": ");                   \
    myStepper.read_REG(NAME, &regval);  \
    Serial.println(regval, BIN);                  \

void print_regdump()
{
  uint32_t regval;

  PRINT_REG(TMC_REG_GCONF, regval)
  PRINT_REG(TMC_REG_GSTAT, regval)
  PRINT_REG(TMC_REG_IOIN, regval)
  PRINT_REG(TMC_REG_TSTEP, regval)
  PRINT_REG(TMC_REG_XDIRECT, regval)
  PRINT_REG(TMC_REG_MSCNT, regval)
  PRINT_REG(TMC_REG_MSCURACT, regval)
  PRINT_REG(TMC_REG_CHOPCONF, regval)
  PRINT_REG(TMC_REG_DRV_STATUS, regval)
  PRINT_REG(TMC_REG_PWM_SCALE, regval)
  PRINT_REG(TMC_REG_LOST_STEPS, regval)

}

void setup(){
  Serial.begin(115200);
  Serial.println("Waiting for serial...");
  
  while (!Serial);

  Serial.println("Hello");

  // pins
  pinMode(EN_PIN, OUTPUT);
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
  digitalWrite(EN_PIN, HIGH); // disable driver
  digitalWrite(DIR_PIN, LOW); // chose direction
  digitalWrite(STEP_PIN, LOW); // no step yet
  digitalWrite(EN_PIN, LOW); // enable driver

  // stepper
  myStepper.init();

  uint32_t ioin;
  myStepper.read_REG(TMC_REG_IOIN, &ioin); 
  if (ioin >> 24 != 0x11) {
      Serial.println("SPI Communication failed or incompatible firmware");
      while (true);
  }
  
  // verify connection
  uint32_t chopconf;
  myStepper.read_REG(TMC_REG_CHOPCONF, &chopconf);
  chopconf = chopconf ^ 0x05;
  myStepper.write_REG(TMC_REG_CHOPCONF, chopconf);
  
  uint32_t chopconf_r;
  myStepper.read_REG(TMC_REG_CHOPCONF, &chopconf_r);
  

  if (chopconf != chopconf_r) {
      Serial.println("Could not set CHOPCONF!!!");
      Serial.print("Expected ");
      Serial.print(chopconf);
      Serial.print(" Got ");
      Serial.print(chopconf_r);
      while (true);
  }
  
  chopconf = chopconf ^ 0x05;
  myStepper.write_REG(TMC_REG_CHOPCONF, chopconf);


  //myStepper.set_mres(64); // ({1,2,4,8,16,32,64,128,256}) number of microsteps
  //myStepper.set_IHOLD_IRUN(31,31,5); // ([0-31],[0-31],[0-5]) sets all currents to maximum
  //myStepper.set_I_scale_analog(1); // ({0,1}) 0: I_REF internal, 1: sets I_REF to AIN
  //myStepper.set_tbl(1); // ([0-3]) set comparator blank time to 16, 24, 36 or 54 clocks, 1 or 2 is recommended


  myStepper.set_mres(128);

//  myStepper.set_chm(2);
//  myStepper.set_toff(8); // ([0-15]) 0: driver disable, 1: use only with TBL>2, 2-15: off time setting during slow decay phase


  myStepper.set_hstrt(0);
  myStepper.set_hend(13);


  // configure motor current
  myStepper.set_IHOLD_IRUN(1,31,31);
  myStepper.set_I_scale_analog(1);
  
  myStepper.set_tbl(4);
  
  // configure dcStep  mode
  myStepper.set_VDCMIN(1);
  myStepper.set_DCCTRL(50, 10);

  myStepper.set_vhighchm(1);
  myStepper.set_vhighfs(1);

  myStepper.set_sgt(127);

  myStepper.set_TCOOLTHRS(1);
  myStepper.set_TPWMTHRS(1);


  myStepper.set_toff(8); // ([0-15]) 0: driver disable, 1: use only with TBL>2, 2-15: off time setting during slow decay phase

  Serial.println("Finished Configuration: ");
  print_regdump();

  // get ready
  digitalWrite(EN_PIN, LOW); // enable driver
}

void loop(){
  // make a step
  digitalWrite(STEP_PIN, HIGH);
  delayMicroseconds(42);
  digitalWrite(STEP_PIN, LOW);
  delayMicroseconds(42);

  static int sometimes = 0;
  if (sometimes++ > 100) {
    sometimes = 0;
    
    while (!Serial);

    uint32_t drv_status = 0;
    myStepper.read_REG(TMC_REG_DRV_STATUS, &drv_status);
    uint32_t stallguard = drv_status & 0x1ff;  
    uint32_t current = (drv_status >> 16) & 0x1f;

    Serial.print("Stallguard value: ");
    Serial.println(stallguard);

    Serial.print("Current: ");
    Serial.println(current);

    int32_t lost_steps;
    myStepper.read_REG(TMC_REG_LOST_STEPS, &lost_steps);

    static int32_t last_lost_steps = 0;
    
    Serial.print("Lost steps: ");
    Serial.println(lost_steps);
    
    const int STEP_THRESHOLD = 5;
    const int DIRCHANGE_HOLDOFF = 2;
    /*
    static int dirchange_timeout = DIRCHANGE_HOLDOFF;

    if ((lost_steps >= last_lost_steps + STEP_THRESHOLD) || (lost_steps <= last_lost_steps - STEP_THRESHOLD)  ) {
      if (--dirchange_timeout < 0) {
        
        Serial.print("Lost Steps: ");
        Serial.println(lost_steps);
        
        Serial.print("Last Lost Steps: ");
        Serial.println(lost_steps);
        
        Serial.print("dirchange_timeout: ");
        Serial.println(dirchange_timeout);

        digitalWrite(DIR_PIN, !digitalRead(DIR_PIN));
        
        last_lost_steps = lost_steps;
        dirchange_timeout = DIRCHANGE_HOLDOFF;
      }
    }
    */
  }
}