loud noise when running steppers simultaneously (teensystep)

Status
Not open for further replies.
Hi all,

Hoping someone here can shed some light on a somewhat esoteric problem. I'm running six stepper motors simultaneously, and I need to be able to move them to different positions in synchrony – as if they were physically geared together. Without getting into too much detail, I'm spinning big metal rings of different sizes that need to line up every so often.

The problem I'm having is that the steppers run flawlessly individually with the TeensyStep library individually, but when I try to run them simultaneously, they make horrible noises – a loud, low frequency rattle/grinding noise and a regular ticking/hammering noise.

I ran into the same problem with a Mega running AccelStepper, which is part of the reason I switched to Teensy in the first place.

I'm using these gecko drives,, which have been great so far. These are the motors I'm using. Also Teensy 3.6 with 74hct245 logic converters. I know the hardware configuration is good because I can spin each of the six motors individually at basically any speed, and the motion is smooth, accurate, and quiet.

I'm giving them 48V (because steppers can be driven at higher speeds with more voltage) and 4.2A (rated current) via three 10A meanwell supplies. I've tried microstepping up to 128usteps, and it helps, but not a whole lot.

The code is trivial; the relevant snippet is:

Code:
long whole_revs[] = { 29128, 60082, 14060, 44378, 1091, 29880 }; // number of steps to spin each ring fully around
const int revolutions[] = {6, 3, 12, 4, 2, 6}; // how many times to spin each ring
long targets[6];

for (int i = 0; i < 6; i++) {
    targets[i] = whole_revs[i]*revolutions[i]; // destination = steps per revolution x number of revolutions
    steppers[i]->setTargetAbs(targets[i]);
    if (!controller.isRunning()) controller.moveAsync(steppers);
  }

My theories are:
  1. because the targets are slightly different, there's some kind of fractional increment that gets built up over time, effectively creating a sawtooth movement in the pulse output. not sure why this should be the case, since in theory it should calculate a constant speed or interval for each motor (some number of microseconds) and step the motor that often. ...right?
  2. something about the power scheme is causing problems when multiple steppers are active simultaneously
  3. there's some esoteric setting in the drivers that i haven't found yet
  4. i've angered the gods of small electronics and this is my punishment

Any thoughts?
 
Can you please post a full sketch including the setup of the controller and the steppers?
 
Wow, Luni in the flesh! I'm honored.

And sure – there's a lot to the sketch around moving the motors, so I was trying to keep it brief, but here's the whole thing, maybe you'll be able to spot something I haven't.

Most of it is Serial, for control and debugging –*the important part (I think) is what I quoted above: when you send "x" over the serial, it calls startnewcycle(); that's when the noise happens. It also happens to a lesser degree if I run two at a time via the customcycle() function, sending for example "X 1.0 0.0 0.0 0.0 0.0 1.0" to spin motors 0 and 5 one full revolution.

Code:
#include <TeensyStep.h>  // library for teensy-based motor control: https://luni64.github.io/TeensyStep/documentation/
#include <TimeLib.h>
#include <SD.h>
#include <SPI.h>

const int chipSelect = BUILTIN_SDCARD;

StepControl controller; // controller object (manages individual steppers)

File logfile;

const int indicator = LED_BUILTIN, // light
  estop = 17, // input from the e-stop. 
  log_time = 250, // in ms, how often to report postitions.
  sd_write_time = 1000, // defines SD read/write operations, so don't make this too small
  motor_steps_per_rev = 200; // how many steps per one motor revolution
int max_speed = 40000, // top speed
  acceleration = 20000;  // acceleration = steps/sec/sec;

/* const int avg_sens_widths[] = {122, 124, 118, 100, 51, 110}; // rough number of steps it takes to traverse a sensor */
/* const int avg_sens_widths[] = {115, 112, 98, 100, 175, 128}; // rough number of steps it takes to traverse a sensor @ 400ustep */
/* const int avg_sens_widths[] = {460, 448, 392, 400, 700, 512}; // rough number of steps it takes to traverse a sensor @ 1600ustep */
const int avg_sens_widths[] = {14720, 14336, 12544, 12800, 22400, 16384}; // rough number of steps it takes to traverse a sensor @ 128kustep

