Servo Feedback Hack (free)

        This Instructable brought to you by the kind folks a Rachel's Electronics
        Visit  www.rachelselectronics.com  for cool electronics kits and breadouts!

This hobby servo hack will add shaft position feedback by tapping into the servo's own internal potentiometer. The only parts that need are wire and a little bit of solder, making this hack practically free. Common electronics workshop tools are required, so if you're minimally set-up and already have a servo, it should take you less than an hour to make the modification and run the test code.
I thought of this after building projects with servos and getting frustrated by the fact that after restart, the servo would zoom around if it wasn't already at the first coded position. There are code samples below (arduino) that map the servo's range to the feedback with minimal offset.
    As with any hack, you assume some risk of wrecking your servo motor, so use a cheap one!

Discussion topics include: Collision Detection. Gestural Keyframing. Software Increase of Torque. Haptic Control.... (uuh, fill in the blank folks!)

Parts and tools needed to complete this mod:
>  Arduino (or other microcontroller) and it's attendant parts.
>  Hobby servo (Featured: HITEC-322HD)
>  Stranded wire (Should be smaller than 20 gauge. I use 24g)
>  Set of small screwdrivers.
>  Wire snips
>  Wire strippers
>  Soldering iron and solder.
>  Multimeter.
>  A few resistors (1K, 4.7K, 3.3K, 2.2K, or similar)
>  Small knife

The Hitec servo comes apart very easily. Simply remove the four screws (bottom of servo, where wires come out). This also makes it possible to access the gear train (top of the servo, where the shaft comes out), so make sure that you keep the rest of the housing together. If you have a loose housing, put some tape over the seam to hold it together while you make this modification. Lift off the cover plate, and you will expose the bottom of the control PCB. We have to dig deeper to find out which one of these solder blobs is connected to the wiper pin of the internal potentiometer. Use a small flat screwdriver to pry up the PCB. start in one of the corners opposite from the cables. the PCB is soldered directly to the motor pins, and the whole unit should slide out smoothly. Careful not to let the gear housing open up! Now you can see that the yellow wire is the one that goes to the wiper, and also locate the solder blob that we will be attaching out sensor wire to.

Looking for the potentiometer wiper wire.
    On the HS-322 this is the little yellow wire that dives deep into the housing. Turn your servo all the way to one direction, and plug the servo cable into power and ground. Test voltage at the solder blob indicated in the previous step with your multimeter. Then turn the servo all the way in the other direction. I got 0.40V and 1.87V at each extreme. I hacked two Hitec servos for this instructable. The other one reads 0.41V and 1.86V. While you're probing around there with your meter, notice how close some of the surface mount parts are. In the next step, we will be soldering right next to them.

Soldering is fun and you will get better at it the more you do it!
    I have a Weller WP25. It's a workhorse. The dial got pulled off and eaten by the dog at some point, but I can still approximate the settings. I set the temperature just below the mid point for most of my re-work. (~550 degrees F). The tip I'm using is catalog # TP-7, if you're shopping. You can do this maneuver with a blunter iron, but please watch out for the surface-mount (SMT) parts adjacent.
    The trick to making this go smoothly is to pre-tin the end of wire that you will add to the circuit. First strip about 1/2" of insulation from the wire, twist up the strands, and then add solder. There is already a pretty big blob on the PCB, so we don't need much on the wire, but a good coating will help the connection flow. Next, trim the tinned end down to a length that fits to the solder blob in question.
    With the exposed bit wire already tinned it becomes a pretty straight-forward "heat-em-up". Hold the tinned and trimmed end of the wire over the solder blob. if it sticks out to far past, you may need to trim some more (see last photo this step). When you're satisfied with the length, place the wire on top of the solder blob, and put the iron on top of it. You're kind of sadwiching the wire end between the iron and the blob. Now is the time to cultivate patience. All you have to do is wait for the heat from the iron to melt the solder that's in the wire. That will transmit heat to the solder blob, melting it, and then the wire will mush into the blob. When that happens, remove the iron and keep holding the wire in place while it cools. You can blow on it gently if you like.

     In the unlikely and potentially disastrous event that you 'bridge' the solder blob and it's closest neighbor, I have some tips for pulling your servo back from the brink. First, let the parts cool. Then take a close look at the are. You should see clearly a patch of green PCB between the blob and the SMT part. That's a resistor, by the way and if you have a magnifying glass you can read the  three digit value (first two numbers are the value, third adds the zeros). Take a close look at the last picture below. Inside the yellow square, and just below the solder blob of interest, the PCB has some green patterns in it. The light colored green patches are copper traces (or fields, or pads) and the dark colored lines are non-conducting spaces between traces. The pad that the blob is on is isolated from the field around it, but connected to the left end of the SMT resistor. Can you see that? Good. If you've bridged the blob to the left end of the resistor only, then your good as done. There is already a connection there. The right side is a problem. You can try to wick or suck the solder away, and if there is alot, you may need to try, but be aware that you don't want to wick-up or suck-up the SMT part!  If there is just a little bridge, then you can cut it with your knife. The solder is mostly tin and some other stuff. Very easy to cut with a little steel blade. When you cut, don't expect to go straight through the middle of a blob. It's more like carving or whittleing. Care should be taken not to cut through a trace or other PCB connection. And watch out for those wires.
     If you can't see the resistor on the PCB, the next best place is stuck under surface tension to the tip of your soldering iron. Roasted.

