Help with smooth servo movement

[jasper1234567890]

Hi!

i am trying to make my servo movement smooth but it isn't quite working out, i watched this video: How To Make Robots Move Smoothly | Arduino Tutorial - YouTube start at 4:35 By James Bruton
and with his code:

#include <Servo.h>

int pot1;
int pot2;
int pot3;
int pot4;
int pot5;
int pot6;

float pot1Scaled;
float pot2Scaled;
float pot3Scaled;
float pot4Scaled;
float pot4aScaled;
float pot5Scaled;
float pot6Scaled;

float pot1Smoothed;
float pot2Smoothed;
float pot3Smoothed;
float pot4Smoothed;
float pot4aSmoothed;
float pot5Smoothed;
float pot6Smoothed;

float pot1SmoothedPrev;
float pot2SmoothedPrev;
float pot3SmoothedPrev;
float pot4SmoothedPrev;
float pot4aSmoothedPrev;
float pot5SmoothedPrev;
float pot6SmoothedPrev;

Servo servo1;
Servo servo2;
Servo servo3;

Servo servo4;
Servo servo5;
Servo servo6;
Servo servo7;

void setup() {

  pinMode(A0, INPUT);
  pinMode(A1, INPUT);
  pinMode(A2, INPUT);
  pinMode(A3, INPUT);
  pinMode(A4, INPUT);
  pinMode(A5, INPUT);

  Serial.begin(115200);

  servo1.attach(4);
  servo2.attach(5);
  servo3.attach(6);

  servo4.attach(7);
  servo5.attach(8);
  servo6.attach(9);
  servo7.attach(10);

  servo1.writeMicroseconds(1200);
  servo2.writeMicroseconds(1650);
  servo3.writeMicroseconds(1650);

  servo4.writeMicroseconds(1500);
  servo5.writeMicroseconds(1550);
  servo6.writeMicroseconds(1500);
  servo7.writeMicroseconds(1450);

}

void loop() {

  pot1 = analogRead(A0);
  pot2 = analogRead(A1);
  pot3 = analogRead(A2);
  pot4 = analogRead(A3);
  pot5 = analogRead(A4);
  pot6 = analogRead(A5);

  // scale all pots for the servo microseconds range

  pot1Scaled = ((pot1 - 512) * -1.6) + 1200;
  pot2Scaled = (pot2 - 512) + 1500;
  pot3Scaled = (pot3 - 512) + 1500;
  pot4Scaled = ((pot4 - 512) * 1.2) + 1500;
  pot4aScaled = ((pot4 - 512) * -1.2) + 1500;
  pot5Scaled = (pot5 - 512) * 1.5;
  pot6Scaled = (pot6 - 512) + 1650;

  pot4Scaled = constrain(pot4Scaled,900,1500);
  pot4aScaled = constrain(pot4aScaled,1500,2100);

  // smooth pots

  pot1Smoothed = (pot1Scaled * 0.02) + (pot1SmoothedPrev * 0.98);
  pot2Smoothed = (pot2Scaled * 0.02) + (pot2SmoothedPrev * 0.98);
  pot3Smoothed = (pot3Scaled * 0.02) + (pot3SmoothedPrev * 0.98);
  pot4Smoothed = (pot4Scaled * 0.05) + (pot4SmoothedPrev * 0.95);
  pot4aSmoothed = (pot4aScaled * 0.05) + (pot4aSmoothedPrev * 0.95);
  pot5Smoothed = (pot5Scaled * 0.02) + (pot5SmoothedPrev * 0.98);
  pot6Smoothed = (pot6Scaled * 0.02) + (pot6SmoothedPrev * 0.98);  

  // bookmark previous values

  pot1SmoothedPrev = pot1Smoothed;
  pot2SmoothedPrev = pot2Smoothed;
  pot3SmoothedPrev = pot3Smoothed;
  pot4SmoothedPrev = pot4Smoothed;
  pot4aSmoothedPrev = pot4aSmoothed;
  pot5SmoothedPrev = pot5Smoothed;
  pot6SmoothedPrev = pot6Smoothed;

  Serial.print(pot1Smoothed);
  Serial.print(" , ");
  Serial.print(pot2Smoothed);
  Serial.print(" , ");
  Serial.print(pot3Smoothed);
  Serial.print(" , ");
  Serial.print(pot4Smoothed);
  Serial.print(" , ");
  Serial.print(pot5Smoothed);
  Serial.print(" , ");
  Serial.println(pot6Smoothed);

  // write servos

  servo1.writeMicroseconds(pot1Smoothed);
  servo2.writeMicroseconds(pot6Smoothed - pot5Smoothed);
  servo3.writeMicroseconds(pot6Smoothed + pot5Smoothed);

  servo5.writeMicroseconds(pot2Smoothed);
  servo7.writeMicroseconds(pot2Smoothed);

  servo4.writeMicroseconds(pot4Smoothed);
  servo6.writeMicroseconds(pot4aSmoothed);



  delay(5);                      // run loop 200 times/second

}

