motor_control.h

motor_control.h

/* The MorningRod uses an ESP32 and TMC5130 motor driver

The datasheet for the TMC can be found here:
https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC5130_datasheet_Rev1.15.pdf

The TMC works by having the ESP manipulate bits in the motor driver.
The TMC is not an ordinary driver, it is a ver smart Motion Controller.
It can function completely on its own. All the ESP does is set bits to change the properties of the ESP.

It is very simple to do, here is how:

To set a parameter, the sendData() function initiates an SPI transfer from the ESP to the TMC in the following format:

sendData(0x10+0x80, 0xA00);

In this example, 0x10 is the address we want to change. Go to the datasheet and you will find it on page 33, Section 6.2
This is the IHOLD_IRUN parameter which allows us to set the current we want the motor to use.

We then add 0x80 to this address in order to give it write permission (Page 29 Note #2)

Then we send the value we want (0xA00) in either hex or decimal format.

In this case, we want to change the IRUN value to change the running current, bits 8...12.

But notice that the entire register uses 20 bits (0-19)

In this case, we only want to change bits 8-12, which corresponds to a number that is 0-31 (we will get to that later)

Let's say we want to send the value '10' to this register, here is what it will look like...

19 18 17 16 15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 //Bit numbers
--------------------------------------------------
 0  0  0  0  0  0  0  0  1  0 1 0 0 0 0 0 0 0 0 0 // Binary value
 --------------------16--8--4-2-1-----------------// Decimal Value

 In the above example, I have laid out our 20 bits. Bit 9 and 11 are flipped in order to create the value '10'
This is because bit 12 has a value of 16, and bit 8 is 1.

 In this case, our binary value is 00000000101000000000, which is 0xA00

 So we have sent the value '10' to bits 8..12, how does this change the current?
 Current is changed by using the formula on page 67

 It uses the formula at the bottom of the page to determine current. The value '10' we sent is the CS value
 The Rsense value is either 0.075 ohms or 0.091, depending on the version of your PCB

 Using these values, you can determine and set the current.
*/

//Here is all the relevant code that I think is needed to run the MorningRod

#define chipCS 27
#define SCLK 26
#define MISO 33
#define MOSI 25

#define CLOCKOUT 15
#define ENABLE_PIN 32
#define btn1 17
#define btn2 16

#define DIR_OPEN 1
#define DIR_CLOSE 2

#define MOVE_OPEN 5
#define MOVE_CLOSE 6

#define COOLCONF_DEFAULT 0

#define GET_VELOCITY preferences.getLong("velocity",100000)
#define STALLGUARD preferences.getInt("stallguard", -9)

#define MOVE_DISTANCE preferences.getFloat("move_distance", 0)
#define MOVE_PERCENT preferences.getFloat("move_percent", 389911.13)

#include "driver/ledc.h"
#include "driver/periph_ctrl.h"
#include "driver/timer.h"

#include <Arduino.h>
#include <SPI.h>
#include <Preferences.h>

void setup() {

  Serial.begin(115200);

  pinMode(btn1,INPUT_PULLUP);
  pinMode(btn2,INPUT_PULLUP);

  // for storing the times over power cycle
  preferences.begin("auto-curtain", false);

  clockout_setup();
  setup_motors();
   
  }
}


void loop() {

while(true) {
    // buttons

    // A press sets the command to open or close the track motor.
    if(!digitalRead(btn1)){
      command = MOVE_CLOSE;
    }
    if(!digitalRead(btn2)){
      command = MOVE_OPEN;
    }
    
    // commands are sent from other threads so that blocking function calls can be
    // called without causing bizzare hickups in the other threads
    
    if(command!=-1){
      DEBUG_STREAM.print("Executing command ");
      DEBUG_STREAM.println(command);
    }
    if(command==MOVE_CLOSE){
      move_close();
      last_dir=DIR_CLOSE;
      //DEBUG_STREAM.println("Move Close Executed");
    }else if(command==MOVE_OPEN){
      move_open();
      last_dir=DIR_OPEN;
    }
    if(command!=-1)DEBUG_STREAM.println("[ready for next movement]");
    command = -1;
    delay(16);
  }
}


