Dependencies:   Motor Servo mbed

main.cpp

Committer:
probst
Date:
2015-02-27
Revision:
0:9e14f994576d

File content as of revision 0:9e14f994576d:

// define necessary includes
#include "mbed.h"
#include "Motor.h"
#include "Servo.h"
#define PULSE .0009
#define MAX 90

//define the two servos
Servo servoOne(p21);
Servo servoTwo(p22);

//define the motor
Motor m(p23,p24,p25);
Serial pc(USBTX, USBRX);


int main()
{
   //calibrate the two servos
   servoOne.calibrate(PULSE,MAX);
   servoTwo.calibrate(PULSE,MAX);
   
   //declare the initial motor speed
   double motorSpeed = .1;
   
   //set the tow servoes to their initial positions
   float position = 0;
   float positionTwo = 1;
    
    while(1)
    {
      //declare the initial positions
      servoOne = position;    //0
      servoTwo = positionTwo; //1
      
      //spin the servos in opposite directions forward
    while(position < 1 && positionTwo > 0)
      {
          if(motorSpeed != 1){motorSpeed = motorSpeed + .1;};
          m.speed(motorSpeed);
          position = position + .0001;
          positionTwo = positionTwo - .0001;
          servoOne = position;
          servoTwo = positionTwo;
          
          
          wait(.00025);
          
      }
      
      //spin the servos in opposite positions backwards
    while(position > 0 && positionTwo < 1)
    {
      m.speed(-motorSpeed);
      position = position - .0001;
      positionTwo = positionTwo + .0001;
      servoOne = position;
      servoTwo = positionTwo;
      wait(.00025);
      
      }
      
    
          
    }   

}