int activeletter = 4; // for testing purposes
String command = ""; // logical control

// const long whole_revs[] = { 4572, 9440, 2229, 6983, 400, 5082 }; // measured manually on the mockup
/* long whole_revs[] = { 29128, 60082, 14060, 44378, 1091, 29880 }; // real build, measured manually */
/* long whole_revs[] = { 116512, 240328, 56240, 177512, 6000, 119520 }; // real build, measured * 4 for 1600ustep */
long whole_revs[] = { 3728384, 7690496, 1799680, 5680384, 192000, 3824640 };
const long offsets[] = {0, 0, 0, 0, 0, 0}; // distance from sensor 0 to home position
const int revolutions[] = {6, 3, 12, 4, 2, 6}; // number of times to revolve per cycle
long positions[6];
long prev_positions[6];
long period = 30; // unused
long targets[6]; // to compare with positions[]

int certainties[] = { 0, 0, 0, 0, 0, 0 };
boolean searching = false;
boolean flips[] = {false, true, true, false, false, false};
float thetas[6];
int speeds[6]; // unused?

/* TEENSY */
int lpins[][5] = { // letter pins. two dimensional array of [letters][pins].

                  // 0      1   2     3      4
                  // step, dir, en, sens1, sens2
                  {2, 3, 4, 33, 34}, // 0
                  {5, 6, 7, 35, 36}, // 1
                  {8, 9, 10, 37, 38}, // 2
                  {11, 12, 24, 39, 14}, // 3
                  {25, 26, 27, 15, 0},  // 4
                  {28, 29, 30, 16, 0}  // 5

};


/* MEGA */
/* int lpins[][5] = { // letter pins. two dimensional array of [letters][pins]. */

/*   // 0      1   2     3      4 */
/*   // step, dir, en, sens1, sens2 */
/*   {A0, A1, 38, 27, 7}, // 0 */
/*   {A6, A7, A2, 33, 4}, // 1 */
/*   {46, 48, A8, 2, 22}, // 2 */
/*   {26, 28, 24, 50, 5}, // 3 */
/*   {36, 34, 30, 3, 0},  // 4 */
/*   {47, 32, 45, 35, 0}  // 5 */

/* }; */

boolean debug = true,  // flag to turn on/off serial output
  powertoggle = true, // indicator as to whether we've disabled the motors or not
  need_to_log = true,
  SDokay = true,
  force_log = false,
  log_pos = true, sd_log = true; // flags to note whether we've logged positions during moves (either to serial or eventually to SD)


// initiate stepper array (will be populated in setup)
Stepper *steppers[6] = {NULL, NULL, NULL, NULL, NULL, NULL};

