BlueTurtle
Active member
Hey everyone,
I'm trying to make the T4 work with TMC2130 Stepper Drivers so that I can use them without endstops. The following code works fine when I connect to my T4 with RPi but when I connect T4 to my Mac the T4 gives out wrong output from serial (Almost everything turns out to be 0 and stepper motor doesn't stop when it hits an obstacle. Everything works when my T4 is connected to Rpi). I'm wondering how I can fix this issue to continue my development from my laptop rather than RPi because it is too slow.
Board: My Custom board with T4 for controller
Rpi: Raspberry SC15184 Pi 4 Model B 2019 Quad Core 64 Bit WiFi Bluetooth (2GB), Arduino 1.8.10 with Teensyduino Addon
Macbook Pro, Catalina 10.15.3, Latest Teensyduino
I'm trying to make the T4 work with TMC2130 Stepper Drivers so that I can use them without endstops. The following code works fine when I connect to my T4 with RPi but when I connect T4 to my Mac the T4 gives out wrong output from serial (Almost everything turns out to be 0 and stepper motor doesn't stop when it hits an obstacle. Everything works when my T4 is connected to Rpi). I'm wondering how I can fix this issue to continue my development from my laptop rather than RPi because it is too slow.
Board: My Custom board with T4 for controller
Rpi: Raspberry SC15184 Pi 4 Model B 2019 Quad Core 64 Bit WiFi Bluetooth (2GB), Arduino 1.8.10 with Teensyduino Addon
Macbook Pro, Catalina 10.15.3, Latest Teensyduino
Code:
#include <TMC2130Stepper.h>
#include <TMC2130Stepper_REGDEFS.h>
#include <MeanFilterLib.h>
// Motor Parameters // Parameters known to work for Z axis
#define MOTOR_STEPS 200.0 // Steps per rev typically 200 (1.8°) to 400 (0.9°)
#define MICROSTEPS 16 // [16]
#define DRIVER_CURRENT_mA 900 // [900] Current in milliAmps
#define MAX_SPEED_RPS 60 // [60]
#define MIN_SPEED_RPS 1 // [1]
#define INIT_SPEED_RPS 3 // [3] Final speed target for homing
#define INIT_SPEED_ACTUAL 2 // [2] Initial speed of the stepper
#define SPEED_INCR_VAL 0.2 // [0.2] Speed change per +,- inputs
#define ACCEL_RATE 0.1 // [0.1] Speed change per DISPLAY_INTERVAL until target speed is reached
#define INIT_SGT_VALUE 1 // [1] Initial StallGuardTreshold Value
#define DISPLAY_INTERVAL 100 // In milliSeconds. How often to display values
#define stepZ 32
#define csZ 31
#define enZ 30
uint32_t previousStep; // Holds the last time a step was made in milliSeconds.
char buff[100];
bool dirZ = 1;
// Stepper Driver Initialization
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_RPS;
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 to be used in stepping with interrupts.
IntervalTimer stepTimer;
//timer interrupt function
void Step() {
unsigned long currentMicros = micros();
digitalWrite(stepZ, HIGH);
if (currentMicros - previousStep >= 1) {
previousStep = micros();
digitalWrite(stepZ, LOW);
}
}
void setup() {
pinMode(enZ, OUTPUT);
pinMode(stepZ, OUTPUT);
pinMode(csZ, OUTPUT);
digitalWrite(enZ, HIGH); //deactivate driver (LOW active)
digitalWrite(stepZ, LOW);
digitalWrite(csZ, HIGH);
// menu
Serial.begin(115200); //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.
{
}
//SPI.begin();
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.coolstep_min_speed(0xFFFFF);
driverZ.diag1_stall(1);
driverZ.diag1_active_high(1);
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));
while (Serial.available() > 0) {
int8_t read_byte = Serial.read();
if (read_byte == '0') {
digitalWrite( enZ, HIGH );
current_speed = INIT_SPEED_ACTUAL;
}
else if (read_byte == '1') {
digitalWrite( enZ, LOW );
}
else if (read_byte == '+') {
if (target_speed < MAX_SPEED_RPS) {
target_speed += SPEED_INCR_VAL;
Serial.print("Faster\r\n");
}
}
else if (read_byte == '-') {
if (target_speed > MIN_SPEED_RPS) {
target_speed -= SPEED_INCR_VAL;
Serial.print("Slower\r\n");
}
}
else if (read_byte == 'r') { // reverse
dirZ = !dirZ;
driverZ.shaft_dir(dirZ);
current_speed = INIT_SPEED_ACTUAL;
if (dirZ == 1) {
Serial.println("Up");
} else {
Serial.println("Down");
}
}
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);
}
}
}
if ((ms - last_time) > DISPLAY_INTERVAL) //run every 0.01s
{
last_time = ms;
uint32_t TSTEP = driverZ.TSTEP();
if (TSTEP != 0xFFFFF) { // if driver is stepping
noInterrupts(); // Disable intterupts to get accurate reading from TMC2130
uint32_t drv_status = driverZ.DRV_STATUS();
int mean = meanFilter.AddValue((drv_status & STALLGUARD_bm) >> STALLGUARD_bp);
if (((drv_status & STALLGUARD_bm) >> STALLGUARD_bp) == 1) {
Serial.println("STALL DETECTED");
digitalWrite(enZ, HIGH);
}
sprintf(buff, "RPS:%2.1f SGT:%02d SGV:%04lu", current_speed, sg_value, (drv_status & SG_RESULT_bm) >> SG_RESULT_bp);
Serial.print(buff);
sprintf(buff, "TSTEP: %05lu", TSTEP);
Serial.print(buff);
sprintf(buff, "SG: %d I:04%lu \r\n", mean, (drv_status & CS_ACTUAL_bm) >> CS_ACTUAL_bp);
Serial.print(buff);
}
if (fabs(current_speed - target_speed) >= ACCEL_RATE) {
if (current_speed < target_speed) {
current_speed += ACCEL_RATE;
}
else {
current_speed -= ACCEL_RATE;
}
}
interrupts(); // Enable intterupts to generate accurate Steps.
}
}