// Code to create clock frequency that is required by the TMC5130.
// See Page 114 section 28.2
void clockout_setup(){
  periph_module_enable(PERIPH_LEDC_MODULE);
  
  uint32_t bit_width = 2; // 1 - 20 bits
  uint32_t divider = 320; // Q10.8 fixed point number, 0x100 — 0x3FFFF
  uint32_t duty_cycle = 1 << (bit_width - 1);
  
  float freq_mhz = ((uint64_t) LEDC_APB_CLK_HZ << 8) / (double) divider / (1 << bit_width) / 1000000.0;
  printf("\nfrequency: %f MHz\n", freq_mhz);
  
  ledc_timer_set(LEDC_HIGH_SPEED_MODE, LEDC_TIMER_0, divider, bit_width, LEDC_APB_CLK);
  ledc_timer_rst(LEDC_HIGH_SPEED_MODE, LEDC_TIMER_0);
  ledc_timer_resume(LEDC_HIGH_SPEED_MODE, LEDC_TIMER_0);
  ledc_channel_config_t pwm_pin_cfg = {
  CLOCKOUT, // chosen GPIO output for clock
  LEDC_HIGH_SPEED_MODE, // speed mode
  LEDC_CHANNEL_0, // ledc channel
  LEDC_INTR_DISABLE, // interrupt type
  LEDC_TIMER_0, // timer select
  duty_cycle // duty cycle
  };
  ledc_channel_config(&pwm_pin_cfg);
}


extern Preferences preferences;   // preferences used to configure motor stallguard and curve values.

// These empty function definitions allow the functions to be called before they are created.
// They are written towards the bottom of the file.

unsigned long sendData(unsigned long address, unsigned long datagram);
void stopMotor(); // track motor is motor two
void delayStall(long timeout);
void waitStall(long timeout);
void turnMotor(int dir);

// these variables keep track of which motors are running
bool motor_running = false;

//Functions that closes the curtains (Called in the loop)
void move_close(){
  
  digitalWrite(ENABLE_PIN,LOW);       // enable the TMC5130
  sendData(0x10+0x80, 0x00011500);     // 25 = 1.97A current for close function //11500 =1.66A works/ 11600 works / 
  sendData(0xA0,0x00000000); //RAMPMODE=0
  
  sendData(0x14+0x80, 99000);//GET_VELOCITY-1); // VCOOLTHRS: This value disable stallGuard below a certain velocity to prevent premature stall

//Stallguard_open will need to change due to higher current than stallguard_close
  int q=STALLGUARD;
  DEBUG_STREAM.print("Stall Open value: ");
  DEBUG_STREAM.println(q);
  q&=0x6F;
  q=q<<16;
  sendData(0xED, COOLCONF_DEFAULT|q);     // STALLGUARD_OPEN
  
  //Ramp Profile Parameters
  //Change these values to change the acceleration and deceleration of the motor
  //Page 75 Section 14.2
  sendData(0x24+0x80, 1000); //A1
  sendData(0x26+0x80, 4000); //AMAX
  sendData(0x28+0x80, 800);     // DMAX
  sendData(0x2A+0x80, 3000); //D1
  sendData(0x23+0x80, 0);     // VSTART
  sendData(0x2B+0x80, 10); //VSTOP
  sendData(0x25+0x80, 40000); //V1
  sendData(0x27+0x80, GET_VELOCITY); //VMAX

  delay(5);
 
  // clear flags
  sendData(0x35, 0);
  sendData(0x34+0x80, 0x400); // Enable stallguard
  
  DEBUG_STREAM.println("Opening to: ");
  DEBUG_STREAM.println(-MOVE_PERCENT);
  
  sendData(0xAD, MOVE_PERCENT); //XTARGET: Positive makes it move right
  sendData(0xA1, 0); // set XACTUAL to zero
  
  motor_running = true;

    while(((sendData(0x35, 0)&0x200)==0)&&((sendData(0x35,0)&0x40)==0)){   // wait for position_reached flag OR a STALL EVENT
    delayMicroseconds(500);  // shortened the delay to make the following if statement more sensitive
  }

  stopMotor();
  digitalWrite(ENABLE_PIN,HIGH);
  Serial.println("[close complete]");
}

