teemuatlut / TMCStepper

MIT License
501 stars 196 forks source link

STM32F103 and TMC5160-BOB compatibility? #180

Open BlueOwlrus opened 3 years ago

BlueOwlrus commented 3 years ago

TL;DR: This code works on Arduino Nano, but doesnt work on STM32F103C8T6. Motor just does nothing, no locking, no rotation. Software SPI works fine, STEP/DIR worsk fine, wiring is fine, motor is fine, boards and driver are fine. When running this code on STM32, the driver sends an error "Open Load A and B" It feels like the driver just doesnt apply enough current to rotate the stepper, since voltage between motor coils and GND is measured 24v (with multimeter, not oscilloscope, so it may be wrong)

I'm making a project which requires a very simple task: rotate stepper at certain speed and monitor torque (using stall guard feature). At first I was using Arduino Nano and it was working fine. But after I switched to STM32F103C8T6 (so called "Blue Pill") it just stopped working. I've tried different motors (huge nema23 and small nema17). I've checked all the wiring, tried to use different boards and drivers, etc. I've checked step and dir pins using Saleae logic analyzer: step/dir generation works fine. Softwafe also works fine, I've tested it (also it does connect with the driver, so obviously it's working).

I'm 100% sure the problem is in the code.

I used absolutly same code for arduino and stm32, except for pins numbers: (this is just a test code I upload to STM32 to test movement, not the real one from the big project)

#include <TMCStepper.h>
#include <AccelStepper.h>

#define EN_PIN    PA15 //enable
#define DIR_PIN   PA11 //direction
#define STEP_PIN  PA12 //step

#define R_SENSE   0.075f //TMC5160: 0.075 Ohm

#define STALL_VALUE      14 // [-64..63]

#define ENDSTOP_PIN PA8 

#define CS_PIN PB8 //All pins here re tolerant to 5v
#define MOSI_PIN PB6
#define MISO_PIN PB4
#define SCK_PIN PB7

int32_t STEPS_PER_REV = 3200;

//TMC5160Stepper tmc = TMC5160Stepper(CS_PIN, R_SENSE); //use hardware SPI
TMC5160Stepper tmc = TMC5160Stepper(CS_PIN, R_SENSE, MOSI_PIN, MISO_PIN, SCK_PIN); //use software SPI

AccelStepper Stepper;

void setup()
{
    Serial.begin(9600);
  //set pins
  pinMode(EN_PIN, OUTPUT);
  digitalWrite(EN_PIN, HIGH); //deactivate driver (LOW active)
  pinMode(DIR_PIN, OUTPUT);
  digitalWrite(DIR_PIN, LOW); //direction: LOW or HIGH
  pinMode(STEP_PIN, OUTPUT);
  digitalWrite(STEP_PIN, LOW);

  pinMode(CS_PIN, OUTPUT);
  digitalWrite(CS_PIN, HIGH);
  pinMode(MISO, INPUT_PULLUP);

    delay(1000);
    int32_t stallValue = 14;
    int32_t current = 500;

  //set driver config
  tmc.begin();
  tmc.toff(4); //off time
  tmc.blank_time(24); //blank time
  //tmc.en_pwm_mode(1); //enable extremely quiet stepping
  tmc.microsteps(64); //16 microsteps
  tmc.rms_current(current); //RMS
  tmc.sfilt(true); // Improves SG readout.
  //tmc.rdsel(0b01);
  tmc.semin(5);
  tmc.semax(2);
  tmc.sedn(0b01);
  tmc.sgt(stallValue);

  //outputs on (LOW active)
  digitalWrite(EN_PIN, LOW);

  Stepper = AccelStepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
  Stepper.setAcceleration(5000);
  Stepper.setMaxSpeed(2000);
  Stepper.setEnablePin(EN_PIN);
  Stepper.disableOutputs();

  Stepper.move(100000L);
}

void loop() {
    Stepper.run();
  static uint32_t last_time=0;
  uint32_t ms = millis();

  if((ms-last_time) > 50) //run every 50ms
  {
    Stepper.run();
    last_time = ms;

    if(tmc.diag0_error()){ errorToNextion(F("DIAG0 error"));    }
    Stepper.run();
    if(tmc.ot())         { errorToNextion(F("Overtemp."));      }
    Stepper.run();
    if(tmc.otpw())       { errorToNextion(F("Overtemp. PW"));   }
    Stepper.run();
    if(tmc.s2ga())       { errorToNextion(F("Short to Gnd A")); }
    Stepper.run();
    if(tmc.s2gb())       { errorToNextion(F("Short to Gnd B")); }
    Stepper.run();
    if(tmc.ola())        { errorToNextion(F("Open Load A"));    }
    Stepper.run();
    if(tmc.olb())        { errorToNextion(F("Open Load B"));    }
    Stepper.run();

    //This part does work. Somehow.
    TMC2130_n::DRV_STATUS_t data{0}; //using this because tmc5160 class doesnt have this. Works fine.
    Stepper.run();
    data.sr = tmc.DRV_STATUS();
    Stepper.run();
    int val = map(data.sg_result, 0, 1024, 0, 100);
    Serial.println(val);
  }
  if (Stepper.distanceToGo() < 1000) {
        Stepper.move(100000L);
    }
    Stepper.run();
}