link for the code:

Add files via upload

i managed to make this:
receiver code

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

#define CE_PIN 49
#define CSN_PIN 48

RF24 radio(CE_PIN, CSN_PIN);
const byte pipe[] = "00001";

const byte servoPins[] = {2, 5, 4, 3};     //s1 =(2) s2 =(5) s3 =(4) s4 =(3)
const byte servosCount = sizeof servoPins / sizeof * servoPins;
const int initialPositions[servosCount] = {90, 90, 90, 84};
const int mappedRanges[servosCount][2] = {{80, 100}, {65, 115}, {65, 115}, {80, 88}};

Servo servos[servosCount];

struct __attribute__ ((packed)) t_message {
  int16_t rawValues[servosCount];
} payload;
uint8_t messageBuffer[sizeof(t_message)];

void setup() {
  pinMode(10, OUTPUT);

  for (byte i = 0; i < servosCount; i++) {
    servos[i].write(initialPositions[i]);     //set starting positions
    servos[i].attach(servoPins[i]);
  }

// Serial
  Serial.begin(9600);
//radio
  radio.begin();
  radio.openReadingPipe(0, pipe);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
}

void loop() {
  if (radio.available()) {
    radio.read(messageBuffer, sizeof messageBuffer);
    memcpy(&payload, messageBuffer, sizeof payload);
    Serial.print(F("New positions: "));
    for (byte i = 0; i < servosCount; i++) {
      float angle = map(payload.rawValues[i], 0, 1023, mappedRanges[i][0], mappedRanges[i][1]);

      angle = angle * 100;
   float smoothAngle = (angle * 0.05) + (angle * 0.95);
   float finalAngle = smoothAngle;

      Serial.print(angle); Serial.write('\t');
      servos[i].write(finalAngle);     
    }
    Serial.println();
  }
delay(30);
}

previous receiver code:

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

#define CE_PIN 49
#define CSN_PIN 48

RF24 radio(CE_PIN, CSN_PIN);
const byte pipe[] = "00001";

const byte servoPins[] = {2, 5, 4, 3};     //s1 =(2) s2 =(5) s3 =(4) s4 =(3)
const byte servosCount = sizeof servoPins / sizeof * servoPins;
const int initialPositions[servosCount] = {90, 90, 90, 84};
const int mappedRanges[servosCount][2] = {{80, 100}, {65, 115}, {65, 115}, {80, 88}};

Servo servos[servosCount];

struct __attribute__ ((packed)) t_message {
  int16_t rawValues[servosCount];
} payload;
uint8_t messageBuffer[sizeof(t_message)];

void setup() {
  pinMode(10, OUTPUT);

  for (byte i = 0; i < servosCount; i++) {
    servos[i].write(initialPositions[i]);     //set starting positions
    servos[i].attach(servoPins[i]);
  }

// Serial
  Serial.begin(9600);
//radio
  radio.begin();
  radio.openReadingPipe(0, pipe);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
}

void loop() {
  if (radio.available()) {
    radio.read(messageBuffer, sizeof messageBuffer);
    memcpy(&payload, messageBuffer, sizeof payload);
    Serial.print(F("New positions: "));
    for (byte i = 0; i < servosCount; i++) {
      int angle = map(payload.rawValues[i], 0, 1023, mappedRanges[i][0], mappedRanges[i][1]);
      Serial.print(angle); Serial.write('\t');
      servos[i].write(angle);     
    }
    Serial.println();
  }
delay(30);
}

transmitter code:

#include <SPI.h>
#include <RF24.h>
#include <nRF24L01.h>

#define CE_PIN 7
#define CSN_PIN 6

RF24 radio(CE_PIN, CSN_PIN);
const byte pipe[] = "00001";

const byte joystickPins[] = {A7, A2, A3, A5};
const byte joysticksCount = sizeof joystickPins / sizeof * joystickPins;