//Function that opens the curtains (Called in the loop)
void move_open(){

  digitalWrite(ENABLE_PIN,LOW);       // enable the TMC5130
  sendData(0x10+0x80, 0x00010A00);     // 13 = 1.06A current for open function
  sendData(0xA0,0x00000000); //RAMPMODE=0
  
  sendData(0x14+0x80, GET_VELOCITY-1); // VCOOLTHRS: This value disables stallGuard below a certain velocity to prevent premature stall

//Stallguard_open will need to change due to higher current than stallguard_close
  int q=STALLGUARD;
  DEBUG_STREAM.print("Stall Open value: ");
  DEBUG_STREAM.println(q);
  q&=0x6F;
  q=q<<16;
  sendData(0xED, COOLCONF_DEFAULT|q);     // STALLGUARD_OPEN
  
  //Ramp Profile Parameters
  //Change these values to change the acceleration and deceleration of the motor
  //Page 75 Section 14.2
  sendData(0x24+0x80, 1000); //A1
  sendData(0x26+0x80, 4000); //AMAX
  sendData(0x28+0x80, 800);     // DMAX
  sendData(0x2A+0x80, 3000); //D1
  sendData(0x23+0x80, 0);     // VSTART
  sendData(0x2B+0x80, 10); //VSTOP
  sendData(0x25+0x80, 40000); //V1
  sendData(0x27+0x80, GET_VELOCITY); //VMAX

  delay(5);
 
  // clear flags
  sendData(0x35, 0);
  sendData(0x34+0x80, 0x400); // Enable stallguard
  
  DEBUG_STREAM.println("Opening to: ");
  DEBUG_STREAM.println(MOVE_PERCENT);
  
  sendData(0xAD, -MOVE_PERCENT); //XTARGET: Positive makes it move right
  sendData(0xA1, 0); // set XACTUAL to zero
  
  motor_running = true;

    while(((sendData(0x35, 0)&0x200)==0)&&((sendData(0x35,0)&0x40)==0)){   // wait for position_reached flag OR a STALL EVENT
    delayMicroseconds(500);  // shortened the delay to make the following if statement more sensitive
  }

  stopMotor();
  digitalWrite(ENABLE_PIN,HIGH);
  Serial.println("[open complete]");
}


// this function disables the TMC5130
void opt_motors(){
  if(!(motor_running))digitalWrite(ENABLE_PIN,HIGH);
}

// gracefully stops motor and handles background optimizations
void stopMotor(){
  sendData(0x23+0x80, 0);             // set VMAX and VSTART to zero, then enable positioning mode
  sendData(0x27+0x80, 0);             //  > doing this stops the motor
  sendData(0x20+0x80, 0);             //
  while(sendData(0x22, 0)!=0)         // wait for the motor to stop (VACTUAL != 0 until stopped)
    delayMicroseconds(10);
  sendData(0x21+0x80, 0);             // target=xactual=0 to keep motor stopped
  sendData(0x2D+0x80, 0);             //
  sendData(0x23+0x80, 0x180);         // fix VMAX and VSTART to previous values so motor can run again
  sendData(0x27+0x80, GET_VELOCITY);  //
  motor_running = false;              // mark that the shaft motor is stopped
  opt_motors();                       // disable the motor driver if possible
}

/*  ====================================
 * 
 *    BEGIN MOTOR DRIVER BACKEND
 * 
 *  ====================================
 */

// exchange data with the TMC5130 (DO NOT EDIT THIS FUNCTION)
unsigned long sendData(unsigned long address, unsigned long datagram){
  //TMC5130 takes 40 bits of data: 8 address and 32 data
  delay(10);
  uint8_t stat;
  unsigned long i_datagram=0;
  digitalWrite(chipCS,LOW);
  delayMicroseconds(10);
  stat = SPI.transfer(address);
  i_datagram |= SPI.transfer((datagram >> 24) & 0xff);
  i_datagram <<= 8;
  i_datagram |= SPI.transfer((datagram >> 16) & 0xff);
  i_datagram <<= 8;
  i_datagram |= SPI.transfer((datagram >> 8) & 0xff);
  i_datagram <<= 8;
  i_datagram |= SPI.transfer((datagram) & 0xff);
  digitalWrite(chipCS,HIGH);
  return i_datagram;
}