void setup() {

  setSyncProvider(getTeensy3Time);


  if (debug) {
    Serial.begin(115200);
    while(!Serial);  // CAUTION: THIS WILL HANG IF NOT CONNECTED TO COMPUTER
    delay(150); // gives time for serial to establish itself.
    Serial.println("=================== setup =====================");

    if (timeStatus()!= timeSet) {
      Serial.println("Unable to sync with the RTC");
    } else {
      Serial.println("RTC has set the system time");
    }

    Serial.print("current time: ");
    Serial.print(hour());
    printDigits(minute());
    printDigits(second());
    Serial.print(" ");
    Serial.print(day());
    Serial.print(" ");
    Serial.print(month());
    Serial.print(" ");
    Serial.print(year());
    Serial.println();

    Serial.print("max speed: ");
    Serial.print(max_speed);
    Serial.print( " accel: ");
    Serial.print(acceleration);
    Serial.println();
  }
  if (debug) Serial.println("---------");

  if (!SD.begin(chipSelect)) {
    if (debug) Serial.println("SD card initialization failed");
    SDokay = false;
  }
  if (debug) Serial.println("SD card initialization ok");

  SD_read_positions();

  // initiate steppers
  for (int i = 0; i < 6; i++) {
    steppers[i] = new Stepper(lpins[i][0], lpins[i][1]);

    // set speeds + accelerations
    steppers[i]->setMaxSpeed(max_speed);
    steppers[i]->setAcceleration(acceleration);
    steppers[i]->setPosition(positions[i]);
  }

  // pin declarations
  for (int s = 0; s < 6; s++) {
    if (debug) Serial.print("== letter ");
    if (debug) Serial.print(s);
    if (debug) Serial.println(" ==");
    for (int p = 0; p < 5; p++) {
      if (lpins[s][p] != 0) {
        if (p < 3) {
          if (debug) Serial.print("setting pin to output: ");
          if (debug) Serial.println(lpins[s][p]);
          pinMode(lpins[s][p], OUTPUT);
        }
        else {
          if (debug) Serial.print("setting pin to input_pullup: ");
          if (debug) Serial.println(lpins[s][p]);
          pinMode(lpins[s][p], INPUT_PULLUP);
        }
      }
    }

    digitalWrite(lpins[s][2], HIGH); // enable all motors
  }

  pinMode(LED_BUILTIN, OUTPUT);
  pinMode(estop, INPUT);

  if (debug) Serial.println("---------");

  if (debug) Serial.print("estop status: ");
  if (debug) Serial.println((digitalRead(estop) == HIGH) ? "high" : "low");

  while (digitalRead(estop) == LOW) {
    if (debug) Serial.println("estop active. doing nothing.");
    delay(1000);
  } // hang until estop is reset

  if (debug) Serial.println("waiting 5secs at end of setup for you to turn on the motors");
  for(int i = 5; i > 0; i--) {
    if (debug) Serial.println(i);
    digitalWrite(indicator, HIGH);
    delay(250);
    digitalWrite(indicator, LOW);
    delay(750);
  }
  if (debug) Serial.println("ok going...");

} // end setup


