Diff: main.cpp

Revision:

7:d25708589910

Parent:

6:da101b92bc2d

--- a/main.cpp	Tue Jul 14 14:22:51 2015 +0000
+++ b/main.cpp	Tue Jul 14 15:48:52 2015 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+
 // Data parser by Philip Goosen
  
  /*#define TX USBTX
@@ -6,9 +7,12 @@
  
 #define TX PA_2
 #define RX PA_3
+#define VERSION 1
  
 Serial pc(TX, RX); // tx, rx
 DigitalOut led(LED1);
+
+using namespace std;
  
  int isDigit(char digit)
 {
@@ -61,12 +65,13 @@
     wait(0.5);
     led=0;
     pc.baud(115200);
-    pc.printf("Philip's program");
+    pc.printf("Philip's program VERSION");
     while(1)
     {
         //pc.putc(pc.getc() + 1);
         if (pc.getc() == '$' && pc.getc() == '#')
         {
+                printf("\nRecieved input...\n");
                 /*led=1;
                 wait(1);
                 led=0;
@@ -78,13 +83,53 @@
                 
                     //int count = toDigit(c);
                     int x = 0;
+                    bool work = true;
                     
                     while (x < 3)
                     {
-                        pc.printf("%d ", x);
+                        char cur = pc.getc();
+                        char next = pc.getc();
+                        
+                        // Add alpha checks to these
+                        if (cur == 'M' && x==0 && isDigit(next))
+                        {
+                            motor = toDigit(next);
+                        }
+                        else if (cur == 'D' && x==1 && isDigit(next))
+                        {
+                            direction = toDigit(next);
+                        }
+                        else if (cur == 'S' && x==2 && isDigit(next))
+                        {
+                            char readSpeed[3] = {' ', ' ', '\0'};
+                            readSpeed[0] = next;
+                            //readSpeed[2] = '\0';
+                            cur = pc.getc();
+                            if ( isDigit(cur) )
+                            {
+                                readSpeed[1] = cur; 
+                            }
+                            else
+                            {
+                                readSpeed[1] = '\0';
+                            }
+                            speed = atoi(readSpeed);
+                        }
+                        else
+                        {
+                            work = false;
+                            printf("Invalid format");
+                        }
+                            
+                        
                         x++;
                     }
-                    //while loop with counter
+                    if (work)
+                    {
+                        //set values to servo library
+                        pc.printf("\nMotor %d Direction %d Speed %d", motor, speed, direction);
+                        
+                    }
         }
     }
 }
\ No newline at end of file