Smooth Multi Servo movement example with variying start, end speed.

I wrote a smooth sine wave transition for servos between any two positions with some added features.
Recent edit: fixed cross contamination delay problem

Features:
-Multiple servos can run simultaneously, each with different start speed, end speed, start position, end position and delay time to start values.
-the option of different start and end speeds allows the on-the-fly FOR loop stretch of the sine wave to simulate a more natural movement
-all position computation is done at the (1700 unit) resolution of the servo so there is basically little to no twitch.
-you can declare the desired movement positions of the servo in microseconds (position value)
-servo variables and movement function is modular so more servos can be added (I usually run a similar code with 6 servos simultaneous movement)
Limitations:
-all servo movement function is only 1 direction for each sine wave movement of servo, code waits for all servos to complete their singular motion before being able to go to next programmed move function -I am working on a continuous multi-count wave based on arrays… I will update latter.

The code is attached, a sample of the code is shown here (due to word limit on posting)

//############### small sample of larger code included here, does not include void loop() nor variable establishment.     
//start of primary move function that includes all servos and is called up and activated per each event
void movef(float ecycle,float w81,float spa1,float spb1,float yprev1,float ynext1,float w82,float spa2,float spb2,float yprev2,float ynext2)
{
         
         delay(1);   //master delay 
         count1 = 1;   //reset of count# 
         speedtick1 = 1;  //reset of speedtick#
         b1=(2*pie/(sinsize*2));  //coefficient of sine math function 
         
         count2 = 1; 
         speedtick2 = 1;    
         b2=(2*pie/(sinsize*2));     
          
            if(ynext1 > yprev1)  //sets sine wave coefficients depending on positions
            {
              a1= (ynext1-yprev1)/2;  //coefficient of sine math function 
              c1= (1.5)*pie;           //coefficient of sine math function 
              d1= yprev1+a1;            //coefficient of sine math function 
            }
            else  //(ynext# < yprev#)
            {
              a1= (yprev1-ynext1)/2;
              c1= (0.5)*pie;  
              d1= yprev1-a1; 
            }
              if(ynext2 > yprev2)
            {
              a2= (ynext2-yprev2)/2;  
              c2= (1.5)*pie; 
              d2= yprev2+a2; 
            }
            else  //(ynext# < yprev#)
            {
              a2= (yprev2-ynext2)/2;
              c2= (0.5)*pie;  
              d2= yprev2-a2; 
            }
            
              //###coefficient establishment for all sine waves end ###
            
            int per1=0; //reset of all trigger values in case there is carry over from a previous run
            int per2=0; 
           
              
       //global loop for all servos cycles by each servo and sees if it has an tiny move.                             
       for (float count = 0; count < ecycle; count +=1)  
       {  
         //traditional speed values start off as spa# and end up as spb# as count# ticks away on the fly as curve is being drawn. 
        // result is a sine curve that is compressed in the x axis on one end (spa#=large number) and stretched on other end (spb#=small number).   
         
         if (spa1 > spb1) {speed1=((count1+1)/sinsize)*(spa1-spb1) + spb1;} //start fast end slow 
         else {speed1= ((count1+1)/sinsize)*(spb1-spa1)+ spa1;} // start slow end fast 
         
         if (spa2 > spb2) {speed2=((count2+1)/sinsize)*(spa2-spb2) + spb2;} //start fast end slow 
         else  {speed2= ((count2+1)/sinsize)*(spb2-spa2)+ spa2;} // start slow end fast    
         
       
        
  // servo #1   3 states or cases 
        if (count < w81) //case 1 servo not ready to move yet      
            {
            servo1.writeMicroseconds(yprev1);  
            
            }
        
         else if (count>w81 && count1 > sinsize) //case 3 motion is done and position is held 
            {
            servo1.writeMicroseconds(ynext1); 
            
            per1=1; //declares this servo is finished with its movement 
            }
         
         else if (count > w81)   //case 2 sin wave function active with optional hold position while big loop asks other servos for their turn  
            {
              
              if (count1 < sinsize && speedtick1 == 1)  //new position of servo is written 
                {   
                   yvar1= a1*sin((count1)*b1+c1)+d1;  //the math function
                   servo1.writeMicroseconds(yvar1);   //throws a command at the servo 
                   speedtick1 += 1; // start of increment to count for possible pauses at this position to simulate slow 
                   count1 += 1; //increments sine wave operator x in y=f(x) 
                   
                }
                else if (speedtick1 > 1 && speedtick1 < speed1)  //sine wave is sustained at old value for 1 to speed# as counted by speedtick# 
                {
                  servo1.writeMicroseconds(yvar1); 
                  speedtick1 += 1;  //increments speedtick1 to delay the servo at one position along its travel to simulate a speed
                  
                }
                else //sine wave is now permitted to continue by having speedtick# reset 
                {
                  count1+=1; //lock out the sine function from going past sinsize by ever increasing count# 
                 speedtick1 = 1; //reset for next fly through of sine function 
                 
                }  
            }  //end if statement for case 2
 
       //servo #2   
          if (count < w82)     //notes same as servo #1 above 
            {
            servo2.writeMicroseconds(yprev2);  
           
            }
         
         else if (count>w82 && count2 > sinsize) 
            {
            servo2.writeMicroseconds(ynext2); 
            
            per2=1; 
            }
         
         else if (count > w82)    
            {
               
              if (count2 < sinsize && speedtick2 == 1)
                {   
                   yvar2= a2*sin((count2)*b2+c2)+d2;  
                   servo2.writeMicroseconds(yvar2);
                   speedtick2 += 1;   
                   count2 += 1; 
                 }
                else if (speedtick2 > 1 && speedtick2 < speed2)  
                {
                  servo2.writeMicroseconds(yvar2); 
                  speedtick2 += 1;
                  
                }
                else 
                {
                  count2+=1; 
                 speedtick2 = 1; 
                }  
            }    
         
       if(per1 == 1 && per2 == 1)//breaks FOR loop out of further un necessary cycles as all servos report their movement complete
         {
          break; 
         }
    
   }  //end of for loop statement for all servo steps  
 
} //end of void subroutine function for entire move function