void errorToNextion(String str) {
    Serial.println(str);
    //delay(1000);
}

I have tried all the variants with enableOutputs(), invertPins(), etc. So this code should work. At least it does work on arduino. Also I recieve "Open Load A/B" message every loop. I belive this is not normal and I dont get such error on arduino.

Besides testing STEP/DIR pins via Logic Analyzer, I've checked voltage between GND and motor windings. It always shows 24v (same as my power supply). But i'm not sure if this is constant voltage, since i'm using only multimeter, not oscilloscope. It feels like the driver just doesnt apply enough current to rotate the stepper. Motor doesnt lock, doesnt show any signs of movement.

Again, this code works when uploaded to arduino with pins corrected. Motor rotates and I dont get "Open Load" messages.

Please help, I've spent 2 weeks trying to figure whats going on and I'm still blind here. Sorry for my English, not my first language.

BlueOwlrus commented 3 years ago

image Softwafare SPI at ~760kHz 1 - SCK 2 - MOSI 3 - MISO As I remeber datasheet for tmc5160, 4 bytes out of 5 from MOSI output should be later seen in MISO. Graph shows SPI works fine.

EDIT image I added tmc.rms_current() reading into every cycle (every 50ms). It shows perfect 292 mA (I setup it as 300mA). So driver has correct settings about rms current. Also I tried to slow down software SPI to 58kHz, like it was advised for Teensy. But actually TMC5160 should be able to handle even 4MHz, according to datasheet.

Here's code for SG and RMS current output:

Serial.print(tmc.sg_result(), DEC);
Serial.print(" - ");
Serial.println(tmc.rms_current(), DEC);

where tmc is the driver object. TMC5160Stepper tmc = TMC5160Stepper(CS_PIN, R_SENSE, MOSI_PIN, MISO_PIN, SCK_PIN);

teemuatlut commented 3 years ago

Hey, I'll try to test it out myself on the weekend since I have few of those blue and black pills. I also have development going on for pure STM HAL (without Arduino) that I've tested with the black pill.

But for now I'll say that the 2160/5160 don't read anything from the driver since all the register (IHOLD_IRUN and GLOBAL_SCALER) are write only. The rms_current method reads from internally cached register values. I would recommend using the .version() method for testing connectivity.

The STM chips are quite a bit faster and it's possible there are some timing constraints that need to be taken into account. Perhaps with time before/after asserting CS pin or something.

teemuatlut commented 3 years ago

image

#include <TMCStepper.h>
#include <AccelStepper.h>

#define EN_PIN    PA15 //enable
#define DIR_PIN   PA11 //direction
#define STEP_PIN  PA12 //step

#define R_SENSE   0.075f //TMC5160: 0.075 Ohm

#define STALL_VALUE      7 // [-64..63]

#define ENDSTOP_PIN PA8 

#define CS_PIN PB8 //All pins here re tolerant to 5v
#define MOSI_PIN PB6
#define MISO_PIN PB4
#define SCK_PIN PB7

constexpr uint32_t steps_per_mm = 80;
int32_t STEPS_PER_REV = 3200;

TMC5160Stepper tmc(CS_PIN, R_SENSE, MOSI_PIN, MISO_PIN, SCK_PIN); //use software SPI
AccelStepper Stepper(AccelStepper::DRIVER, STEP_PIN, DIR_PIN);
HardwareTimer StepPulseTimer(TIM1);

void RunStepper() {
  Stepper.run();
}

void setup()
{
  Serial.begin(9600);

  pinMode(MISO, INPUT_PULLUP);

  delay(1000);
  uint16_t current = 500;

  //set driver config
  tmc.begin();

  TMC5130_n::IOIN_t ioin{tmc.IOIN()}; // Shared with 5160

  if (ioin.version == 0xFF || ioin.version == 0) {
    Serial.println("Driver communication error");
    while(true);
  }

  Serial.print("Driver firmware version: ");
  Serial.println(ioin.version);

  if (!ioin.sd_mode) {
    Serial.println("Driver is hardware configured for SPI mode");
    while(true);
  }
  if (ioin.drv_enn_cfg6) {
    Serial.println("Driver is not hardware enabled");
    while(true);
  }

  StepPulseTimer.setOverflow(1000, HERTZ_FORMAT);
  StepPulseTimer.attachInterrupt(RunStepper);
  StepPulseTimer.resume();

  tmc.toff(4); //off time
  tmc.blank_time(24); //blank time
  tmc.en_pwm_mode(false); //enable extremely quiet stepping
  tmc.microsteps(16); //16 microsteps
  tmc.rms_current(current); //RMS
  tmc.sfilt(true); // Improves SG readout.
  //tmc.rdsel(0b01);
  tmc.semin(5);
  tmc.semax(2);
  tmc.sedn(0b01);
  tmc.sgt(STALL_VALUE);

  //outputs on (LOW active)
  digitalWrite(EN_PIN, LOW);

  Stepper.setMaxSpeed(50*steps_per_mm); // 100mm/s @ 80 steps/mm
  Stepper.setAcceleration(1000*steps_per_mm); // 2000mm/s^2
  Stepper.setEnablePin(EN_PIN);
  Stepper.setPinsInverted(false, false, true);
  Stepper.enableOutputs();

  Stepper.move(100000L);
}

