Need fastest way to write to servos

Projects Programming
1

I apologize if this is a vague question. Essentially, I have a stepper motor running in the main loop at a high speed. I need to be able to change the positions of two servos while this motor is running. The issue is that at high speeds, there are noticable "bumps" in the stepper's movement which results in step and position loss. I believe this is caused by the if() statement inside my while loop that is writing to the servos, but I could be wrong. When I remove the writeServos() function from the while loops and run at high speeds, there are no bumps.

Here is some of my code including the main loop and the function I used to write to the servos. The positions I write to are stored in 2 array that I call from during the loop. I am running the stepper with the AccelStepper library. I am using an Arduino MKR Zero to drive this which is definitely not the best choice for this kind of application but it is what I have.

int lastPos1;
int lastPos2;

void writeServos(int pos1, int pos2){
  if(pos1 != lastPos1){
    servo1.write(pos1);
    
  }
  if(pos2 != lastPos2){
    servo2.write(pos2);
  }
  lastPos1 = pos1;
  lastPos2 = pos2;
}

int lastAmplitude;

void loop() {
  //amplitude = getAmplitude(); //288 * 2;
  amplitude = 686;

  if(amplitude != lastAmplitude){
    createServoArray(amplitude);
  }

  speed = getSpeed();
  //speed = 4000;
  int accel = speed / 1.15; // dont change this scalar
  int i = 0;

  long cycleStartTime = micros();
    
  stepper.moveTo(-amplitude);
  Serial.println("moving to -" + String(amplitude));
  //setSpeed();
  stepper.setMaxSpeed(speed);
  stepper.setAcceleration(accel);
  int lastPosition = 0;

  if(!outsideBounds){
    while(stepper.distanceToGo() < 0 && !outsideBounds){
      if(stepper.currentPosition() != lastPosition){  // <------- I believe the lines contained in this statement are the issue
        writeServos(servoArray[i], servo2Array[i]);
        i++;
        lastPosition = stepper.currentPosition();
      }
      stepper.run();
    }
  
    stepper.stop();
    stepper.runToPosition();
    stepper.moveTo(0 + error);    
    Serial.println("moving to 0");
  
    while(stepper.distanceToGo() > 0 && !outsideBounds){
      if(stepper.currentPosition() != lastPosition){  // <------- I believe the lines contained in this statement are the issue
        writeServos(servoArray[i], servo2Array[i]);
        i++;
        lastPosition = stepper.currentPosition();
      }
      stepper.currentPosition();
      stepper.run();
    }
  
    // cycle end print lines
    stepper.stop();
    stepper.runToPosition();
    cycles++;
    error = stepError();
    actualHz = 1 / ((micros() - cycleStartTime) / 1e6); 
    Serial.println("cycle: " + String(cycles));
    Serial.println("hz: " + String(actualHz, 6));
    Serial.println("encoder pos: " + String(encoder_pos) + ", step error: " + String(error));
    Serial.println("");
    delayMicroseconds(5000);
    lastAmplitude = amplitude;
    i = 0;
  }
}

Does anyone have any suggestions as to how I can fix this? Is there a faster way to write to servos? Or a way I could restructure this to make runtime faster?

2

You should post all of your code.

3

Ok, here you go. Most of it isn't relevant to my question

#include <AccelStepper.h>
#include <Servo.h>
#include "avdweb_AnalogReadFast.h"

// stepper variables

const int enaPin = 0;
const int dirPin = 1;
const int stepPin = 2;

int msMultiplier = 2;
int spr = 200 * msMultiplier;
int speed;
int maxSpeed = 7500; // 7500 fastest allowable
bool isRunning = false;

int currentPos;

AccelStepper stepper = AccelStepper(1, stepPin, dirPin);

// servo variables

Servo servo1;
Servo servo2;

const int servo1Pin = 10;
const int servo2Pin = 12;
const int minPulseWidth = 544;
const int maxPulseWidth = 2400;
const int midPos = 34 + (156 - 32) / 2;
int servoPos = 0;
int servoAmp = 100;

// encoder variables

const int channelAPin = 9;
const int channelBPin = 8;
const int indexPin = 7;
int encoder_pos = 0;
bool start_homing = false;
//int error = 0;

// other variables

#define stepPotPin A5
#define ampPotPin A4
const int limitPin = 6;
bool limit_hit = false;
bool homed = false;

// position variables

int upperEncoderBound = 44700;
int lowerEncoderBound = 0;
int stepsToTop = 3180;
bool outsideBounds = false;

