Diff: BT.cpp

Revision:

0:6593a6faef38

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/BT.cpp	Sun Apr 02 17:43:54 2017 +0000
@@ -0,0 +1,124 @@
+#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;
+
+    }
+}