#include <Servo.h> // For Servo motors
#include <Wire.h>
#include <Adafruit_MotorShield.h> // For the DC hobby motors
// The speed can also be varied at 0.5% increments

// A Servo has a potentiometer inside it that counts the number of
// degrees that the shaft moves in. It does not stop until it
// reaches the specified number, recieved form the PWM signal.

// The Servo does not send a signal back to the micro-controller
// (Closed-loop). Instead, the Servo is expected to make the turn
// before the next signal is recieved.
// Which is why a delay is necessary.

// The direction the Servo rotates in, depends on the length of
// this pulse. 1ms will move the shaft anticlockwise at -90°,
// 1.5ms will move the shaft to 0°, and a pulse of 2ms will move
// the shaft clockwise at +90°. Varying these pulse widths between
// the 1ms and 2ms mark, moves the shaft to a specific degree
// within this maximum range of 180°.

// As there is no limit to the length of the pulse,
// a Continuous Servo can rotate pass a full 360°.
// But the delay will need to be increased, since it cannot be
// less than the pulse width (With about 5ms to spare).

// These pulses are repeated every
// 20 milliseconds (ms) so the motor holds this position.

int servoNo1Speed;
int servoNo2Speed;
int servoNo3Speed;
int servoNo4Speed;
// Setting the Position or Speed
// The Parallax servo's are neutral at 94

int servoDelay=25; // 25 milliseconds
// The Servo needs to be given enough time to change its position

int servoNo1comPin=9;
int servoNo2comPin=10;
int servoNo3comPin=11;
int servoNo4comPin=6;
// The Communications or 'Signal' wires are attached to the PWM pins.

Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;
// The Servos are given names, making them into objects

Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *ServoV3 = AFMS.getMotor(1);
Adafruit_DCMotor *ServoV4 = AFMS.getMotor(2);

// Declare the Adafruit_MotorShield and the separate Power supply
// to each Servo a name. This also section also describes which
// M port the Voltage will come from. The (1); and (2); refers
// to the M1 and M2 screw-in block terminals.

// The other two servos are powered by the Motor Shield,
// but controled through the PWM pins on the Arduino.

// The empty pair of brackets () on Adafruit_MotorShield()
// is the default I2C address of the shield, which is 0x060
// If another MotorShield is stacked ontop, a separate declaration
// is needed and a 1 is placed between the brackets. This sets up
// a communication line between the Arduino and the new MotorShield object
// by referencing its unique I2C address of 0x061 (Manually soldered in).

void setup() {
 Serial.begin(9600);

 // Attaching the Servo's Signal wires to their respective pins
 pinMode(servoNo1comPin,OUTPUT);
 Servo1.attach(servoNo1comPin); // Arduino Header No.9

 pinMode(servoNo2comPin,OUTPUT);
 Servo2.attach(servoNo2comPin); // Arduino Header No.9

 pinMode(servoNo3comPin,OUTPUT);
 Servo3.attach(servoNo3comPin); // Arduino Header No.9

 pinMode(servoNo4comPin,OUTPUT);
 Servo4.attach(servoNo4comPin); // Arduino Header No.9

  AFMS.begin();
  // Similar to the Serial.begin() function, but for the Shield
  // The empty brackets have a default frequency of 1.6KHz
}

void loop() {

ServoV3->run(FORWARD);  // Turns on the motor
ServoV3->setSpeed(113); // 113 should be 6V
   // The setSpeed() parameter accepts a value between 0 and 255.
   // It controls the power provided to the DC motor using PWM.
   // while the function is called 'setSpeed', this only controls
   // the voltage to the Servo, which would normally set the speed
   // of a DC Hobby Motor.

servoNo1Speed=0;
servoNo2Speed=0;
servoNo3Speed=-20;
servoNo4Speed=0;

servoNo1Speed+=94;
servoNo2Speed+=94;
servoNo3Speed+=94;
servoNo4Speed+=94;
// Because Parallax servo's are neutral at 94, any number the user enters
// needs to less or more. So instead of requiring them to add or subtract
// from 94, the Arduino does it for them.

Servo3.write(servoNo3Pos);
delay(servoDelay);

}