long startTime = 0;
long endTime = 0;
int amplitude;
//int ampInSteps = 50 * (137 / 10);
int ampInSteps = 288 * 2;
int ampInTicks = amplitude * 78.7401;


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

  pinMode(enaPin, OUTPUT);
  digitalWrite(enaPin, LOW);
  pinMode(dirPin, OUTPUT);

  pinMode(channelAPin, INPUT_PULLDOWN);
  pinMode(channelBPin, INPUT_PULLDOWN); 
  pinMode(indexPin, INPUT_PULLDOWN);

  pinMode(limitPin, INPUT);
  digitalWrite(limitPin, LOW);

  pinMode(stepPotPin, INPUT);
  pinMode(ampPotPin, INPUT);

  attachInterrupt(digitalPinToInterrupt(channelAPin), encode, CHANGE);
  attachInterrupt(digitalPinToInterrupt(indexPin), indexHit, RISING);
  attachInterrupt(digitalPinToInterrupt(limitPin), limit_switch_hit, FALLING);
  //attachInterrupt(digitalPinToInterrupt(stepPotPin), setSpeed, CHANGE);

  servo1.attach(servo1Pin, minPulseWidth, maxPulseWidth);
  servo2.attach(servo2Pin, minPulseWidth, maxPulseWidth);

  home();
}

int calculateError(){
  int ticksLost = 0;

  if(encoder_pos > upperEncoderBound / 2){
    ticksLost = upperEncoderBound - encoder_pos;
  }else{
    ticksLost = 0 - encoder_pos;
  }
  return (ticksLost / 137) * 10;
}

void home(){
  Serial.println("starting homing sequence");
  servo1.write(midPos);
  servo2.write(midPos);
  digitalWrite(dirPin, LOW);

  while(limit_hit != true){
    //single_step(1);
    runAtSpeed(2000, 1);
  }

  digitalWrite(dirPin, HIGH);

  while(encoder_pos < upperEncoderBound){
    //single_step(1);
    runAtSpeed(2000, 0);
  }

  digitalWrite(dirPin, LOW);
  Serial.println(ampInTicks);
  stepper.setCurrentPosition(0);
  digitalWrite(dirPin, LOW);
  Serial.println("homed");
  homed = true;
  delay(1000);
}

void limit_switch_hit(){
  static unsigned long last_interrupt_time = 0;
  unsigned long interrupt_time = millis();

  if(interrupt_time - last_interrupt_time > 50){
    Serial.println("limit hit");
    if(limit_hit == true){
      digitalWrite(enaPin, LOW);
      //stepper.stop();
      outsideBounds = true;
    } 
    encoder_pos = 0;
    limit_hit = true;
    last_interrupt_time = interrupt_time;
  }
}

void encode(){
  int a = digitalRead(channelAPin);
  int b = digitalRead(channelBPin);

  if(a == HIGH){
    if(b == LOW){
      encoder_pos++;
    }else{
      encoder_pos--;
    }
  }else{
    if(b == LOW){
      encoder_pos--;
    }else{
      encoder_pos++;  
    }
  }
  
  if((encoder_pos > upperEncoderBound + 500 || encoder_pos < 150) && homed == true){ 
    digitalWrite(enaPin, LOW); 
    Serial.println("encoder detects out of bounds");
    //stepper.stop();
    outsideBounds = true;
  }
}

void indexHit(){
  Serial.println("index");
}

void index(){
  static unsigned long last_interrupt_time = 0;
  unsigned long interrupt_time = millis();

  if(interrupt_time - last_interrupt_time > 200 && limit_hit == true){
    encoder_pos = 0;
    if(start_homing == false){
      stepper.stop();
      Serial.println("Stepper homed.");
      start_homing = true;
    }else{
      homed = true;
    }
    Serial.println("at index");
  }
  last_interrupt_time = interrupt_time;
}

unsigned long lastStepTime;

void runAtSpeed(unsigned long stepInterval, int dir){
  unsigned long time = micros();

  if(time - lastStepTime >= stepInterval){
    if(dir == 1){
      digitalWrite(dirPin, LOW);
      currentPos += 1;
    }else{
      digitalWrite(dirPin, HIGH);
      currentPos -= 1;
    }

    stepper.step(currentPos);
    lastStepTime = time;
  }
}

int getSpeed(){
  int potVal = analogReadFast(stepPotPin);
  if(potVal == -1){
    isRunning = false;
    return 0;
  }else{
    isRunning = true;
    return map(potVal, 0, 932, 2900, 10000); // tweak the min and max output values so that pot always maps to 0.5 Hz (2900) -> 3.0 Hz ()
  }
}

int getAmplitude(){
  int amp = map(analogReadFast(ampPotPin), 0, 932, 50, 200) * 137 / 10; // * (10 / 137) * 2; 
  return amp;
}