int printCount = 0;

void loop() {
  static uint32_t last_time=0;
  uint32_t ms = millis();

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

    if(tmc.diag0_error()){ errorToNextion(F("DIAG0 error"));    }
    if(tmc.ot())         { errorToNextion(F("Overtemp."));      }
    if(tmc.otpw())       { errorToNextion(F("Overtemp. PW"));   }
    if(tmc.s2ga())       { errorToNextion(F("Short to Gnd A")); }
    if(tmc.s2gb())       { errorToNextion(F("Short to Gnd B")); }
    if(tmc.ola())        { errorToNextion(F("Open Load A"));    }
    if(tmc.olb())        { errorToNextion(F("Open Load B"));    }

    // TMC5160 uses the same DRV_STATUS layout as 2130
    TMC2130_n::DRV_STATUS_t data{tmc.DRV_STATUS()};
    int val = map(data.sg_result, 0, 1024, 0, 100);
    Serial.print(val);
    Serial.print(" ");
    if (++printCount >= 30) {
      printCount = 0;
      Serial.println();
    }
  }
  if (Stepper.distanceToGo() < 1000) {
    Stepper.move(100000L);
  }
}

void errorToNextion(String str) {
  Serial.println(str);
  //delay(1000);
}
BlueOwlrus commented 3 years ago

Huge thanks for your help! This is my setup: image (I marked soldering with yellow, solder CLK16 short to GND and solder R1 with 0 Ohm resistor, unsolder R2) After launched your code, I'm getting this:

Driver firmware version: 48
Driver is not hardware enabled

So, it means my driver does communicate with STM32 properly... but hardware not enabled?

From what I managed to find, I don't quite undrestand what drv_enn_cfg6 means. Since in TMC5130_bitfields.h it's bit number 4:

 namespace TMC5130_n {
   struct IOIN_t {
     constexpr static uint8_t address = 0x04;
     union {
       uint32_t sr;
       struct {
         bool  refl_step : 1,
               refr_dir : 1,
               encb_dcen_cfg4 : 1,
               enca_dcin_cfg5 : 1,
               drv_enn_cfg6 : 1,
               enc_n_dco : 1,
               sd_mode : 1,
               swcomp_in : 1;
         uint16_t : 16;
         uint8_t version : 8;
       };
     };
   };
 }

And according to tmc5160 datasheet bit number 4 in IOIN register is DRV_ENN: image While your name "drv_enn_cfg6" also contains "cfg6", which should be bit #5 and be "ENC_N_DCO_CFG6".

Anyways, if its all about CFG6 pin, I dont have to pull it down or up, since i'm using SD_MODE=1 image

If its all about DRV_ENN.... well isn't it just an ENABLE pin for motor? image

So I still dont understand whats wrong here. And the biggest question, why my code worked on arduino, and not on stm32 D: How the hell it can be platfrom related.

P.S. I managed to make my code (which I included in first post) to lock the motor. But motor is still not moving. Also it feels like motor locks stronger if I apply more current in settings. So driver does communicate, it does receives settings, it does receives step/dir... but it doesnt move...

EDIT After commenting out this:

  if (ioin.drv_enn_cfg6) {
    Serial.println("Driver is not hardware enabled");
    //while(true);
  }

The motor locks strongly (or better say "holds", i'm not good at english) and the serial looks like this:

Driver firmware version: 48
Driver is not hardware enabled
Open Load A
Open Load B
8 Open Load A
Open Load B
9 Open Load A
Open Load B
9 Open Load A
Open Load B
8 Open Load A
Open Load B
9 Open Load A
Open Load B
8 Open Load A
Open Load B
9 Open Load A

EDIT2 I just measured voltage between GND and REFR (DIR pin on TMC5160-BOB) and its 3.3v. It obviously normal, since STM32 outputs 3.3v, not 5v... Maybe TMC5160 doesnt see this as HIGH logic level? I didnt find this info in datasheet. But why does SPI on TMC5160 works with 3.3 and STEP/DIR not? image