サーボモーターが４つ、スイッチが４つ、リセットスイッチが１つ。

 
//For Monma
//Servo*4 and Switch*4 and ResetSwitch*1
//2009.7.17

//Pin
//DIGITAL PIN (8,9,10,11)
int Servo_pin[4]={8,9,10,11};

//ANALOG IN PIN (0, 1, 2, 3)
int Switch_pin[4]={14,15,16,17};
//ANALOG IN PIN (5)
int ResetSwitch_pin = 19;

//ServoSpeed
int Speed_val[4]={50,50,50,50};



//Value
signed int Servo_val[4]={
  0,0,0,0};
int MaxCnt=2000;

//Init
void setup(){
  for(int i=0; i<4; i++){
    pinMode(Servo_pin[i],OUTPUT);
    pinMode(Switch_pin[i],INPUT);
    Servo_val[i]=90;
  }
  pinMode(ResetSwitch_pin,INPUT);
}

//Loop
void loop(){
  for(int i=0; i<1; i++){
    if(digitalRead(Switch_pin[i])){
      Servo_val[i]+=Speed_val[i];
      if(Servo_val[i]>MaxCnt)Servo_val[i]=MaxCnt;
    } 
    else{
      if(digitalRead(ResetSwitch_pin)==1)  Servo_val[i]=90;
    }
  }
  Control_Servo4();
}


void Control_Servo4(){
  //Control Servo 4
  for(int i=0; i<4; i++){
    digitalWrite(Servo_pin[i],HIGH);
  }
  for(int i=0; i<4; i++){
    delayMicroseconds(Servo_val[i]+500/4);
  }
  for(int i=0; i<4; i++){
    digitalWrite(Servo_pin[i],LOW);
  }
  for(int i=0; i<4; i++){
    delayMicroseconds((10000-(Servo_val[i]+500))/4);
  }
  delayMicroseconds(10000);
}