int stepError(){
  return (upperEncoderBound - encoder_pos) * 10 / 137;
}

int cycles = 0;
int error = 0;
float actualHz;

int servoArray[686 * 2];
int servo2Array[686 * 2];

void createServoArray(int amp){
  Serial.println("creating servo array.");
  int index = 0;
  for(int i = -amp; i < 0; i++){
    //Serial.println(String(i) + ", doing first half");
    if(i < (-amp / 2)){
      servoArray[index] = map(i, -amp, -amp / 2, midPos, (midPos + servoAmp / 2));
      //Serial.println("to amp, " + String(map(stepper.distanceToGo(), -amplitude, -amplitude / 2, midPos, (midPos + servoAmp / 2))));
    }else{
      //Serial.println("to 0, " + String(map(stepper.distanceToGo(), -amplitude / 2, 0, (midPos + servoAmp / 2), midPos)));
      servoArray[index] = map(i, -amp / 2, 0, (midPos + servoAmp / 2), midPos);
    }
    index++;
  }

  for(int i = amp; i > 0; i--){
    //Serial.println("second half");
    if(i > (amp / 2)){
      //Serial.println(String(index) + ", " + String(map(i, amplitude, amplitude / 2, midPos, (midPos - servoAmp / 2))));
      servoArray[index] = map(i, amp, amp / 2, midPos, (midPos - servoAmp / 2));
      //Serial.println(map(stepper.distanceToGo(), amplitude, amplitude / 2, midPos, (midPos - servoAmp / 2)));
    }else{
      //Serial.println(map(stepper.distanceToGo(), amplitude / 2, 0, (midPos - servoAmp / 2), midPos));
      servoArray[index] = map(i, amp / 2, 0, (midPos - servoAmp / 2), midPos);
    }
    index++;
  }

  for(int i = 0; i < amplitude * 2; i++){
      servo2Array[i] = map(servoArray[i], midPos - servoAmp / 2, midPos + servoAmp / 2, midPos + servoAmp / 2, midPos - servoAmp / 2);
  }

  // for(int i = 0; i < amplitude * 2; i++){
  //   if(i == amplitude){
  //     Serial.print("middle, ");
  //   }
  //   Serial.print(String(servoArray[i]) + ", ");
  // }
}

int lastPos1;
int lastPos2;

void writeServos(int pos1, int pos2){
  if(pos1 != lastPos1){
    servo1.write(pos1);
    
  }
  if(pos2 != lastPos2){
    servo2.write(pos2);
  }
  lastPos1 = pos1;
  lastPos2 = pos2;
}

int lastAmplitude;

void loop() {
  //amplitude = getAmplitude(); //288 * 2;
  amplitude = 686;

  if(amplitude != lastAmplitude){
    createServoArray(amplitude);
  }

  speed = getSpeed();
  //speed = 4000;
  int accel = speed / 1.15; // dont change this scalar
  int i = 0;

  long cycleStartTime = micros();
    
  stepper.moveTo(-amplitude);
  Serial.println("moving to -" + String(amplitude));
  //setSpeed();
  stepper.setMaxSpeed(speed);
  stepper.setAcceleration(accel);
  int lastPosition = 0;

  if(!outsideBounds){
  while(stepper.distanceToGo() < 0 && !outsideBounds){
    //Serial.println(stepper.distanceToGo());
    // if(stepper.distanceToGo() < (-amplitude / 2)){
    //   writeServos(map(stepper.distanceToGo(), -amplitude, -amplitude / 2, midPos, (midPos + servoAmp / 2)));
    //   //Serial.println("to amp, " + String(map(stepper.distanceToGo(), -amplitude, -amplitude / 2, midPos, (midPos + servoAmp / 2))));
    // }else{
    //   //Serial.println("to 0, " + String(map(stepper.distanceToGo(), -amplitude / 2, 0, (midPos + servoAmp / 2), midPos)));
    //   writeServos(map(stepper.distanceToGo(), -amplitude / 2, 0, (midPos + servoAmp / 2), midPos));
    // }
    //writeServos(servoArray[i]);
    if(stepper.currentPosition() != lastPosition){
      writeServos(servoArray[i], servo2Array[i]);
      i++;
      lastPosition = stepper.currentPosition();
    }
    //stepper.currentPosition();

    stepper.run();
    //delayMicroseconds(5000);
  }

  stepper.stop();
  stepper.runToPosition();
  stepper.moveTo(0 + error);    
  Serial.println("moving to 0");

  while(stepper.distanceToGo() > 0 && !outsideBounds){
    //Serial.println(stepper.distanceToGo());
    // if(stepper.distanceToGo() > (amplitude / 2)){
    //   writeServos(map(stepper.distanceToGo(), amplitude, amplitude / 2, midPos, (midPos - servoAmp / 2)));
    //   //Serial.println(map(stepper.distanceToGo(), amplitude, amplitude / 2, midPos, (midPos - servoAmp / 2)));
    // }else{
    //   //Serial.println(map(stepper.distanceToGo(), amplitude / 2, 0, (midPos - servoAmp / 2), midPos));
    //   writeServos(map(stepper.distanceToGo(), amplitude / 2, 0, (midPos - servoAmp / 2), midPos));
    // }

    if(stepper.currentPosition() != lastPosition){
      writeServos(servoArray[i], servo2Array[i]);
      i++;
      lastPosition = stepper.currentPosition();
    }
    stepper.currentPosition();
    stepper.run();
  }

  // cycle end print lines
  stepper.stop();
  stepper.runToPosition();
  cycles++;
  error = stepError();
  actualHz = 1 / ((micros() - cycleStartTime) / 1e6); 
  Serial.println("cycle: " + String(cycles));
  Serial.println("hz: " + String(actualHz, 6));
  Serial.println("encoder pos: " + String(encoder_pos) + ", step error: " + String(error));
  Serial.println("");
  delayMicroseconds(5000);
  lastAmplitude = amplitude;
  i = 0;
  }
}
4

