Browse Source

Change direction on step loss

master
JonathanS 3 years ago
parent
commit
983b75b825
  1. 151
      src/StallGuard.ino
  2. 83
      src/getting_started.ino

151
src/StallGuard.ino

@ -1,151 +0,0 @@
/**
* Author Teemu Mäntykallio
*
* Plot TMC2130 motor load using the stallGuard value.
* You can finetune the reading by changing the STALL_VALUE.
* This will let you control at which load the value will read 0
* and the stall flag will be triggered. This will also set pin DIAG1 high.
* A higher STALL_VALUE will make the reading less sensitive and
* a lower STALL_VALUE will make it more sensitive.
*
* You can control the rotation speed with
* 0 Stop
* 1 Resume
* + Speed up
* - Slow down
*/
#define MAX_SPEED 40 // In timer value
#define MIN_SPEED 1000
#define STALL_VALUE 0 // [-64..63]
// Note: You also have to connect GND, 5V and VM.
// A connection diagram can be found in the schematics.
#define EN_PIN 18 // Nano v3: 16 Mega: 38 //enable (CFG6)
#define DIR_PIN 19 // 19 55 //direction
#define STEP_PIN 20 // 18 54 //step
#define CS_PIN 21 // 17 64 //chip select
#define MOSI_PIN 16
#define MISO_PIN 14
#define SCK_PIN 15
#include <TMC2130Stepper.h>
#include <TMC2130Stepper_REGDEFS.h>
TMC2130Stepper driver(EN_PIN, DIR_PIN, STEP_PIN, CS_PIN, MOSI_PIN, MISO_PIN, SCK_PIN);
bool vsense;
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;
}
void setup() {
//init serial port
{
Serial.begin(250000); //init serial port and set baudrate
while(!Serial); //wait for serial port to connect (needed for Leonardo only)
Serial.println("\nStart...");
pinMode(EN_PIN, OUTPUT);
pinMode(DIR_PIN, OUTPUT);
pinMode(STEP_PIN, OUTPUT);
pinMode(CS_PIN, OUTPUT);
digitalWrite(EN_PIN, HIGH); //deactivate driver (LOW active)
digitalWrite(DIR_PIN, LOW); //LOW or HIGH
digitalWrite(STEP_PIN, LOW);
digitalWrite(CS_PIN, HIGH);
SPI.begin();
pinMode(MISO, INPUT_PULLUP);
}
//set TMC2130 config
{
//driver.push();
//driver.toff(3);
//driver.tbl(1);
//driver.hysteresis_start(4);
//driver.hysteresis_end(-2);
driver.rms_current(600); // mA
//driver.microsteps(16);
//driver.diag1_stall(1);
//driver.diag1_active_high(1);
driver.coolstep_min_speed(0xFFFFF); // 20bit max
//driver.THIGH(0);
//driver.semin(5);
//driver.semax(2);
//driver.sedn(0b01);
//driver.sg_stall_value(STALL_VALUE);
}
// Set stepper interrupt
{
cli();//stop interrupts
TCCR1A = 0;// set entire TCCR1A register to 0
TCCR1B = 0;// same for TCCR1B
TCNT1 = 0;//initialize counter value to 0
OCR1A = 256;// = (16*10^6) / (1*1024) - 1 (must be <65536)
// turn on CTC mode
TCCR1B |= (1 << WGM12);
// Set CS11 bits for 8 prescaler
TCCR1B |= (1 << CS11);// | (1 << CS10);
// enable timer compare interrupt
TIMSK1 |= (1 << OCIE1A);
sei();//allow interrupts
}
//TMC2130 outputs on (LOW active)
digitalWrite(EN_PIN, LOW);
vsense = driver.vsense();
}
ISR(TIMER1_COMPA_vect){
PORTF |= 1 << 0;
PORTF &= ~(1 << 0);
PORTF |= 1 << 5;
PORTF &= ~(1 << 5);
}
void loop()
{
/*digitalWrite(STEP_PIN, HIGH);
delayMicroseconds(100);
digitalWrite(STEP_PIN, LOW);
delayMicroseconds(100);
*/
static uint32_t last_time=0;
uint32_t ms = millis();
while(Serial.available() > 0) {
int8_t read_byte = Serial.read();
if (read_byte == '0') { TIMSK1 &= ~(1 << OCIE1A); digitalWrite( EN_PIN, HIGH ); }
else if (read_byte == '1') { TIMSK1 |= (1 << OCIE1A); digitalWrite( EN_PIN, LOW ); }
else if (read_byte == '+') {
if (OCR1A > MAX_SPEED) OCR1A -= 20;
Serial.print("Set speed to " );
Serial.println((uint16_t)OCR1A);
}
else if (read_byte == '-') {
if (OCR1A < MIN_SPEED) OCR1A += 20;
Serial.print("Set speed to " );
Serial.println((uint16_t)OCR1A);
}
}
if((ms-last_time) > 100) //run every 0.1s
{
last_time = ms;
uint32_t drv_status = driver.DRV_STATUS();
Serial.print("0 ");
Serial.print((drv_status & SG_RESULT_bm)>>SG_RESULT_bp , DEC);
Serial.print(" ");
Serial.println(rms_current((drv_status & CS_ACTUAL_bm)>>CS_ACTUAL_bp), DEC);
// Serial.print("DRV_STATUS=0b");
// Serial.println(driver.DRV_STATUS(), BIN);
}
}

83
src/getting_started.ino

@ -91,7 +91,6 @@ void setup(){
myStepper.set_mres(128);
// myStepper.set_tbl(2);
// 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
@ -100,9 +99,18 @@ void setup(){
myStepper.set_hend(13);
myStepper.set_IHOLD_IRUN(1,15,15);
// 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);
@ -110,13 +118,8 @@ void setup(){
myStepper.set_TPWMTHRS(1);
myStepper.set_vhighchm(1);
myStepper.set_vhighfs(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
// enable dcStep mode
myStepper.set_VDCMIN(1);
myStepper.set_DCCTRL(70, 10);
@ -127,31 +130,47 @@ void setup(){
void loop(){
// make a step
digitalWrite(STEP_PIN, HIGH);
delayMicroseconds(23);
delayMicroseconds(42);
digitalWrite(STEP_PIN, LOW);
delayMicroseconds(23);
uint32_t drv_status = 0;
myStepper.read_REG(TMC_REG_DRV_STATUS, &drv_status);
uint32_t stallguard = drv_status & 0x1ff;
//Serial.println(stallguard);
uint32_t current = (drv_status >> 16) & 0x1f;
uint32_t lost_steps;
myStepper.read_REG(TMC_REG_LOST_STEPS, &lost_steps);
static int i = 0;
if ((i++ % 1000) == 0) {
Serial.print("Lost Steps: ");
Serial.println(lost_steps);
delayMicroseconds(42);
static int sometimes = 0;
if (sometimes++ > 100) {
sometimes = 0;
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;
int32_t lost_steps;
myStepper.read_REG(TMC_REG_LOST_STEPS, &lost_steps);
static int32_t last_lost_steps = 0;
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;
}
}
}
//Serial.println(myStepper.debug());
//Serial.println((myStepper.isReset() ? "RESET " : "----- "));
//Serial.println((myStepper.isError() ? "ERROR " : "----- "));
//Serial.println((myStepper.isStallguard() ? "SLGRD " : "----- "));
//Serial.println((myStepper.isStandstill() ? "STILL " : "----- "));
}

Loading…
Cancel
Save