SERVO_SHIMMY_V75.ino (10 KB)

Here is a video of it working: http://youtu.be/GPOCk5xg1B8

Nice video.

I am easing my way into servo integration at the moment, so this code is very timely.

Impressive! I was trying to program a sinewave movement for a 8 servo snake-bot. But I got stuck, when I got unexpected negative values for angle= sin (4).

BTW: your code could be faster, when you use pre-calculated values for "c1 or c2" instead of calculating " (x.5)*pie " every time :D

Just my two Cents Olaf

The code is intentionally bloated due to simplification actually adding difficulty until all the additions to the software are done. I still have to add array data entry, multi wave, and more lifelike twitching based on random values.

o_lampe: Impressive! I was trying to program a sinewave movement for a 8 servo snake-bot. But I got stuck, when I got unexpected negative values for angle= sin (4).

BTW: your code could be faster, when you use pre-calculated values for "c1 or c2" instead of calculating " (x.5)*pie " every time :D

Just my two Cents Olaf

Update on the code, I added multiple sine wave while still maintaining the start and end speed available difference among all waves. Improved the notes:

// Features:
// -multiple servos
// -sine wave function follow between any 2 positions among a servos range
// -no need to select if the servo is setup in reverse or go forward.
// -resolution of the servo is selectable so the increment movement of the signal equals the servo
// -changeable ramp up start speed, ramp down end speed (spa# and spb#)
// -time delay for each servo to start it’s movement (w8#)
// -multiple waves between positions (s#)

// Problems:
// -some cross contamination on a servo that is told to hold still, I saw a bit of 2 degree flex.
This may be that I am in a microsecond range of the servo outside of manufacturer’s recommendations which is true. ???

// Limitations:
// -cycle movement where all servos are phased together.
// -no future development items addressed.

// Future Development:
// -array based individual servo path yet still maintaining the single master loop of position/dwell/wait incrementing
// -optional pauses between cycles,
// -time based triggers to accomplish task sequences
// -spline stitching of positions

SERVO_SHIMMY_V83.ino (12.9 KB)

nice code !!

im an art student and i would need such code for 5 servos is it possible that u could write me this code for 5 servos

i tryed it on my own but i am not able to do so

it would be great if u could help me

best. dave