void loop() {

  if (digitalRead(estop) == LOW) command = "shutoff";


  for (int i = 0; i < 6; i++) {
    positions[i] = steppers[i]->getPosition();
  }

  log_position();





  /*           SERIAL INPUT             */

  if (debug && Serial.available() > 0) {
    int incoming = Serial.read();
    Serial.print("input: ");
    Serial.print(incoming);
    Serial.print("\t letter: ");
    Serial.write(incoming);
    Serial.println();

    if (incoming > 47 && incoming < 54) {
      // input is one of 0-5
      // subtract 49 because the ascii code for 1 is 49
      activeletter = incoming - 48;
      if (debug) Serial.print("new active letter: ");
      if (debug) Serial.println(activeletter);
    }

    switch(incoming) {
    case 67: // C
      command = "calibrate1";
        break;
    case 99: // c
      command = "calibrate2";
      break;
    case 32: // space
      command = "shutoff";
      break;
    case 104: // h
      command = "softstop";
      break;
    case 114: // r
      steppers[activeletter]->setTargetRel(random(-whole_revs[activeletter], whole_revs[activeletter]));
      controller.move(*steppers[activeletter]);
      break;
    case 109: // m
      absolutemove(Serial.parseInt(), Serial.parseInt());
      break;
    case 77: // M
      relativemove(Serial.parseInt(), Serial.parseInt());
      break;
    case 112: // p
      command = "powertoggle";
      break;
    case 120: // x
      command = "cycle";
      break;
    case 88: // X
      customcycle(Serial.parseFloat(),
                  Serial.parseFloat(),
                  Serial.parseFloat(),
                  Serial.parseFloat(),
                  Serial.parseFloat(),
                  Serial.parseFloat());

    case 115: // s
      setspeed(Serial.parseInt());
      break;
    case 97: // a
      setaccel(Serial.parseInt());
      break;
    case 108: // l
      report(Serial.parseInt());
      break;
    case 113: // q
      SD_read_positions();
      for (int i = 0; i < 6; i++) {
        steppers[i]->setPosition(positions[i]);
      }
      break;
    case 81: // Q
      force_log = true;
      log_position();
      break;
    case 119: // w
      overwriteposition(Serial.parseInt(), Serial.parseInt());
      break;
    case 87: // W
      if (debug) Serial.println("reset all to zero:");
      for(int i = 0; i < 6; i++) {
        overwriteposition(i, 0);
      }
      break;
    }




  }


  /*             COMMANDS           */

  if (command == "calibrate1") {
    calibrate1(activeletter);
  }
  else if (command == "calibrate2") {
    calibrate2(activeletter);
  }
  else if (command == "softstop") {
    if (debug) Serial.println("soft stopping...");
    controller.stop();
    if (debug) Serial.print("final positions: ");
    for (int i = 0; i < 6; i++) {
      if (debug) Serial.print(steppers[i]->getPosition());
      if (debug) Serial.print("\t");
    }
    if (debug) Serial.println();
    force_log = true;
    log_position();

  }

  else if (command == "powertoggle") {
    powertoggle = !powertoggle;
    for (int i = 0; i < 6; i++) {
      digitalWrite(lpins[i][2], powertoggle);
    }
    if (debug) Serial.print("power toggled. enable pins ");
    if (debug) Serial.println((powertoggle) ? "ON" : "OFF");
  }

  else if (command == "shutoff") {
    if (debug) Serial.print("POWER SHUTOFF! checking again in ");

    for (int i = 0; i < 6; i++) {
      digitalWrite(lpins[i][2], LOW);
    }
    controller.emergencyStop();
    powertoggle = false;
    for (int i = 3; i > 0; i--) {
      if (debug) Serial.print(i);
      if (debug) Serial.print("... ");
      delay(1000);
    }
    if (debug) Serial.println();
  }

  else if (command == "cycle") {
    if (debug) Serial.println("starting cycle!");
    startnewcycle();
  }
  command = "";
}


/*      MAIN CYCLE:      */

void startnewcycle() {

  for (int i = 0; i < 6; i++) {
    targets[i] = whole_revs[i]*revolutions[i];
    steppers[i]->setTargetAbs(targets[i]);
    need_to_log = true;
    if (!controller.isRunning()) controller.moveAsync(steppers);
  }
}

/* ---------------------- */

time_t getTeensy3Time()
{
  return Teensy3Clock.get();
}

void printDigits(int digits){
  // utility function for digital clock display: prints preceding colon and leading 0
  Serial.print(":");
  if(digits < 10)
    Serial.print('0');
  Serial.print(digits);
}

void SD_read_positions() {
  logfile = SD.open("positions.txt", FILE_READ);
  if (!logfile){
    if (debug) Serial.println("can't open position log file");
  }
  else {
    if (debug) Serial.println("SD positions log file opened ok");
  }

  if (debug) Serial.println("----------");
  if (logfile.available()) {
    if (debug) Serial.print("positions: ");
    for (int i = 0; i < 6; i++) {
      positions[i] = logfile.parseInt();
      if (debug) Serial.print(positions[i]);
      if (debug) Serial.print("\t");
    }
    if (debug) Serial.println("// EOF");
  }
  logfile.close();
}


void relativemove(int m, long tgt) {
  if (debug) Serial.print("relative move: motor ");
  if (debug) Serial.print(m);
  if (debug) Serial.print(", amount: ");
  if (debug) Serial.println(tgt);
  steppers[m]->setTargetRel(tgt);
  controller.moveAsync(*steppers[m]);
  need_to_log = true;
}

