Diff: main.cpp

Revision:

20:f0ca65974329

Parent:

19:c900c40b270e

Child:

21:18b585a44155

--- a/main.cpp	Wed Jul 22 22:00:05 2015 +0000
+++ b/main.cpp	Mon Jul 27 21:57:37 2015 +0000
@@ -1,23 +1,4 @@
 #include "robot.h"
-#include <sstream>
-
-int mode = 0;
-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];
-
-// GAINS
-//////////////////////////////
-float k_p = 0.0;
-float k_i = 0.0;
-float k_d = 0.0;
-//////////////////////////////
 
 /*
  * This function will be called at approximately <SPEED> when the control mode is LINE_FOLLOW_MODE
@@ -55,8 +36,8 @@
     power = (proportional * k_p) + (integral * k_i) + (derivative * k_d);
     
     //new speeds
-    rightspeed = speed + power;
-    leftspeed = speed - power;
+    rightspeed = speed - power;
+    leftspeed = speed + power;
     
    if(rightspeed < MIN)
         rightspeed = MIN;
@@ -121,6 +102,7 @@
         // mode
         }else if(cmd[0]=='c'){
             mode = atoi(&cmd[2]);
+            xbee.printf("mode set to %d\n", mode);
         // gains
         }else if(cmd[0]=='g'){
             if(cmd[2]=='p'){
@@ -166,6 +148,24 @@
     return;
 }
 
+void communicate()
+{
+    led1 = 1;
+    pi.sensor_reading(sensors);
+    int* s = sensors; // just to make the next line more compact
+    xbee.printf("s:%i,%i,%i,%i,%i\n", s[0], s[1], s[2], s[3], s[4]);
+    xbee.printf("p:%f\n", pi.line_position());
+    xbee.printf("m:%d\n", mode);
+    led1 = 0;
+}
+
+void control()
+{
+    if(mode==LINE_FOLLOW_MODE){
+        line_follow_loop();
+    }
+}
+
 int main() {
     
     xbee.attach(&Rx_interrupt, Serial::RxIrq);
@@ -173,17 +173,25 @@
     wait(2);
     xbeeReset = 1;
     pi.sensor_auto_calibrate();
+    leftspeed = 0.0;
+    rightspeed = 0.0;
+    
     r_index = 0;
     received[0] = '\0';
     mode = MANUAL_MODE;
+    
+    communication.attach(&communicate, 0.1);
+    controls.attach(&control, 0.05);
 
     while(1){
-
-        led1 = 1;
-        wait_ms(50);
+        led3 = mode;
+        //wait_ms(25);
+        
+        /*
         if(mode==LINE_FOLLOW_MODE){
             line_follow_loop();
         }
+        */
 
         // reversing these is more intuitive
         // pi.left_motor(leftspeed);
@@ -193,9 +201,11 @@
         pi.right_motor(leftspeed);
         __enable_irq();
 
+        /*
         led1 = 0;
-        wait_ms(50);
+        wait_ms(25);
         
+        /*
         pi.sensor_reading(sensors);
         int* s = sensors;
         __disable_irq();
@@ -207,6 +217,7 @@
         __disable_irq();
         xbee.printf("m:%d\n", mode);
         __enable_irq();
+        */
     }
 
     return 0;