File content as of revision 0:901a3ec83ba4:

/*******************************************************
              DC MODOR CONTROLLER LIBRARY
 *******************************************************           
         IMPORTANT: GO TO README FOR DOCUMENT!
********************************************************/

#include "mbed.h"
#include "LSCServo.h"
#include "DC_Motor_Controller.h"


DC_Motor_PID motor(D6,D3,D4,D5,792); //M1, M2, INA, INB, PPR 
DigitalIn but(D7);
LSCServo robot_arm(D1, D0); //D1 = Tx, D0 = Rx

int main() {
    
    motor.set_pid(0.008, 0, 0, 0.10);

    but.mode(PullUp);
    
    
    wait(0.5);motor.set_out(0.4,0);
    while(1)
    {
            if(but) { motor.set_out(0,0); break;}
    }
    
    wait(0.5);
    motor.reset();
    wait(0.5);
    motor.move_angle(615);
    wait(2);
    
    robot_arm.moveServos(3,1000,2,185,3,560,4,0); // initial pos to put an arrow
    wait(2);
    
    robot_arm.moveServos(3,1000,2,185,3,560,4,800); // catch/crawl the arror
    robot_arm.moveServos(3,1000,2,185,3,190,4,800); // rotate down
    wait(2);
    robot_arm.moveServos(3,1500,2,760,3,190,4,800); // rotate left
    wait(2);
    
    robot_arm.moveServos(3,1000,2,580,3,190,4,0); // open the crawler
    //robot_arm.moveServos(3,1000,2,600,3,190,4,0); // rotate a little back
    motor.move_angle(420);
    
    robot_arm.moveServos(3,1000,2,185,3,560,4,0); // reset to init
    
}