Eliminating Servo "shaking"

Hi all,

I have a 3D printed robotic hand and am using 5 180 degree servos to control the five fingers. I am remotely controlling the hand using a glove equipped with 5 flex sensors. My program is simply taking the sensor value and turning it into a servo degree. My biggest problem is that I can’t figure out how to stabilize the reading of the sensor values and prevent the servos from constantly moving even though the sensor values are not changing. I have tried playing with the delay time but that does not work very well either. Any help would be much appreciated! Thank you!

#include <Servo.h>
Servo Thumb1;
Servo Index2;
Servo Middle3;
Servo Ring4;
Servo Pinky5;

int FlexSensor1 = A0;
int ServoDegree1 = 0;
int FlexValue1 = 0;

int FlexSensor2 = A1;
int ServoDegree2 = 0;
int FlexValue2 = 0;

int FlexSensor3 = A2;
int ServoDegree3 = 0;
int FlexValue3 = 0;

int FlexSensor4 = A3;
int ServoDegree4 = 0;
int FlexValue4 = 0;

int FlexSensor5 = A4;
int ServoDegree5 = 0;
int FlexValue5 = 0;

void setup()
{
Serial.begin(9600);
Thumb1.attach(5);
Index2.attach(10);
Middle3.attach(11);
Ring4.attach(6);
Pinky5.attach(9);

Thumb1.write(0);
Index2.write(0);
Middle3.write(0);
Ring4.write(0);
Pinky5.write(0);
}