void absolutemove(int m, long tgt) {
  if (debug) Serial.print("absolute move: motor ");
  if (debug) Serial.print(m);
  if (debug) Serial.print(", target: ");
  if (debug) Serial.println(tgt);
  steppers[m]->setTargetAbs(tgt);
  controller.moveAsync(*steppers[m]);
  need_to_log = true;
}

void setspeed(int s) {
  max_speed = s;
  if (debug) Serial.print("speed set to ");
  if (debug) Serial.println(s);
  for (int i = 0; i < 6; i++) {
    steppers[i]->setMaxSpeed(max_speed);
  }
}

void setaccel(int a) {
  if (debug) Serial.print("acceleration set to ");
  if (debug) Serial.println(a);
  acceleration = a;
  for (int i = 0; i < 6; i++) {
    steppers[i]->setAcceleration(acceleration);
  }
}

void report(int m) {
  if (debug) Serial.print("position of motor ");
  if (debug) Serial.print(m);
  if (debug) Serial.print(": ");
  if (debug) Serial.println(steppers[m]->getPosition());
}

void customcycle(float a, float b, float c, float d, float e, float f) {
    float fracs[] = {a, b, c, d, e, f};
    if (debug) Serial.println("custom cycle:");
    for (int i = 0; i < 6; i++) {
      if (debug) Serial.print(fracs[i]);
      if (debug) Serial.print("\t");
    }
    if (debug) Serial.println();

    if (debug) Serial.print("targets: ");
    for (int i = 0; i < 6; i++) {
      targets[i] = whole_revs[i]*fracs[i];
      steppers[i]->setTargetAbs(targets[i]);
      need_to_log = true;
      if (debug) Serial.print(targets[i]);
      if (debug) Serial.print("\t");
    }
    if (debug) Serial.println();
    if (debug) Serial.println("running");
    controller.moveAsync(steppers);
}

void overwriteposition(int m, long pos) {
  if (debug) Serial.print("overwriting motor ");
  if (debug) Serial.print(m);
  if (debug) Serial.print(" to ");
  if (debug) Serial.print(pos);
  if (debug) Serial.print(". confirmed at:");

  steppers[m]->setPosition(pos);
  positions[m] = pos;
  need_to_log = true;
  sd_log = true;
  if (debug) Serial.println(steppers[m]->getPosition());
}

void log_position() {
  // logs the position once every so often (interval defined global log_time variable)
  int sum = 0;

  if (force_log) {
    need_to_log = true;
    sd_log = true;
  }


  if (need_to_log) {

    if ((millis() % log_time < log_time/3 && log_pos == true) || force_log == true) {
      log_pos = false;
      if (debug) Serial.println("need to log");

      /* if (debug && sense(E_pins[3]) && sense(E_pins[4])) Serial.print("------->"); */
      if (debug) Serial.print("positions: ");
      if (debug) {
        for (int i = 0; i < 6; i++) {
          Serial.print(positions[i]);
          if (i < 5) Serial.print("\t");
        }
        Serial.println();
      }


      if ((millis() % sd_write_time < sd_write_time/3 && sd_log == true) || force_log == true) {
        if (debug) Serial.println("-- logging to SD card --");
        sd_log = false;
        logfile = SD.open("positions.txt", FILE_WRITE);
        if (logfile) {
          logfile.seek(0);
          logfile.truncate();
          for (int i = 0; i < 6; i++) {
            logfile.print(positions[i]);
            logfile.print((i < 5) ? ',' : '\n');
          }
          logfile.close();
        }
      }
      for (int i = 0; i < 6; i++) {
        sum += (prev_positions[i] == positions[i]) ? 0 : 1;
        prev_positions[i] = positions[i];
      }
      if (sum == 0) {
        sd_log = false;
        need_to_log = false;
      }
      else {
        sd_log = true;
        need_to_log = true;
      }
    }

    if (millis() % log_time > log_time*2/3) log_pos = true;
    if (millis() % sd_write_time > sd_write_time*2/3) sd_log = true;

  }
  force_log = false;
}
 
