Teensy 4.0 and TMC2130 StallGuard Sensorless Homing

Status
Not open for further replies.

BlueTurtle

Active member
Hello Everyone,

I have been trying to use the TMC2130 stepper driver and hopefully without endstop switches using the "StallGuard" feature. However I have been getting really frustrated with the set up and any help will be really appreciated.

I'm not going to post a schematic because I think the hardware is fine. I have checked my SPI lines with a logic analyzer and everything seems fine. I can control the driver via SPI, change motor direction, current, power mode etc just fine. It's the stallGuard sensorless homing function that I'm having trouble with.

I'm using the original TMC2130 stepper motor drivers (https://www.filastruder.com/product...tm_source=google&utm_campaign=Google Shopping).

I'm using the TMC2130 Library but the StallGuard example code there has been written for Arduino Mega and the timers don't really work for the Teensy. For that reason I adopted the code that I have found HERE, written for ESP32 with the Teensy timers.

Here is my code :

#define MOTOR_STEPS 200.0 // steps per rev typically 200 (1.8°) to 400 (0.9°)
#define MICROSTEPS 16
#define DRIVER_CURRENT_mA 750 // current in milliamps

#define MAX_SPEED_RPS 60
#define MIN_SPEED_RPS 2.3
#define INIT_SPEED_RPS 2.3 // a target
#define INIT_SPEED_ACTUAL 2.3 // accel from this to target
#define SPEED_INCR_VAL 0.2 // change amount per +,- inputs
#define ACCEL_RATE 0.05 // speed changes by this much per DISPLAY_INTERVAL until target speed

#define DISPLAY_INTERVAL 100 // in milliseconds. How oftn to display values

#define INIT_SGT_VALUE 8

#define stepZ 32
#define csZ 31
#define enZ 30

uint32_t stepWait = 1;
uint32_t previousStep;

char buff[100];
bool dirZ = 1;


#include <TMC2130Stepper.h>
#include <TMC2130Stepper_REGDEFS.h>
#include <MeanFilterLib.h>
TMC2130Stepper driverZ = TMC2130Stepper(csZ);

bool vsense;
volatile uint16_t speed_rps = INIT_SPEED_RPS; // initial speed
int8_t sg_value = INIT_SGT_VALUE; // initial stallGuard Value

float current_speed = INIT_SPEED_ACTUAL;
float target_speed = INIT_SPEED_ACTUAL;
bool steps_enabled = false;

MeanFilter<long> meanFilter(10);


uint16_t rms_current(uint8_t CS, float Rsense = 0.11) {
return (float)(CS+1)/32.0 * (vsense?0.180:0.325)/(Rsense+0.02) / 1.41421 * 1000;
}

// used to get step interval from RPM
uint32_t step_interval_us(float rps) {
uint32_t steps_per_second = ((MOTOR_STEPS * MICROSTEPS * rps));
return (1000000 / steps_per_second);
}


// setup timer
IntervalTimer stepTimer;

//timer interrupt function
void Step(){
unsigned long currentMicros =micros();
digitalWrite(stepZ, HIGH);

if (currentMicros-previousStep >=20){
previousStep = micros();

digitalWrite(stepZ, LOW);
}





}


void setup() {
// menu
Serial.begin(250000); //init serial port and set baudrate
while(!Serial);
Serial.println("\r\nTMC2130 StallGuard2 test program\r\n");
delay(1);
Serial.println("'+' = faster");
delay(1);
Serial.println("'-' = slower");
delay(1);
Serial.println("'1' = run");
delay(1);
Serial.println("'0' = pause");
delay(1);
Serial.println("'r' = reverse");
delay(1);
Serial.println("'i' = increase StallGuard Value");
delay(1);
Serial.println("'d' = decrease StallGuard Value");
delay(1);
Serial.println("Send '1' character to begin (\r\n");
delay(1);

while(Serial.available() == 0) // Wait for the serial input.
{
}

pinMode(enZ, OUTPUT);
pinMode(stepZ, OUTPUT);
pinMode(csZ, OUTPUT);

SPI.begin();
pinMode(MISO, INPUT_PULLUP);

digitalWrite(enZ, HIGH); //deactivate driver (LOW active)
// Declare driver direction

//
digitalWrite(stepZ, LOW);
digitalWrite(csZ, HIGH);



// Set timer interrupt that generates pulses


//driverZ.begin();

driverZ.push();
driverZ.toff(3);
driverZ.tbl(1);
//driverZ.stealthChop(0); // ???????
driverZ.hysteresis_start(4);
driverZ.hysteresis_end(-2);
driverZ.setCurrent(DRIVER_CURRENT_mA, 0.11, 0.5);

driverZ.microsteps(MICROSTEPS);
driverZ.diag1_stall(1);
driverZ.diag1_active_high(1);
driverZ.coolstep_min_speed(0xFFFFF); // 20bit max
driverZ.THIGH(0);
driverZ.semin(5);
driverZ.semax(2);
driverZ.sedn(0b01);
driverZ.sg_stall_value(sg_value);

//TMC2130 outputs on (LOW active

stepTimer.begin(Step, step_interval_us(current_speed));

digitalWrite(enZ, LOW);
vsense = driverZ.vsense();
}

