/*
 * Wires: Brown: Ground, Red: Power, Orange: Control Signal
 * Red to power (5V), Brown to ground, Orange to pin PWM Pin (such as 9)
 */


#include <Servo.h> 
//somehow include motor_drivesystem_functionstuff
int servoPin = 9; 
int angle;
Servo Servo1; //Declare servo object

void setup() {
  // We need to attach the servo to the used pin number 
   Servo1.attach(servoPin);

//Writing in setup to make it only run once.

delay(500);
//Starts at 0, which will be when the forklift is as high as can go.
   angle = 0;
   Servo1.write(angle); 
   delay(1000); 

//Located cradle
//Move forward enough to be close, but not too close that we hit the cradle as I lower the forklift.

delay(1000); //Pause for the demo.


//Lower forklift to approximate cradle pickup position. Height of base is 83 mm
for(angle = 0; angle <= 38; angle += 1)    // command to move from 0 degrees to 38 
//(may want to go a couple degrees lower in case it hits the top of the base)
  {                                  
    Servo1.write(angle);                 //command to rotate the servo to the specified angle
    delay(50);
  }
  delay(3000);

  
//Move the robot further up to be under the cradle. 

delay(5000); //Delay is to move up for demo.

//Lift cradle
for(angle = 38; angle >= 5; angle -= 1)    // command to move from 38 degrees to 5 (may want to go all the way to zero.)
  {                                  
    Servo1.write(angle);                 //command to rotate the servo to the specified angle
    delay(50);
  }
  delay(500);
  //Move around, get to baby drop off point.
delay(4000);

//Lower arm to drop baby off
for(angle = 5; angle <= 65; angle += 1)    // command to move from 5 degrees to 65
  {                                  
    Servo1.write(angle);                 //command to rotate the servo to the specified angle
    delay(50);
  }
  delay(500);

  //Back away from cradle
  delay(4000);

  //Bring arm back up.

for(angle = 65; angle >= 0; angle -= 5)    // command to move from 65 degrees to 0
  {                                  
    Servo1.write(angle);                 //command to rotate the servo to the specified angle
    delay(30);
  }
  delay(500);  

   //Move to other stuff in arena.
   
}

void loop() {
//May write in here later when I am not worried about repeats.

}