struct __attribute__ ((packed)) t_message {
  int16_t rawValues[joysticksCount];
} payload, previousPayload;


void setup() {
  pinMode(10, OUTPUT);
  radio.begin();
  Serial.begin(9600);
  radio.openWritingPipe(pipe);
  radio.setPALevel(RF24_PA_MIN);
  radio.stopListening();
}

void loop() {
  // read the joysticks
  for (byte i = 0; i < joysticksCount; i++) {
    payload.rawValues[i] = analogRead(joystickPins[i]);
    payload.rawValues[i] = analogRead(joystickPins[i]) & 0xFFFD; // two reads for stability, dropping the 2 LSb to filter out instability
  }

  // broadcast the data if it has changed
  if (memcmp(&payload, &previousPayload, sizeof(t_message)) != 0) {  // returns 0 when they match, https://cplusplus.com/reference/cstring/memcmp/
    radio.write(&payload, sizeof(payload));
    previousPayload = payload;
  }
delay(30);
}

i only plugged my calbe in for 0.3s (that was a good idea) because the servo's instantly moves to their max. or min. mappedRanges. and that is not ok.

in his code he has 3 variables all with a funtion, but i only managed to get 2. so im missing one, but i don't know wich one and how to add it to the code.

[J-M-L]

what arduino do you use?
can you share the wiring (circuit )and how things are powered?

[alto777]

@jasper1234567890

i managed to make this:

Did you run the code you didn't write and get the results that the author enjoyed?

a7

[Whandall]

@jasper1234567890

    radio.read(messageBuffer, sizeof messageBuffer);
    memcpy(&payload, messageBuffer, sizeof payload);

Why do you copy the packet? I would use

    radio.read(&payload, sizeof payload);

@jasper1234567890

   float smoothAngle = (angle * 0.05) + (angle * 0.95);

A rather complicated way for

  float smoothAngle = angle;

@jasper1234567890

delay(30);

Why?

[Whandall]

@jasper1234567890

    angle = angle * 100;

This is strange too.

[J-M-L]

I had only seen the old receiving code.. You missed the point on smoothing data and got confused by the author original idea when dealing with a digital pin...

@jasper1234567890

    for (byte i = 0; i < servosCount; i++) {
      float angle = map(payload.rawValues[i], 0, 1023, mappedRanges[i][0], mappedRanges[i][1]);

      angle = angle * 100;
   float smoothAngle = (angle * 0.05) + (angle * 0.95);
   float finalAngle = smoothAngle;

      Serial.print(angle); Serial.write('\t');
      servos[i].write(finalAngle);     
    }

you don't need x100, you need to take X% of the new reading and (1.0-X)% of the previous value (which this you need to maintain as a global variable)

try something like

#include <SPI.h>
#include <nRF24L01.h>
#include <RF24.h>
#include <Servo.h>

#define CE_PIN 49
#define CSN_PIN 48

RF24 radio(CE_PIN, CSN_PIN);
const byte pipe[] = "00001";

const byte servoPins[] = {2, 5, 4, 3};     //s1 =(2) s2 =(5) s3 =(4) s4 =(3)
const byte servosCount = sizeof servoPins / sizeof * servoPins;
int previousAngles[servosCount];
const int initialPositions[servosCount] = {90, 90, 90, 84};
const int mappedRanges[servosCount][2] = {{80, 100}, {65, 115}, {65, 115}, {80, 88}};

Servo servos[servosCount];

struct __attribute__ ((packed)) t_message {
  int16_t rawValues[servosCount];
} payload;


void setup() {
  pinMode(10, OUTPUT);

  for (byte i = 0; i < servosCount; i++) {
    servos[i].write(initialPositions[i]);     //set starting positions
    servos[i].attach(servoPins[i]);
    previousAngles[i] = initialPositions[i];
  }

  // Serial
  Serial.begin(9600);
  //radio
  radio.begin();
  radio.openReadingPipe(0, pipe);
  radio.setPALevel(RF24_PA_MIN);
  radio.startListening();
}

void loop() {
  if (radio.available()) {
    radio.read((uint8_t *) &payload, sizeof payload);
    Serial.print(F("New positions: "));
    for (byte i = 0; i < servosCount; i++) {
      float angle = map(payload.rawValues[i], 0, 1023, mappedRanges[i][0], mappedRanges[i][1]);
      previousAngles[i] = (angle * 0.2) + (previousAngles[i] * 0.8);
      Serial.print(previousAngles[i]); Serial.write('\t');
      servos[i].write(previousAngles[i]);
    }
    Serial.println();
  }
  delay(30);
}