//Motor setup code to run once in Setup.
void setup_motors(){ 
  pinMode(chipCS,OUTPUT);
  pinMode(CLOCKOUT,OUTPUT);
  pinMode(ENABLE_PIN, OUTPUT);
  digitalWrite(chipCS,HIGH);
  digitalWrite(ENABLE_PIN,LOW);

  SPI.setBitOrder(MSBFIRST);
  SPI.setClockDivider(SPI_CLOCK_DIV16);
  SPI.setDataMode(SPI_MODE3);
  SPI.begin(SCLK,MISO,MOSI,chipCS); // Edit 'pins.h' to change pins

  sendData(0x00+0x80, 0x0);            // General settings / en_pwm_mode OFF
  sendData(0x6C+0x80, 0x000101D5);     // CHOPCONF
  sendData(0x10+0x80, 0x00010D00);     // IHOLD_IRUN // 0x00011900 = 25 = 2 Amps // 0x00010D00 = 13 = 1 Amp
  sendData(0x20+0x80,0x00000000);      //RAMPMODE=0

  sendData(0x60+0x80,  0xAAAAB554);    // writing value 0xAAAAB554 = 0 = 0.0 to address 25 = 0x60(MSLUT[0])
  sendData(0x61+0x80,  0x4A9554AA);    // writing value 0x4A9554AA = 1251300522 = 0.0 to address 26 = 0x61(MSLUT[1])
  sendData(0x62+0x80,  0x24492929);    // writing value 0x24492929 = 608774441 = 0.0 to address 27 = 0x62(MSLUT[2])
  sendData(0x63+0x80,  0x10104222);    // writing value 0x10104222 = 269500962 = 0.0 to address 28 = 0x63(MSLUT[3])
  sendData(0x64+0x80,  0xFBFFFFFF);    // writing value 0xFBFFFFFF = 0 = 0.0 to address 29 = 0x64(MSLUT[4])
  sendData(0x65+0x80,  0xB5BB777D);    // writing value 0xB5BB777D = 0 = 0.0 to address 30 = 0x65(MSLUT[5])
  sendData(0x66+0x80,  0x49295556);    // writing value 0x49295556 = 1227445590 = 0.0 to address 31 = 0x66(MSLUT[6])
  sendData(0x67+0x80,  0x00404222);    // writing value 0x00404222 = 4211234 = 0.0 to address 32 = 0x67(MSLUT[7])
  sendData(0x68+0x80,  0xFFFF8056);    // writing value 0xFFFF8056 = 0 = 0.0 to address 33 = 0x68(MSLUTSEL)
  sendData(0x69+0x80,  0x00F70000);    // writing value 0x00F70000 = 16187392 = 0.0 to address 34 = 0x69(MSLUTSTART)
  sendData(0x70+0x80,  0x00000000);    // PWMCONF

  //Standard values for speed and acceleration
  int q=STALLGUARD;
  DEBUG_STREAM.print("Stall value: ");
  DEBUG_STREAM.println(q);
  q&=0x7F;
  q=q<<16;
  sendData(0x6D+0x80, COOLCONF_DEFAULT|q); // STALLGUARD
   
  stopMotor();

}


/*
Previous Blynk code

The motor needs to have certain parameters sent to it by the user

Here are several that are required

1) Digital open button via app
2) Digital close button via app
3) Set velocity parameter
4) Set stallGuard value (required when user is setting up motor for first time)
5) Set max distance (User needs to set the distance of travel in inches. Use the formula in function below to convert inches to steps)

Below is the old code used in Blynk to allow the user to control these parameters from the app. 
Use these as a reference
*/



BLYNK_WRITE(V122) { // set global velocity
  DEBUG_STREAM.print("set all velocity: ");
  long q=param.asInt()*1000L;
  preferences.putLong("velocity", q);
  DEBUG_STREAM.println(q);
  sendData(0xA7, q);     // VMAX_M1
}

BLYNK_WRITE(V123) { // set stallguard value
  DEBUG_STREAM.print("set stall: ");
  int q=param.asInt()-64;
  DEBUG_STREAM.println(q);
  if(q>63)q=63;
  if(q<-64)q=-64;
  preferences.putInt("stallguard", q);
  q&=0x7F;
  q=q<<16;
  sendData(0x6D+0x80, COOLCONF_DEFAULT|q);     // STALLGUARD
}

BLYNK_WRITE(V22) { // set distance value
  DEBUG_STREAM.print("set distance: ");
  preferences.putFloat("move_distance", param.asInt()*32492.59347);
  //float q=param.asInt()*32492.59347; // One inch require this many microsteps
  //preferences.putFloat("track_distance", q);
  DEBUG_STREAM.println(MOVE_DISTANCE);
}