Hm, that was not what I meant. I was interested in a minimal example. Anyway, I extracted the info I needed from your huge sketch and did this:

Code:
#include <TeensyStep.h> // library for teensy-based motor control: https://luni64.github.io/TeensyStep/documentation/

StepControl controller; // controller object (manages individual steppers)

int max_speed    = 40000; // top speed
int acceleration = 20000; // acceleration = steps/sec/sec;

Stepper* steppers[6] = {new Stepper{0, 6}, new Stepper{1, 7}, new Stepper{2, 8}, new Stepper{3, 9}, new Stepper{4, 10}, new Stepper{5, 11}};

void setup()
{
    while(!Serial){}

    for (int i = 0; i < 6; i++)
    {
        steppers[i]->setMaxSpeed(max_speed);
        steppers[i]->setAcceleration(acceleration);
    }

    long whole_revs[]       = {29128, 60082, 14060, 44378, 1091, 29880}; // number of steps to spin each ring fully around
    const int revolutions[] = {6, 3, 12, 4, 2, 6};                       // how many times to spin each ring
    long targets[6];

    for (int i = 0; i < 6; i++)
    {
        targets[i] = whole_revs[i] * revolutions[i]; // destination = steps per revolution x number of revolutions
        Serial.printf("s%d: %d\n",i, targets[i]);
        steppers[i]->setTargetAbs(targets[i]);
    }
    controller.moveAsync(steppers);

} // end setup

void loop()
{
}

It prints the following targets:
Code:
s0: 174768
s1: 180246
s2: 168720
s3: 177512
s4: 2182
s5: 179280

And generates the following step sequence:
Screenshot 2021-09-13 211808.jpg

So, it does what it is supposed to do but you probably don't like it. TeensyStep uses Bresenham's algorithm which defines a lead motor (the motor which needs most steps, here s2) and then decides on each step of the lead motor if one of the slave motors needs to do a step. If the speeds are close, you end up with what you see: Most of the time the motors run at the same speed but from time to time the algorithm skips one step to make sure that all motors end up at their respective targets.
If you test it with more different speeds you'll end up with a more smooth movements. I'm afraid that Bresenham's algorithm can't do better then this.

Instead of moving the motors synced by TeensyStep, you could pre calculate the required speeds yourself and let the motors run independently (each motor gets its own controller). Since all timers are running of the same frequency this works pretty good but of course you are limited to 4 motors per Teensy...

Sorry to not have better news :-(
 
Ah so! thanks for the clarification (and sorry to dump a huge sketch on you). That graph is exactly what I suspected – I'm sure those skipped steps every once in a while are what's causing the noise.

In your opinion... would it work to switch to using the RotateControl class, calculate speeds beforehand, and manually keep an eye on position with RotateAsync? Presumably it wouldn't be too hard to implement acceleration as well with overrideSpeed if needed.

Or are you saying that even with RotateControl, grouping all six under one controller means Bresenham comes into play?

Thank you so much for your help –and for your library, it's so great.
 
In your opinion... would it work to switch to using the RotateControl class, calculate speeds beforehand, and manually keep an eye on position with RotateAsync? Presumably it wouldn't be too hard to implement acceleration as well with overrideSpeed if needed.

No, I'd stick with the step controllers, that ensures that you end up with the right position. Just precalc the speed of each motor that they reach their target at the same time.

Or are you saying that even with RotateControl, grouping all six under one controller means Bresenham comes into play?
Yes, as soon as you give more than one motor to a controller (step or rotation) it moves them synced with Bresenham. The only chance you have is moving each motor with its dedicated controller. I'd get a second Teensy to move all 6.
 
No, I'd stick with the step controllers, that ensures that you end up with the right position. Just precalc the speed of each motor that they reach their target at the same time.

Copy that. Makes sense. Thanks again for the response – really appreciate you taking the time. Next time you're in San Francisco, I owe you a beer =)
 
Status
Not open for further replies.
Back
Top