(typed here fully untested)

[dlloyd]

With an ESP32 and the ESP32 ESP32S2 AnalogWrite library, you can control a servo with about 0.05 degree resolution (16-bit PWM).

This example drives 2 servos with independent speed control ... smooth action can be observed over a wide range of speed.

Dual_Servo_Sweep_Speed.ino - Wokwi Arduino and ESP32 Simulator

[alto777]

@dlloyd

This example drives 2 servos

Yes it does! Why 2x

    delay(5);  // for simulator 

I am a huge fan of the wokwi and am interested in anything that isn't full fidelity or needs be done differently. From real life.

a7

[dlloyd]

@alto777

Yes it does! Why 2x

No reason, other than to demonstrate several ranges of independent speed control. Could be also be fractional. If the speed control is set higher than the actual sevo response, then the servo position won't reach its limits of 0 and 180 deg (when using a continuous sweep function).

   speed1 = (analogRead(speedPin1)) / 480;     // 0-853% speed

The delay(5) for simulator allows the simulation to catch up and not introduce its own "simulated jitter".

[alto777]

@dlloyd

The delay(5) for simulator allows the simulation to catch up and not introduce its own "simulated jitter".

Well I did what I should have and commnted out the two delays. Are you saying that doesn't make trash of the demo in real life?

As I read it, without something slowing the loop down you get odd behaviour. There's not much going on otherwise to slow what would be a very fast loop.

In which case I wonder if you really just always want to run the loop at ~ 100 Hz?

I lost the delay()s and inserted a loop throttle as the first staments in loop(), viz:

  static unsigned long lastTime;
  unsigned long now = millis();
  if (now - millis() < 10)
    return;

  lastTime = now;

Which to no one's surprise made everything smooth, and takes, I think, the wokwi off the hook for anything it is doing diffferent to what real life would do.

a7

[jasper1234567890]

these are alot of responses, i will look deeper into then tommorrow but for now this,
i am using an arduino MEGA,
schematics:

i did not run the code i didn't write because i is of no use to me and no suitable for the project i am working on, i is just there to see how it has to be done.

i don't know why it copys the packet i think it is for stablilty just like i checks the joystick value twice in the transmitter code. ( i believe i havent pasted it in here, but that won't be of use).
i put the delay there because sometimes the receiver stops receiving or the tranmitter stops transmitting, i found out that the receiver needs a reset and the it works just fine.
remember i am not good at arduino (or you could say noob) and i am just trying things so some stuff could be very strange for you guys.

i will test the code you gave tomorrow @J-M-L
and same goes for taking a look at the sweep thing @dlloyd

thank you everyone!
i will be back tomorrow.

[alto777]

@jasper1234567890

i did not run the code i didn't write

I only asked about that because having known good code - for anything you plan to use - is very handy for verifying wiring and power and basic functionality as well as providing a good start on the code you will write.

And having those bits around can come in handy when things aren't going well, to drop back in and make sure nothing hardware-wise has become, for whatever reason, the reason for that.

a7

[jasper1234567890]

all my wiring and power stuff is correct because the 'unedited' receiver codes works just fine with the transmitter code, and now it is just a matter of adding things.

[gcjr]

@jasper1234567890

const byte joysticksCount = sizeof joystickPins / sizeof * joystickPins;

this doesn't make sense to me. is "joysticksCount" not zero?

since "joystickPins" is bytes, why not const byte joysticksCount = sizeof(joystickPins);

[J-M-L]

this is very often found

@jasper1234567890

const byte joystickPins[] = {A7, A2, A3, A5};
const byte joysticksCount = sizeof joystickPins / sizeof * joystickPins;

or its variation

const byte joystickPins[] = {A7, A2, A3, A5};
const byte joysticksCount = sizeof joystickPins / sizeof  joystickPins[0];

You are right that here since the sizeof a byte is 1, you can get the count without dividing by 1... but all this is done at compile time so has no code repercussion and it works regardless of the type of the data, so you'll frequently find it as such.

PS/ although they won't hurt, there is no need for the parenthesis (you use them for a type name, not a variable)

const byte joysticksCount = sizeof joystickPins;

PS2: still waiting to hear from the test of the code I posted in #6

[jasper1234567890]