Well, now that we've got the feedback wire attached, we need to get it out of the housing safely. Ideally, we would have an extra built-in strain relief. Sadly, no. But plastic is a very carve-able material so we will make an extra one. A picture tells a thousand words. Please look closely at the images to see how I'm routing the new wire. Your servo may not look like this, or you may have a different servo altogether. Test fit all the parts together before you cut to find the best position and identify any problem areas. Make little scratch marks or pen marks to orient yourself. Finally, move all the wires away from the area you are actively cutting. This is the final excruciating step and it would suck if you blew it.

Now you've got the thing held together, the new wire is tightly gripped, and where are those funny looking screws??? Your put them somewhere, didn't you?

    A 10 bit Analog to Digital  Conversion (ADC), which is standard on most microcontrollers, returns a value between 0 - 1023 on any analog signal. A 5V reference will yield ~ 5mV per step. Given our measured voltage difference of 1.47 (1.87 - 0.40) from the servo feedback wire, the ADC range would be ~294 steps. That's pretty good but we can do better. We can change the voltage that the arduino perceives as 'maximum' or 1023 as a way to increase the range on low voltage analog sensors, and we do this by building a voltage divider on the A-Ref pin.
    If we use a lower voltage as a reference for the ADC, we can increase the range that the ADC realizes. According to the latest arduino analogReference() notes [http://arduino.cc/en/Reference/AnalogReference], any voltage that is applied to the ARef pin is divided again by an internal 32K resistor. Their example uses a 2.5V input to the A-Ref, and the arduino sees 2.2V instead. With a reference high voltage of 2.2V, the conversion resolves to ~2.1mV per step, and the range of feedback from the hacked servo would be 700 steps. That's more like it. The call to analogReference(EXTERNAL) will tell the arduino to look at the A-Ref pin for measuring analog values. Happily enough, the call to analogReference(DEFAULT) will set the analog reference voltage back to an internal source (~5V or ~3.3V depending on your power supply), so you can move back and forth to read sensors that need the full 5V range to work ;)
     I played around with a few different values (all standard resistance. nothing exotic going on here) and tested with the arduino ADC to find that under a good power supply (9VDC 1.0A wall glob) I got the widest range by tying a 3.3K to GND and a 4.7K to +5. You may have different results. go ahead and play with the values. If you are off, the ADC will spit out 1023 for any reading above the value on A-Ref. Heck, I bet you could use a potentiometer and dial it in live.
     The trouble we're going through is worth it, because the signal that comes out of the internal potentiometer has a bunch of noise in it. The wider range will help to clear out the bad data. Set up your board similarly to the way I have it and run the code on the next page.

   On initial testing, feedback from the internal servo potentiometer works pretty damn good without too much software tweaking. The smoothing filter that I built in software takes 20 readings, discards the highest and lowest 6 values, and then averages the remaining 8. I focused on getting readings while the servo was still in order to test resolution and smoothing functionality. There should be enough code here for you to get yourself into trouble :). Notes below, code follows after in-line.