void loop()
{
static uint32_t last_time=0;
uint32_t ms = millis();
stepTimer.update(step_interval_us(current_speed));



noInterrupts();
while(Serial.available() > 0) {

int8_t read_byte = Serial.read();

if (read_byte == '0') {
digitalWrite( enZ, HIGH );
steps_enabled = true;
current_speed = INIT_SPEED_ACTUAL;
}

else if (read_byte == '1') {
digitalWrite( enZ, LOW );
steps_enabled = true;
}

else if (read_byte == '+') {
if (target_speed < MAX_SPEED_RPS) {
target_speed += SPEED_INCR_VAL;

if (fabs(current_speed - target_speed) >= ACCEL_RATE) {
if (current_speed < target_speed) {
current_speed += ACCEL_RATE;
}
else {
current_speed -= ACCEL_RATE;
}
}
Serial.print("faster\r\n");
}
}

else if (read_byte == '-') {
if (target_speed > MIN_SPEED_RPS) {
target_speed -= SPEED_INCR_VAL;

if (fabs(current_speed - target_speed) >= ACCEL_RATE) {
if (current_speed < target_speed) {
current_speed += ACCEL_RATE;
}
else {
current_speed -= ACCEL_RATE;
}
}

Serial.print("slower\r\n");
}
}

else if (read_byte == 'r') { // reverse
dirZ = !dirZ;
driverZ.shaft_dir(dirZ);
Serial.print(dirZ);
}

else if (read_byte == 'i') { // increase stall value
if (sg_value < 64) {
sg_value++;
driverZ.sg_stall_value(sg_value);
}
}

else if (read_byte == 'd') {
if (sg_value > -64) {
sg_value--;
driverZ.sg_stall_value(sg_value);
}
}
}
interrupts();


if((ms-last_time) > 100) //run every 0.01s
{
last_time = ms;

uint32_t TSTEP = driverZ.TSTEP();

if (TSTEP != 0xFFFFF) { // if driver is stepping

uint32_t drv_status = driverZ.DRV_STATUS();

int mean = meanFilter.AddValue((drv_status & STALLGUARD_bm)>>STALLGUARD_bp);
Serial.println(drv_status);
Serial.println(drv_status & SG_RESULT_bm);



sprintf(buff, "RPS:%2.1f SGT:%02d SGV:%04d", current_speed, sg_value, (drv_status & SG_RESULT_bm)>>SG_RESULT_bp);
Serial.print(buff);

sprintf(buff, " TSTEP: %05d", TSTEP);
Serial.print(buff);

sprintf(buff, " SG: %d I:04%d \r\n",mean, DEC, (drv_status & CS_ACTUAL_bm)>>CS_ACTUAL_bp);
Serial.print(buff);
}
}
}




When I run the motor and cause it to stall by hand, the drv_status changes from 720896 (with no load) to 1507328 (while stalling) but I don't get StallGuard detection flag change at all. I'm kinda lost and would be really happy if someone can help me figure out what is going on.

Thank you
 
you should always wrap any code you post in code tags so it looks like
Code:
int myExampleFunctions(void *ptr)
{
   return 7;
}

stall detection on stepper motors isn't really as reliable as it sounds. I wont pretend to be familiar with this particular chip, but on ones I use it's basically just a current threshold above which the chip assumes the motor has stalled. This does mean you can stall the motor without the chip knowing about it.
You've set sg_value to what looks like a default, play around with this register to see if you can get better results.
Play around with the motor drive speed/torque/step - all these parameters can affect how much current a motor draws and hence how reliable the stall.
You may have to use the drv_status register instead to infer if the motor has stalled
 
you should always wrap any code you post in code tags so it looks like
Code:
int myExampleFunctions(void *ptr)
{
   return 7;
}

stall detection on stepper motors isn't really as reliable as it sounds. I wont pretend to be familiar with this particular chip, but on ones I use it's basically just a current threshold above which the chip assumes the motor has stalled. This does mean you can stall the motor without the chip knowing about it.
You've set sg_value to what looks like a default, play around with this register to see if you can get better results.
Play around with the motor drive speed/torque/step - all these parameters can affect how much current a motor draws and hence how reliable the stall.
You may have to use the drv_status register instead to infer if the motor has stalled

Hello,
Sorry I should’ve given an update. The problem was with another IC not releasing the MISO line on my PCB. I had to cut the trace and included a tri-state buffer for future revisions. It’s working fine for now and if anyone wants a Teensy specific code on how to run the StallGuard, there you have a starting point.
 
Status
Not open for further replies.
Back
Top