void loop()
{

FlexValue1 = analogRead(FlexSensor1);
ServoDegree1 = (.72*(250 - FlexValue1));
Thumb1.write(ServoDegree1);
if (FlexValue1 == 270 <= 20) {
Thumb1.write(0);
}

FlexValue2 = analogRead(FlexSensor2);
ServoDegree2 = (.82*(250 - FlexValue2));
Index2.write(ServoDegree2);
if (FlexValue2 == 250 <= 20) {
Index2.write(0);
}

FlexValue3 = analogRead(FlexSensor3);
ServoDegree3 = (.72*(250 - FlexValue3));
Middle3.write(ServoDegree3);
if (FlexValue3 == 255 <= 50){
Middle3.write(0);
}

FlexValue4 = analogRead(FlexSensor4);
ServoDegree4 = (.72*(250 - FlexValue4));
Ring4.write(ServoDegree4);
if (FlexValue4 == 250 <= 20){
Ring4.write(0);
}

FlexValue5 = analogRead(FlexSensor5);
ServoDegree5 = (.72*(250 - FlexValue5));
Pinky5.write(ServoDegree5);

delay(1000);

}

  if (FlexValue1 == 270 <= 20) {

FlexValue1 will either equal 270, or it wont. Both true (they are equal) and false (they are not equal) are less than 20, so the body of this statement will always be executed.

What are you trying to do here?

And this won't work ServoDegree1 = (.72*(250 - FlexValue1)); You are mixing up integers and floats and a servo needs an integer value

Generally speaking when you read an analog value you should consider it unchanged unless the difference from the previous value exceeds some amount. For example

tempVal = analogRead(A0);
if (abs(tempVal - curVal) > margin) {
   curVal = tempVal;
}

...R

Also - you can apply averaging of the values from your analogRead().

Basically, gather up your reads, read each for so many iterations thru the loop() - say 10 iterations (use a counter variable), for each read add it to an accumulator (arrays help here), then when 10 iterations have passed, divide each accumulator (one per sensor - so your array would have 5 elements) by 10, then output that value (after whatever conversion you need to do) to your servo.

For instance:

#include <Servo.h>

int mySensorVals[] = {0, 0, 0, 0, 0}; // accumulators for each sensor
int mySensors[] = {A0, A1, A2, A3, A4}; // each sensor pin
int myServos[] = {Servo, Servo, Servo, Servo, Servo}; 

int counter = 0; // counter for averaging trigger
int max_count = 10;

void setup() {
  // attach each servo to a pin (not sure if these pin numbers are right...)
  for (int i = 2; i < 7; i++) {
    myServos[i - 2].attach(i);
  }
}

void loop() {
  // for each sensor, accumulate readings
  for (int i = 0; i < 5; i++) {
    mySensorVals[i] += analogRead(mySensors[i]);
  }

  counter++;

  if (counter > max_count - 1) {
    // divide each accumulator to average value
    for (int i = 0; i < 5; i++) {
      mySensorVals[i] = mySensorVals[i] / max_count; 

      // map the average sensor value (0 - 1023) to a servo position (0 - 180)
      // see - https://www.arduino.cc/en/reference/map
      int val = map(mySensorVals[i], 0, 0, 1023, 179);

      // output the caculated value to each servo
      myServos[i].write(val);

      // reset the value
      mySensorVals[i] = 0;
    }

    // wait for servos to get where they are going - note, commented out because it may not be
    // needed due to the time for looping and averaging, plus the analogReads, etc - but leaving
    // it here for completeness - but NOTE!! - this is BAD code!! But I don't have time to 
    // incorporate the blink-without-delay example stuff here... I leave it to you as a challenge.
    // delay(15);

    counter = 0;
  }
}

Note that the above code is not tested at all - I don’t even know if it compiles! - but it should serve as a starting point at least.

Well - hope that helps or at least provides you a jump-off point (anyone here want to correct my code above, test it, etc - feel free - I make no guarantees about it!).

:smiley:

PaulS:

  if (FlexValue1 == 270 <= 20) {

FlexValue1 will either equal 270, or it wont. Both true (they are equal) and false (they are not equal) are less than 20, so the body of this statement will always be executed.

Actually <= has higher precedence than ==, so this becomes

 if (FlexValue1 == 0) {

@madelinerd: Please edit your post, select the code, and put it between [code][/code] tags.

You can do that by hitting the “Code” icon above the posting area. It is the first icon, with the symbol: </>

Some servos have an adjustment screw. See http://learn.parallax.com/node/185. That has an example of adjusting it to stop the shaking.

Actually <= has higher precedence than ==, so this becomes

It’s still impossible to guess what OP was thinking.

Servo test code you can try with your servos. If they shake with this code, you may have servo power issues or hardware/design problems. If the servos are stable, then your problem is probably with your code or flex sensors.

//zoomkat 11-22-12 simple delimited ',' string parse 
//from serial port input (via serial monitor)
//and print result out serial port
//multi servos added 
// Powering a servo from the arduino usually *DOES NOT WORK*.

String readString;
#include <Servo.h> 
Servo myservoa, myservob, myservoc, myservod;  // create servo object to control a servo 

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

  //myservoa.writeMicroseconds(1500); //set initial servo position if desired

  myservoa.attach(6);  //the pin for the servoa control
  myservob.attach(7);  //the pin for the servob control
  myservoc.attach(8);  //the pin for the servoc control
  myservod.attach(9);  //the pin for the servod control 
  Serial.println("multi-servo-delimit-test-dual-input-11-22-12"); // so I can keep track of what is loaded
}

void loop() {

  //expect single strings like 700a, or 1500c, or 2000d,
  //or like 30c, or 90a, or 180d,
  //or combined like 30c,180b,70a,120d,

  if (Serial.available())  {
    char c = Serial.read();  //gets one byte from serial buffer
    if (c == ',') {
      if (readString.length() >1) {
        Serial.println(readString); //prints string to serial port out

        int n = readString.toInt();  //convert readString into a number

        // auto select appropriate value, copied from someone elses code.
        if(n >= 500)
        {
          Serial.print("writing Microseconds: ");
          Serial.println(n);
          if(readString.indexOf('a') >0) myservoa.writeMicroseconds(n);
          if(readString.indexOf('b') >0) myservob.writeMicroseconds(n);
          if(readString.indexOf('c') >0) myservoc.writeMicroseconds(n);
          if(readString.indexOf('d') >0) myservod.writeMicroseconds(n);
        }
        else
        {   
          Serial.print("writing Angle: ");
          Serial.println(n);
          if(readString.indexOf('a') >0) myservoa.write(n);
          if(readString.indexOf('b') >0) myservob.write(n);
          if(readString.indexOf('c') >0) myservoc.write(n);
          if(readString.indexOf('d') >0) myservod.write(n);
        }
         readString=""; //clears variable for new input
      }
    }  
    else {     
      readString += c; //makes the string readString
    }
  }
}

cr0sh:
Basically, gather up your reads, read each for so many iterations thru the loop() - say 10 iterations (use a counter variable), for each read add it to an accumulator (arrays help here), then when 10 iterations have passed, divide each accumulator (one per sensor - so your array would have 5 elements) by 10, then output that value (after whatever conversion you need to do) to your servo

Why would you do it that way ?
You’re dividing your sensing frequency by 10…
Use a moving window average - otherwise known as an ‘Infinite impulse response’ filter:

	//basic digital IIR LPF
	void LPF(const double& newSample, double& currentValue, const double& alpha){
		currentValue =  currentValue + alpha * (newSample - currentValue);
	}

Alpha is a value that will allow you to parameterise how much filtering you want… Use 0 < alpha ≤ 1
It works into this slightly more intuitive version if you expand it out then factor currentValue:

alpha * newSample - currentValue * (1 - alpha)

You dig?

(Or better yet, implement a second order Kalman filter…)

If your flex sensors are setup like voltage dividers/pots, then the below code might help trouble shoot your issues.

//zoomkat multi pot/servo test 3-23-13
//includes dead band for testing and limit servo hunting
//view output using the serial monitor

#include <Servo.h> 
Servo myservo1;  //declare servos
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;

int potpin1 = 0;  //analog input pin A0
int potpin2 = 1;
int potpin3 = 2;
int potpin4 = 3;
int potpin5 = 4;

int newval1, oldval1;  //pot input values
int newval2, oldval2;
int newval3, oldval3;
int newval4, oldval4;
int newval5, oldval5;

void setup() 
{
  Serial.begin(9600);  
  myservo1.attach(2);  
  myservo2.attach(3);
  myservo3.attach(4);
  myservo4.attach(5);
  myservo5.attach(6);
  Serial.println("testing multi pot servo");  
}

void loop()
{ 
  newval1 = analogRead(potpin1);           
  newval1 = map(newval1, 0, 1023, 0, 179); 
  if (newval1 < (oldval1-2) || newval1 > (oldval1+2)){ //dead band 
    myservo1.write(newval1); //position the servo
    Serial.print(newval1); //print the new value for testing 
    Serial.print("a,");
    oldval1=newval1; //set the current old value
  }

  newval2 = analogRead(potpin2);
  newval2 = map(newval2, 0, 1023, 0, 179);
  if (newval2 < (oldval2-2) || newval2 > (oldval2+2)){  
    myservo2.write(newval2);
    Serial.print(newval2);
    Serial.print("b,");
    oldval2=newval2;
  }

  newval3 = analogRead(potpin3);           
  newval3 = map(newval3, 0, 1023, 0, 179); 
  if (newval1 < (oldval1-2) || newval3 > (oldval3+2)){  
    myservo1.write(newval3);
    Serial.print(newval3);
    Serial.print("c,");
    oldval3=newval3;
  }

  newval4 = analogRead(potpin4);           
  newval4 = map(newval4, 0, 1023, 0, 179); 
  if (newval1 < (oldval1-2) || newval4 > (oldval4+2)){  
    myservo1.write(newval4);
    Serial.print(newval4);
    Serial.print("d,");
    oldval4=newval4;
  }

  newval5 = analogRead(potpin5);           
  newval5 = map(newval5, 0, 1023, 0, 179); 
  if (newval1 < (oldval5-2) || newval5 > (oldval5+2)){  
    myservo1.write(newval5);
    Serial.print(newval5);
    Serial.print("e,");
    oldval5=newval5;
  } 
  delay(50);  //to slow loop for testing, adjust as needed
}