What board are you using?

5

Arduino MKR Zero

6

The problem doesn’t appear to be how fast you’re writing to the servo, but rather, that your other functions are blocking for no good reason. (while, do, for etc)

To interleave operations like this requires a complete rethinking the strategy of your code.

Use loop() to do what it does - continuously - and minimise all the other software loops currently within the code.

7

Does "INPUT_PULLDOWN" exist?

8

I should have been more clear; I don't need the loop() to run insanely fast, I need the stepper.run() function to be called as many times per second as possible in the two while loops. Like I said, removing the if statements that are inside these while loops fixes the issue. Is there a way I can fix this without a rewrite?

As far as I am aware input_pulldown does exist

9

I see, on the MKR Zero.

10

I think a good place to start with what you’ve said, is to use AUTO FORMAT to correct all the indenting, repost the complete code in a new post, and look at how the loop could be improved.

There are too many miscellaneous ‘if/maybe/do this’ inside loop(),
Good form suggests those maybe put out into functions, so loop remains as clean as possible,. and makes the code more readable.

11

Hello owenmckenney

You are asking for:

Need fastest way to write to servos

Check you coding first to prevent this may blocking the realtime processing:

Line 111:   while(limit_hit != true){
	Line 118:   while(encoder_pos < upperEncoderBound){
	Line 323:   while(stepper.distanceToGo() < 0 && !outsideBounds){
	Line 349:   while(stepper.distanceToGo() > 0 && !outsideBounds){

and

	Line 129:   delay(1000);
	Line 341:     //delayMicroseconds(5000);
	Line 378:   delayMicroseconds(5000);

Have a nice day and enjoy coding in C++.

1 Like
12

Also, boost your serial speed to 115200 or faster. 9600 is extremely slow and your code has to wait for the Serial.println() to finish writing to the Serial Monitor before moving on. Better yet, if your code is working and you don't need the debugging anymore, get rid of the Serial.print stuff altogether.

13

Output is buffered. What is described here only happens if too much output is sent, the buffer fills, and the code must wait for buffer space to open up.
That being said, yes, raise your baud rate, and strip the serial outputs from the final code using #ifdef statements or other trickery.

1 Like
14

The first instruction in loop() should be:

void loop() {
  stepper.run();

And any other long loop:

while(stepper.distanceToGo() > 0 && !outsideBounds){
  stepper.run(); // <<<<<<<<<
      if(stepper.currentPosition() != lastPosition){  // <------- I believe the lines contained in this statement are the issue
        writeServos(servoArray[i], servo2Array[i]);
        i++;
        lastPosition = stepper.currentPosition();
      }
15

Your question itself does not make sense, because it does not make any sense to write to a servo more than once every 20ms. The servo position is defined by the length of a pulse, and this pulse is created only once every 20ms - no matter how often you call servo.write().
So you should not write as fast as possible to the servo, but as slow as possible.

            if (stepper.currentPosition() != lastPosition) {
                writeServos(servoArray[i], servo2Array[i]);
                i++;
                lastPosition = stepper.currentPosition();
            }

You write to the servo in your while loop with every step created. That is far to often and slows down your loop unnecessarily.

That is only apart from all other useful advices already given.

P.S.I have rarely seen a programme of that length with nearly no comments. Even you will get in trouble, if you want to change it in a few month.

16

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.