A program to control a pick and place robot with 2 driving motors and 3 servo motors

Dependencies:   Servo mbed

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;

    }
}