Diff: main.cpp

Revision:

4:70fea94b29ae

Parent:

3:47cd6b9e4dbb

Child:

5:70c86dbc8832

--- a/main.cpp	Mon Jul 20 22:17:55 2015 -0700
+++ b/main.cpp	Tue Jul 21 14:39:19 2015 -0700
@@ -1,25 +1,60 @@
 #include "robot.h"
 #include <sstream>
 
+int mode;
+const int MANUAL_MODE = 0;
+const int LINE_FOLLOW_MODE = 1;
+
 volatile double leftspeed;
 volatile double rightspeed;
 
 char received[80];
 int r_index;
+int sensors[5];
 
-int sensors[5];
+// PUT GLOBAL CONSTANTS HERE
+//////////////////////////////
+
+//////////////////////////////
 
 /*
-    parses a single command from the stream
-*/
+ * This function will be called at approximately 20 Hz when the control mode is LINE_FOLLOW_MODE
+ */
+int line_follow_loop(){
+    led4 = 1;
+
+    // set these variables to control the robot
+    leftspeed = 0.0;
+    rightspeed = 0.0;
+
+    // set mode to MANUAL_MODE when the end is detected
+    // mode = MANUAL_MODE
+
+    led4 = 0;
+}
+
+
+// INPUT COMMANDS
+// l:<float>    set left wheel speed (only effective in MANUAL_MODE)
+// r:<float>    set right wheel speed (only effective in MANUAL_MODE)
+// c:<int>      change mode
+
+// OUTPUT MESSAGES
+// p:<float>    line position
+// s:<int>,<int>,<int>,<int>,<int>
+//              light sensor values
+// m:<int>      mode
+
 int parse_command(const char* cmd)
 {
-    //xbee.printf("%s\n", cmd);
     if(cmd[1]==':'){
-        if(cmd[0]=='l'){
+        if(cmd[0]=='l' && mode==MANUAL_MODE){
             leftspeed = atof(&cmd[2]);
-        }else if(cmd[0]=='r'){
+        }else if(cmd[0]=='r' && mode==MANUAL_MODE){
             rightspeed = atof(&cmd[2]);
+        }else if(cmd[0]=='c'){
+            mode = atoi(&cmd[2]);
+        }
         }else{
             xbee.printf("%s\n",cmd); 
         }
@@ -41,14 +76,12 @@
             received[r_index]='\0'; // put a null character at the end
             parse_command(received);
             r_index=0;
-            //command.str("");
         } else {
             if(r_index >= 80){
                 r_index=0;
             }
             received[r_index]=read;
             r_index++;
-            //command << read;
         }
     }
     
@@ -68,10 +101,15 @@
     received[0] = '\0';
 
     while(1){
+
         led1 = 1;
         wait_ms(25);
+        if(mode==LINE_FOLLOW_MODE){
+            line_follow_loop()
+        }
         pi.left_motor(leftspeed);
         pi.right_motor(rightspeed);
+
         led1 = 0;
         wait_ms(25);
         pi.sensor_reading(sensors);
@@ -82,7 +120,9 @@
         __disable_irq();
         xbee.printf("p:%f\n", pi.line_position());
         __enable_irq();
-        // xbee.printf("%s\n",received)
+        __disable_irq();
+        xbee.printf("m:%d\n", mode);
+        __enable_irq();
     }
 
     return 0;