Diff: main.cpp

Revision:

3:07a21d1d16e4

Parent:

2:aae3b2c0881c

Child:

6:9e9143e97bae

--- a/main.cpp	Thu Dec 17 18:24:42 2015 +0000
+++ b/main.cpp	Wed Apr 06 22:29:04 2016 +0000
@@ -1,103 +1,39 @@
 #include "mbed.h"
-#include "Servo.h"
-
-#define MAXVEL      100  // From 100 max
-#define SERINC      100   // Move micro seconds up/down
 
 
-DigitalOut myled(LED1);
+
+//AnalogOut r(LED_RED);
+//AnalogOut b(LED_BLUE);
 Serial pc(USBTX, USBRX);
 Serial bt(PTE0, PTE1);
-Servo SLF(D2);           // Servo Left Front
-Servo SLB(D3);           // Servo Left Back
-Servo SRF(D4);           // Servo Right Front
-Servo SRB(D5);           // Servo Right Back
-Servo SARM(D8);          // Servo Arm
-
-
-void decodeInstruction(char instruction);
-void setSpeed(int left, int right);
-void moveServo(int increment);
-void redLed(int status);
 
 char c;
 
-
-int main() {
-    bt.baud(115200);
+int main()
+{
+//    r=0;
+//    b=0;
+    bt.baud(9600);
     while(1) {
-        if (bt.readable()){
+        while(!bt.readable());
+        c = bt.getc();
+        if(c=='S') {
+            while(!bt.readable());
             c = bt.getc();
-            pc.printf("Received: %c\n\r", c);
-            decodeInstruction(c);
-            printf("%c\n", c);
+            if(c=='T') {
+                while(!bt.readable());
+                c = bt.getc();
+                pc.printf("Axis 1: %d\t\t", c);
+                //r=c/255.0;
+                while(!bt.readable());
+                c = bt.getc();
+                pc.printf("Axis 2: %d\n\r", c);
+                //b=c/255.0;
+
+            }
         }
-            
-    }
-}
 
 
-void decodeInstruction(char instruction){
-    switch (instruction){
-        case 'G':
-        case 'g':
-            setSpeed(MAXVEL, -MAXVEL);
-            break;
-        case 'A':
-        case 'a':
-            setSpeed(-MAXVEL, -MAXVEL);
-            break;
-        case 'S':
-        case 's':
-            setSpeed(-MAXVEL, MAXVEL);
-            break;
-        case 'D':
-        case 'd':
-            setSpeed(+MAXVEL, +MAXVEL);
-            break;
-        case 'Q':
-        case 'q':
-            setSpeed(0,0);
-            break;
-        case 'P':
-        case 'p':
-            moveServo(SERINC);
-            break;
-        case 'L':
-        case 'l':
-            moveServo(-SERINC);
-            break;
-        case 'W':
-        case 'w':
-            redLed(1);
-            break;
-        case 'H':
-        case 'h':
-            redLed(0);
-            break;
-    }        
-}
+    }
 
-void setSpeed(int left, int right){
-    float sLeft=0, sRight=0;
-    sLeft = (float)(left + 100)/200;
-    sRight = (float)(right + 100)/200;
-    SLF = sLeft;
-    SLB = sLeft;
-    SRF = sRight;
-    SRB = sRight;
-}
-
-void moveServo(int increment){
-    float temp = 0.0;
-    temp = (float)increment / 100.0;
-    if (temp < 0 && SARM >= abs(temp)){
-        SARM = SARM + temp;
-    }else if (temp > 0 && SARM <= 1 - abs(temp)){
-        SARM = SARM + temp;
-    }
-}
-
-void redLed(int status){
-    myled = status;
 }
\ No newline at end of file