Cool lill block of code to control my 4 servos,

Forum 2005-2010 (read only) General Exhibition
1

Made a Lill block (4650 Bytes) of code to control all the movments of my 4 servos

Heres a very short vid of them panning 180, Yes very boring...

This Second video is what I have made with 4 Servos and a Block of code, Made a lill Arm Worked alight

#include <Servo.h>//Servo Header
Servo Servo1,Servo2,Servo3,Servo4;//Declare Servos
Servo servo[]={Servo1,Servo1,Servo2,Servo3,Servo4};//Toss Into Array
int Pin[]={12,11,10,9};//Toss pins used into array
int CurrentPos[5];//Var for Current Postion

class ServoC{//Servo Class, Move, Update, Enable
public:
  void Move(int,int,int,int,int,int,int,int,char,char,char,char,int);
  int Update(int);
  void Enable();
};

int ServoC::Update(int ServoNum){//Gets servo Pos
  return(servo[ServoNum].read());  
}

void ServoC::Enable(){//Enable Servos
  for(int i=0;i<4;i++){
    servo[i+1].attach(Pin[i]);  
  }
}

void ServoC::Move(int ServoNumber1,int ServoNumber2,int ServoNumber3,int ServoNumber4,int Servo1Deg,int Servo2Deg,int Servo3Deg,int Servo4Deg, char MoveType1, char MoveType2, char MoveType3, char MoveType4, int Spe){
  int Max=0;//Max Value for the For loop
  int Fmax[]={Servo1Deg,Servo2Deg,Servo3Deg,Servo4Deg};//Array for servo Degres
  for(int i=0;i<4;i++){
    if(Fmax[i]>Max){//Finds largest number in the array
      Max=Fmax[i];//Saves it
    }
  }

  if(ServoNumber1!=0)CurrentPos[ServoNumber1]=Update(ServoNumber1);//If servo Number != 0 Update
  if(ServoNumber2!=0)CurrentPos[ServoNumber2]=Update(ServoNumber2);//If servo Number != 0 Update
  if(ServoNumber3!=0)CurrentPos[ServoNumber3]=Update(ServoNumber3);//If servo Number != 0 Update
  if(ServoNumber4!=0)CurrentPos[ServoNumber4]=Update(ServoNumber4);//If servo Number != 0 Update

// Movement types
// + <- Adds Value to current posstion
// - <- Subtracts Value to current posstion
// = <- Makes servo go to Value


  for(int i=0;i<Max;i++){//For loop for movment
    if((ServoNumber1!=0)&&(MoveType1=='+')&&(i<Servo1Deg)){
      servo[ServoNumber1].write(CurrentPos[ServoNumber1]+i);        
    }
    if((ServoNumber2!=0)&&(MoveType2=='+')&&(i<Servo2Deg)){
      servo[ServoNumber2].write(CurrentPos[ServoNumber2]+i);
    }
    if((ServoNumber3!=0)&&(MoveType3=='+')&&(i<Servo3Deg)){
      servo[ServoNumber3].write(CurrentPos[ServoNumber3]+i);
    }
    if((ServoNumber4!=0)&&(MoveType4=='+')&&(i<Servo4Deg)){
      servo[ServoNumber4].write(CurrentPos[ServoNumber4]+i);
    }
    if((ServoNumber1!=0)&&(MoveType1=='-')&&(Servo1Deg>i)){
      servo[ServoNumber1].write(CurrentPos[ServoNumber1]-i);
    }
    if((ServoNumber2!=0)&&(MoveType2=='-')&&(Servo2Deg>i)){
      servo[ServoNumber2].write(CurrentPos[ServoNumber2]-i);
    }
    if((ServoNumber3!=0)&&(MoveType3=='-')&&(Servo3Deg>i)){
      servo[ServoNumber3].write(CurrentPos[ServoNumber3]-i);
    }
    if((ServoNumber4!=0)&&(MoveType4=='-')&&(Servo4Deg>i)){
      servo[ServoNumber4].write(CurrentPos[ServoNumber4]-i);
    }
    delay(Spe);
  }


  if((ServoNumber1!=0)&&(MoveType1=='='))servo[ServoNumber1].write(Servo1Deg);
  if((ServoNumber2!=0)&&(MoveType2=='='))servo[ServoNumber2].write(Servo2Deg);
  if((ServoNumber3!=0)&&(MoveType3=='='))servo[ServoNumber3].write(Servo3Deg);
  if((ServoNumber4!=0)&&(MoveType4=='='))servo[ServoNumber4].write(Servo4Deg);
  delay(Spe);

}



void setup(){
  ServoC S;
  S.Enable(0);
  Servo1.write(180);
  Servo2.write(10);
  Servo3.write(80);
  Servo4.write(0);
  delay(1000);
}



void loop()
{
  ServoC S;
  int time=1000;
  S.Move(
  1//Servo 1 
  ,2//Servo 2
  ,3//Servo 3
  ,4//Servo 4
  ,180//Servo 1 Angle will be 180
  ,10//Servo 2 Angle will be 10
  ,80//Servo 1 Angle will be 18
  ,0//Servo 1 Angle will be 0
  ,'='//Servo 1 Angle will goto Angle
  ,'='//Servo 2 Angle will goto Angle
  ,'='//Servo 3 Angle will goto Angle
  ,'='//Servo 4 Angle will goto Angle
  ,10//Speed of servo per degree tick, +/- 50 degrees would take 500ms, Largest number is used for ammount of ticks
  );
  S.Move(0,0,3,4,180,10,80,50,'=','=','=','+',10);  
  S.Move(0,2,3,4,180,28,33,40,'=','+','+','-',10);
  S.Move(0,2,3,4,180,5,25,18,'-','+','+','+',10); 
  S.Move(0,2,3,4,180,1,1,35,'-','-','-','+',10);  
  S.Move(0,2,3,4,180,20,20,10,'-','+','-','+',10);  
  S.Move(0,2,3,4,180,40,20,30,'-','+','+','-',10);  
  S.Move(1,2,3,4,45,53,68,18,'-','-','-','-',20);  
  S.Move(1,2,3,4,45,53,69,18,'-','-','-','-',20);  
  S.Move(1,2,3,4,45,24,34,11,'-','+','+','+',20);  
  S.Move(1,2,3,4,45,53,68,21,'-','-','-','-',20);  
  S.Move(0,2,3,4,45,90,180,40,'-','=','=','+',5);  
}

Edit:
Added Coments
Added newer Code
Added 2nd Video

2

Cool machine ya got there. I like it.

What do you plan on doing with it (besides depositing old RAM off camera ;))?

3

Hehe, yea camera could have been in alot better place. And It went sort fast at times making motion blur, Crap camera only does 30 fps. But for 24$ I cant complain.

Well I eventuly building the next "version" out of brass box tubing. It will be alot lighter, stronger, lill smaller, And im sick of waiting for glue to dry. Also it will be mounted on wheels driven by 2 gear moters. And figure out what im gonna do with my other 2 servos, Probally just use it for my joints. Replaced one in the base with a much stronger one. The other was replaced in the 1st joint it dint have any troque.