Code Notes: ServoWithFeedback_V1

     This is the first attempt (well, not the first. I wouldn't post that mess;) and it aims to take a one to one reading of servo feedback position, store all 180 points into an array, and then compare the array to new readings as the servo moves to them. Testing for smoothing functionality and sensor drift.



Code Notes: ServoWithFeedback_V2

    Here I'm simplifying the initial range acquisition. I take a high and low reading at either extreme of the range, and us map() to correlate it with degree angles. This works very well. I have set up a sweep that compares current sensor readings from the feedback wire to the expected value returned by map(). The offset is minimal (-1 to 1) on the rising loop. On the falling loop, the offset is more like -4 to -6. This is pretty consistent and could be worked around in software. I was able to apply some slight pressure to the servo horn toward and opposing the direction of travel, and got some nice values evidencing my applied force.
Here you go, have fun.

/*     >Servo With Feedback_V1<
 Testing code for servo with hacked feedback from internal pot.
 Servo control cable connected to digital 2
 Position Feedback cable connected to analog 0
 Build a voltage divider on ARef pin with two 10K resistors.
 The resulting analog reference will be ~2.2V (see http://arduino.cc/en/Reference/AnalogReference)
 This will increase the resolution of the potentiometer reading.
 Use the following commands to toggel between reading the servo feedback
   and reading any other analog pin that needs to see 5V as a reference
 analogReference(EXTERNAL); //sets analog 1023 to voltage on the ARef pin
 analogReference(DEFAULT);  //sets analog 1023 to 5V or 3.3V depending on power supply
 */
#include        //import servo library

Servo Servo1;                //declair the servo!
int reading[20];
int Feedback[181];
int servoPin1 = 2;
int test;               //general purpose int
int offset = 0;
int noise = 50;
int mean;
int result;

boolean done;


void setup() {
  Serial.begin(9600);                     // initialize serial output
  analogReference(EXTERNAL);
  pinMode (servoPin1,OUTPUT);
  Servo1.attach(servoPin1,570,2400);      // turn on servo control at digital pin 2
  setRange();                             // go test the range and set the values
}

void loop() {
   
  Servo1.write(0);
  delay(2000);
  for (int i=0; i<=180; i+=10){
    Servo1.write(i);
    delay (50);
    test = getFeedback();
    offset = test - Feedback[i];
    Serial.print(i);
    Serial.print(" = ");
    Serial.print(test);
    Serial.print("  ");
    Serial.print(offset);
    Serial.print("  ");
    Serial.println(Feedback[i]);
  }
 Serial.println(" ");
}



void setRange(){

  Servo1.write(0);                  // send servo to 0 degree position
  delay(2000);                      // give servo enough time to get there
  for (int i=0; i<=180; i++){
    Servo1.write(i);                // send next degree pulse to servo
    delay(50);                      // let things settle down
    Feedback[i] = getFeedback();    // read the servo feedback
    Serial.print(i);
    Serial.print(" = ");
    Serial.println(Feedback[i]);
  }
}


int getFeedback(){

   for (int j=0; j<20; j++){
      reading[j] = analogRead(0);    //get raw data from servo potentiometer
      delay(3);
    }                                // sort the readings low to high in array                                
    done = false;                    // clear sorting flag             
    while(done != true){             // simple swap sorts numbers from lowest to highest
    done = true;
    for (int j=0; j<20; j++){
      if (reading[j] > reading[j + 1]){     // sorting numbers here
        test = reading[j + 1];
        reading [j+1] = reading[j] ;
        reading[j] = test;
        done = false;
       }
     }
   }
//  for (int j=0; j<20; j++){        //un-comment this for-loop to see the raw ordered data
//      Serial.print(i);
//      Serial.print("  ");
//      Serial.println(reading[j]);
//  }
    mean = 0;
    for (int k=6; k<14; k++){        //discard the 6 highest and 6 lowest readings
      mean += reading[k];
    }
    result = mean/8;                  //average useful readings
    return (result);
}
// END OF SERVO_WITH_FEEDBACK_V1


/*     >Servo With Feedback_V2<
 Testing code for servo with hacked feedback from internal pot.
 Servo control cable connected to digital 2
 Position Feedback cable connected to analog 0
 Build a voltage divider on ARef pin with two 10K resistors.
 The resulting analog reference will be ~2.2V (see http://arduino.cc/en/Reference/AnalogReference)
 This will increase the resolution of the potentiometer reading.
 Use the following commands to toggel between reading the servo feedback
   and reading any other analog pin that needs to see 5V as a reference
 analogReference(EXTERNAL); //sets analog 1023 to voltage on the ARef pin
 analogReference(DEFAULT);  //sets analog 1023 to 5V or 3.3V depending on power supply
 */
#include        //import servo library

Servo Servo1;                //declair the servo!

int feedBack;           //used to hold servo feedback value
int mappedPulse;        //used to hold the value mapped between servo range and degree range
int lowEnd;             //servo feedback at 0 degrees
int highEnd;            //servo feedback at 180 degrees
int reading[20];
int servoPin1 = 2;
int test1;               //general purpose int
int test2;
int offset = 0;
int noise = 50;

boolean rangeTest= false;

void setup() {
 
    Serial.begin(9600);   // initialize serial output
    analogReference(EXTERNAL);
    pinMode (servoPin1,OUTPUT);
    Servo1.attach(servoPin1,570,2400);       // turn on servo control at digital pin 2   
    setRange();                            //go test the range and set the values
  }
 
void loop() {
    Servo1.write(0);
    delay(2000);                       // wait for it to get there
    for (int i=0; i<181; i++){         // loop through degrees going up
      Servo1.write(i);
      delay(50);
      feedBack = getFeedback();        // subroutine smooths data
      mappedPulse = map(i,0,180,lowEnd,highEnd);  // map degrees to setRange() readings
      offset = mappedPulse - feedBack;            // resolution of mapped V actual feedback
      printData();
  }
  for (int i=180; i>0; i--){            // loop through degrees going down
      Servo1.write(i);
      delay(50);
      feedBack = getFeedback();
      mappedPulse = map(i,0,180,lowEnd,highEnd);
      offset = mappedPulse - feedBack;
      printData();
  }
}

void printData(){
    Serial.print(i);
    Serial.print("  =  ");
    Serial.print(feedBack);
    Serial.print(" ");
    Serial.print(offset);
    Serial.print(" ");
    Serial.println(mappedPulse);
}

void setRange(){
  Servo1.write(0);
  delay(2000);              //wait for servo to get there 
  lowEnd = getFeedback();
  Servo1.write(180);
  delay(2000);              //wait for servo to get there  
  highEnd = getFeedback();
  rangeTest = true;
  Serial.print("0= ");
  Serial.print(lowEnd);
  Serial.print(" ");
  Serial.print("180= ");
  Serial.println(highEnd);
 
}
 
 
int getFeedback(){
    int mean;
    int result;
    int test;
    boolean done;
     
    for (int j=0; j<20; j++){
      reading[j] = analogRead(0);    //get raw data from servo potentiometer
      delay(3);
    }                                // sort the readings low to high in array                                
    done = false;              // clear sorting flag             
    while(done != true){       // simple swap sort, sorts numbers from lowest to highest
    done = true;
    for (int j=0; j<20; j++){
      if (reading[j] > reading[j + 1]){     // sorting numbers here
        test = reading[j + 1];
        reading [j+1] = reading[j] ;
        reading[j] = test;
        done = false;
       }
     }
   }
    mean = 0;
    for (int k=6; k<14; k++){        //discard the 6 highest and 6 lowest readings
      mean += reading[k];
    }
    result = mean/8;                  //average useful readings
    return(result);
}
// END SERVO_WITH_FEEDBACK_V2

I put together a more complete code sample based on V2 from step 7. It will automatically set the range of two arrayed servos, save the range data in EEPROM (feedback range and uS pulse range) for future restart. There is also an option to reset the saved range data on restart.
There's also a nice block that generates smooth servo acceleration and deceleration based on the cosine function. Gotta love trig.
Enjoy!




/* >Auto_Set_Range<
Testing code for servo with hacked feedback from internal pot.
Servo control cable connected to digital 10 and 11
Position Feedback cable connected to analog 0 and 1
Servos are declaired in an array for easy addition of more servos.
Build a voltage divider on ARef pin: +5V -- 4.7K -- Aref -- 3.3K -- GND
With a regulated voltage of 4.78V he resulting analog reference will be ~1.85V
(see http://arduino.cc/en/Reference/AnalogReference)
This will increase the resolution of the potentiometer reading.
Use the following commands to toggel between reading the servo feedback
and reading any other analog pin that needs to see 5V as a reference
analogReference(EXTERNAL); //sets analog 1023 to voltage on the ARef pin
analogReference(DEFAULT); //sets analog 1023 to 5V or 3.3V depending on power supply

THIS CODE COMES IN AT 8.9K IN MEMORY. WITHOUT ALL THE SERIAL FEEDBACK IT'S 8.3K
IT COULD STAND TO BE REDUCED BY SOMEONE SMARTER THAN ME, I'M SURE!

*/


#include <Servo.h> //import servo library
#include <EEPROM.h> //import EEPROM library
const int numServos = 2; //how many servos do you have?
const int sPin[numServos] = {10,11}; //what pins do they correlate to?
Servo servo[numServos]; //declare the servo array

int highPulse[numServos]; //high pulse width
int lowPulse[numServos]; //low pulse width
int A[numServos]; //lowPulse feedback reading
int B[numServos]; //highPulse feedback reading
int here[numServos]; //angle moving from
int there[numServos]; //angle moving to

float x[numServos]; //angle converted to radians to derive cosine wave

int h;
int t;
int feedBack; //used to hold servo feedback value
int Button = 2;
int e = 1; //EEPROM address to start storing/retrieving servo data
int angle; //angle derived from cosine function. sent to servo in loop
int btwReadings = 20; //delay time between
int whlReading = 3; //delay time between analog readings of internal pot

boolean rangeTest= false;
boolean doneMove[numServos];

void setup() {

Serial.begin(19200); // initialize serial output
Serial.println("it's on!");
analogReference(EXTERNAL);
pinMode (Button,INPUT);
for (int i=0; i<numServos; i++){
pinMode (sPin[i],OUTPUT);
}
int n = EEPROM.read(0);
if (n == 20){
callRange();
}
if (rangeTest == false){
for (int i=0; i<numServos; i++){
setRange(i); //go test the range and set the values
doneMove[i] = true;
}
rangeTest = true;
EEPROM.write(0,20); //indicate future startups that we've done this!
}
delay(1000);
}

void loop() {

for (int i=0; i<numServos; i++){
if(doneMove[i] == true){
doneMove[i] = false;
here[i] = there[i];
there[i] = random(180.1)+0.5;
if(there[i] == here[i]){there[i] = random(180.1)+0.5;}
if(here[i]<there[i]){x[i]=0;}else{x[i]=180;}
Serial.print("Move servo ");
Serial.print(i);
Serial.print(" from ");
Serial.print(here[i]);
Serial.print(" to ");
Serial.println(there[i]);
}
}

//calcCos(current position, desired position, step, servo array position)
for (int i=0; i<numServos; i++){
angle = calcCos(here[i],there[i],1.5,i);
if (doneMove[i] == false){
servo[i].write(angle);
delay(5);
}
}
}//   END VOID LOOP






/*
THIS FUNCTION AUTO-SETS THE SERVO RANGE
ASSUMES PUSHBUTTON ON PIN 2
*/



void setRange(int x){ //parameter passed is array position of servo
int pb = 0; //used to hold push button reading
int test; //general use variable
int h;
int t;
int pulse = 1500; //first uS pulse used in range test
Serial.print("Press button to set range of Servo[");
Serial.print(x);
Serial.println("].");
while(!pb){
pb = digitalRead(Button);
}
pb = 0;
Serial.print("Setting range limits in ..3");
for (int i=2; i>=0; i--){ //count down three seconds
delay(1000);
Serial.print("..");
Serial.print(i);
}
Serial.println();
servo[x].attach(sPin[x]);
delay(20);
servo[x].writeMicroseconds(pulse); //send servo to middle of range
delay(2000); //wait for it to get there

do{
pulse +=10; //incriment uS pulse width
readMove(x,pulse);
}while(h > t); //condition to keep testing range
highPulse[x] = pulse-20; //stay away from range extreme
B[x] = h-10; //adjust feedback away from extreme
Serial.println();
servo[x].writeMicroseconds(highPulse[x]);
pulse = highPulse[x];
delay(500);
do{
pulse -=10;
readMove(x,pulse);
}while(h < t);

lowPulse[x] = pulse+20;
A[x] = t+10;
servo[x].writeMicroseconds(lowPulse[x]);

feedBack = getFeedback(x); //take current reading from pot
there[x] = map(feedBack,A[x],B[x],0,180); //adjust feedback to degree output
servo[x].attach(sPin[x],lowPulse[x],highPulse[x]); //attach this servo
servo[x].write(there[x]); //send out pulse for where we are

doneMove[x] = true;


test = A[x] >> 8;
writeE(test); //store low feedback reading
writeE(A[x]);
test = B[x] >> 8;
writeE(test); // store high feedback reading
writeE(B[x]);
test = lowPulse[x] >> 8;
writeE(test); //store low control pulse
writeE(lowPulse[x]);
test = highPulse[x] >> 8;
writeE(test); //store high control pulse
writeE(highPulse[x]);

Serial.println("Feedback Range:");
Serial.print(A[x]);
Serial.print(" <> ");
Serial.println(B[x]);
Serial.println("uS Pulse Range:");
Serial.print(lowPulse[x]);
Serial.print(" <> ");
Serial.println(highPulse[x]);
Serial.print("Servo[");
Serial.print(x);
Serial.println("] attached, data saved in EEPROM");

}//end setRange()


void writeE(byte b){
EEPROM.write(e,b);
e +=1 ;
}

void readMove(int n, int p){
t = getFeedback(n);
servo[n].writeMicroseconds(p);
delay(btwReadings);
h = getFeedback(n);
Serial.println(h);
}     //END SET RANGE


/*
THIS FUNCTION READS THE INTERNAL SERVO POTENTIOMETER
*/
int getFeedback(int a){
int j;
int mean;
int result;
int test;
int reading[20];
boolean done;

for (j=0; j<20; j++){
reading[j] = analogRead(a); //get raw data from servo potentiometer
delay(whlReading);
} // sort the readings low to high in array
done = false; // clear sorting flag
while(done != true){ // simple swap sort, sorts numbers from lowest to highest
done = true;
for (j=0; j<20; j++){
if (reading[j] > reading[j + 1]){ // sorting numbers here
test = reading[j + 1];
reading [j+1] = reading[j] ;
reading[j] = test;
done = false;
}
}
}
mean = 0;
for (int k=6; k<14; k++){ //discard the 6 highest and 6 lowest readings
mean += reading[k];
}
result = mean/8; //average useful readings
return(result);
}    // END GET FEEDBACK



/*
THIS FUNCTION CALLS PREVIOUSLY SET RANGE FROM EEPROM
ASSUMES PUSH BUTTON ON PIN 2
*/
void callRange(){
int test;

Serial.print("To reset saved range press button on pin 2 ");
for (int i=5; i>=0; i--){
Serial.print("..");
Serial.print(i);
for (int j=0; j<100; j++){
if (digitalRead(Button) == 1){
Serial.println();
delay(1000);
return;
}
delay(10);
}
}
Serial.println();
Serial.println("Retreiving servo data");
for (int i=0; i<numServos; i++){
test = readE();
A[i] = test << 8; //get stored low feedback reading
A[i] = A[i] + readE();
test = readE();
B[i] = test << 8; //get stored high feedback reading
B[i] = B[i] + readE();
test = readE();
lowPulse[i] = test << 8; //get storeed low control pulse
lowPulse[i] = lowPulse[i] + readE();
test = readE();
highPulse[i] = test << 8; //get stored high control pulse
highPulse[i] = highPulse[i] + readE();
feedBack = getFeedback(i); //take current reading from pot
there[i] = map(feedBack,A[i],B[i],0,180); //adjust feedback to degree output
servo[i].attach(sPin[i],lowPulse[i],highPulse[i]); //attach this servo
servo[i].write(there[i]); //send out pulse for where we are
doneMove[i] = true; //set up to make first move
Serial.println("Feedback Range:");
Serial.print(A[i]);
Serial.print(" <> ");
Serial.println(B[i]);
Serial.println("uS Pulse Range:");
Serial.print(lowPulse[i]);
Serial.print(" <> ");
Serial.println(highPulse[i]);
Serial.print("Servo[");
Serial.print(i);
Serial.println("] attached, data retrieved from EEPROM");
Serial.print("servo ");
Serial.print(i);
Serial.print(" current position = ");
Serial.println(there[i]);
Serial.println();

}

rangeTest = true; //set the rangeTest flag
}//end callRange()


byte readE(){
byte E = EEPROM.read(e);
e +=1;
return E;
}     //END CALL RANGE



/*
THIS FUNCTION CREATES SMOOTH (COSINE) MOVEMENT FROM HERE TO THERE
*/

int calcCos(int h,int th,float s,int n){
int r;
int a;
if(h < th){
x[n] += s;
if (x[n] >= 181){doneMove[n] = true;}
r = (cos(radians(x[n]))*100);
a = map(r,100,-100,h,t);
}
if(h > th){
x[n] -= s;
if (x[n] <= -1){doneMove[n] = true;}
r = (cos(radians(x[n]))*100);
a = map(r,-100,100,h,t);
}
return a;
}     //END CALC COS