Ketan Keskar
/
Pick_and_place_robot
A program to control a pick and place robot with 2 driving motors and 3 servo motors
BT.cpp
- Committer:
- keskarketan
- Date:
- 2017-04-02
- Revision:
- 0:6593a6faef38
File content as of revision 0:6593a6faef38:
#include "mbed.h" #include "Servo.h" //Serial pc(USBTX, USBRX); Serial blue(PTC15, PTC14); // RX, TX DigitalOut fwdA(PTD2);//right - DigitalOut fwdB(PTD0);//right + DigitalOut fwdc(PTD1);//left - DigitalOut fwdd(PTD3);//left + Servo s1(D5); //pwm pin for bottom servo Servo s2(D6); //pwm pin for middle servo Servo s3(D7); //pwm pin for top servo float p1 = 0.5; //initial position for bottom servo float p2 = 0.5; //initial position for middle servo float p3 = 0.5; //initial position for top servo int state=0; //variable for switch-case int main() //main function { while(1) { if(blue.readable()>0) { // if data is available at serial port state = blue.getc(); // assign the character at serial port to 'state' } switch(state) { case 'w': //forward fwdA=1; fwdB=0; fwdc=1; fwdd=0; break; case 's': //reverse fwdA=0; fwdB=1; fwdc=0; fwdd=1; break; case 'b': //brake fwdA=0; fwdB=0; fwdc=0; fwdd=0; break; case 'a': // left turn fwdA=1; fwdB=0; fwdc=0; wait(1); fwdc = 1; fwdd=0; break; case 'd': //right turn fwdA=0; fwdB=0; fwdc=1; fwdd=0; break; //Servo 1 case '1': p1 = 0; wait(1); break; case '2': p1 = 0.5; wait(1); break; case '3': p1 = 1; wait(1); break; //Servo 2 case '4': p2 = p2 + 0.1; wait(1); break; case '5': p2 = 0.5; wait(1); break; case '6': p2 =p2 - 0.1; wait(1); break; //Servo 3 case '7': p3 = 0; wait(1); break; case '8': p3 = 0.5; wait(1); break; case '9': p3 = 1; wait(1); break; } s1 = p1; s2 = p2; s3= p3; } }