robot.c

#include <Servo.h>

int rotatePin = 2;
int verticalPin = 5;
int horizontalPin = 3;
int clawPin = 4;

bool initialized = false;
 
Servo servoRotate;  
Servo servoVertical;  
Servo servoHorizontal;
Servo servoClaw;

int HOME_ROT = 20;
int HOME_VERT = 124;
int HOME_HORIZ = 50;
int HOME_CLAW = 190;
  
void setup()
{
  Serial.begin(74880);  
}

void moveServo(Servo &servo, int from, int to, int delayTime) {
  int servoAngle;

  for(servoAngle = from; servoAngle < to; servoAngle++)
  {                                  
    servo.write(servoAngle);              
    delay(delayTime);  
    Serial.print(servoAngle);                
  }
}

void homeRobot() {
  servoRotate.write(HOME_ROT);
  servoVertical.write(HOME_VERT);
  servoHorizontal.write(HOME_HORIZ);
  servoClaw.write(HOME_CLAW);
}
 
void loop() {
  delay(3000);
  if (!initialized) {
    initialized = true;
    servoRotate.attach(rotatePin);
    servoVertical.attach(verticalPin);
    servoHorizontal.attach(horizontalPin);
    servoClaw.attach(clawPin);
    
    homeRobot();
//    Serial.print("Homed robot");

    delay(1000);
    
//    moveServo(servoRotate, HOME_ROT, 0, 300);
  }

//  homeRobot();
//  delay(1000);
//  move(servoRotate, HOME_ROT, 0, 50);
//  move(servoVertical, HOME_VERT, 0, 50);

//  if (Serial.available()){  //id data is available to read
//
//    int val = Serial.parseInt();
//
//    Serial.print(val);
//    servo.write(val);
//  }

//     servo.write(45);      // Turn SG90 servo Left to 45 degrees
//     delay(1000);
//     servo.write(135);
//
//control the servo's direction and the position of the motor
//
//   servo.write(45);      // Turn SG90 servo Left to 45 degrees
//   delay(1000);          // Wait 1 second
//   servo.write(90);      // Turn SG90 servo back to 90 degrees (center position)
//   delay(1000);          // Wait 1 second
//   servo.write(135);     // Turn SG90 servo Right to 135 degrees
//   delay(1000);          // Wait 1 second
//   servo.write(90);      // Turn SG90 servo back to 90 degrees (center position)
//   delay(1000);
//
////end control the servo's direction and the position of the motor
//
//
////control the servo's speed  
//
////if you change the delay value (from example change 50 to 10), the speed of the servo changes
//  for(servoAngle = 0; servoAngle < 180; servoAngle++)  //move the micro servo from 0 degrees to 180 degrees
//  {                                  
//    servo.write(servoAngle);              
//    delay(5);                  
//  }
//
//  for(servoAngle = 180; servoAngle > 0; servoAngle--)  //now move back the micro servo from 0 degrees to 180 degrees
//  {                                
//    servo.write(servoAngle);          
//    delay(10);      
//  }
  //end control the servo's speed  
}