sorry i haven't been active for the past couple days, but due to the new year stuff i didn't quite get the time to just sit down and work on it again.

but here i am again!

here are my results for the code on post #6:

• checked the code verification, it works.
• servo's move to their starting positions.
• when moving the servo's (s2, s3) they move, but for moving back to the middle when i let go of the joystick it goes slow. it has like a 0,4s wait before moving back.

after editing the number (for testing and understanding it) to '0,4' and '0,6' is went back faster.
but the way it is right now is not how i want i to be in the end, the servo's are moving to fast and maybe the acceleration is not good enough. but i don't now for sure it might be fine and it is just the servo speed.

i know there is a library called VarSpeedServo but it is not on the arduino web editor and i can't seem to upload it.
i know there is another with anglesteps to ajust the speed. just like in this code:

#include <Servo.h>

Servo myservo;  // create servo object to control a servo
#define servoPin 3 //~
#define pushButtonPin 2 

int angle =90;    // initial angle  for servo (beteen 1 and 179)
int angleStep =10;
const int minAngle = 0;
const int maxAngle = 180;

const int type =2;//watch video for details. Link is at the top of this code (robojax)

int buttonPushed =0;

void setup() {
  // Servo button demo by Robojax.com
  Serial.begin(9600);          //  setup serial
  myservo.attach(servoPin);  // attaches the servo on pin 3 to the servo object
  pinMode(pushButtonPin,INPUT_PULLUP);
   Serial.println("Robojax Servo Button ");
   myservo.write(angle);//initial position
}

void loop() {
  if(digitalRead(pushButtonPin) == LOW){
    buttonPushed = 1;
  }
   if( buttonPushed ){
  // change the angle for next time through the loop:
  angle = angle + angleStep;

    // reverse the direction of the moving at the ends of the angle:
    if (angle >= maxAngle) {
      angleStep = -angleStep;
        if(type ==1)
        {
            buttonPushed =0;                   
        }
    }
    
    if (angle <= minAngle) {
      angleStep = -angleStep;
       if(type ==2)
        {
            buttonPushed =0;       
        }
    }
    
    myservo.write(angle); // move the servo to desired angle
      Serial.print("Moved to: ");
      Serial.print(angle);   // print the angle
      Serial.println(" degree");    
  delay(100); // waits for the servo to get there
   }
}

here is the same code but stripped down to the bare minimum:

#include <Servo.h>
Servo myservo;

int angle =90;
int angleStep =10;
const int minAngle = 0;
const int maxAngle = 180;
const int type =2;
int buttonPushed =0;

void setup() {
  myservo.attach(3);
  pinMode(2,INPUT_PULLUP);
   myservo.write(angle);
}

void loop() {
  if(digitalRead(2) == LOW){
    buttonPushed = 1;
  }
   if( buttonPushed ){

  angle = angle + angleStep;

//
    if (angle >= maxAngle) {
      angleStep = -angleStep;
        if(type ==1)
        {
            buttonPushed =0;                   
        }
    }
    
    if (angle <= minAngle) {
      angleStep = -angleStep;
       if(type ==2)
        {
            buttonPushed =0;       
        }
    }
    myservo.write(angle);
  delay(100);
   }
}

would it also be posibble to do something with the sweep code?

[dlloyd]

@jasper1234567890

i am using an arduino MEGA

Mega_Servo_Speed.ino

[J-M-L]

@jasper1234567890

but for moving back to the middle when i let go of the joystick it goes slow. it has like a 0,4s wait before moving back

that's indeed the impact of smoothing the curve. The more of the new value you take in the formula, the more the system is reactive. 90% of the new value + 10% of the old value for example will be much more reactive

You can download an manually install VarSpeedServo from here

go into code and download the zip

you unzip what you get (➜ VarSpeedServo-master) remove the -master from the directory name (so you get just VarSpeedServo) and move that directory next to the other custom library in the Arduino documents folder (on my Mac it's in /Users/username/Documents/Arduino/libraries)

you'll find examples on how to use that library directly bundled with it

[jasper1234567890]

i am on a chromebook using the web editor, and it doesn't work like that for me. from what i can find you can upload libraries in .zip format.
when i upload the .zip it gives this error:
[] parse library.properties: library.properties not found

i found out that this is due to the library not being updated anymore, so it is not possible to use it.

[jasper1234567890]

this looks like it would work for my code. thank you!
i will try to find a way to mix it into my code.