Diff: main.cpp

Revision:

30:ea511cd81f43

Parent:

29:6aa49bba0d81

Child:

31:e36b7722df56

--- a/main.cpp	Wed Aug 05 19:42:25 2015 -0700
+++ b/main.cpp	Thu Aug 06 06:30:50 2015 +0000
@@ -2,29 +2,36 @@
 #include "my_functions.h"
 
 /*
- * This function will be called at approximately 50 hz when the control mode is LINE_FOLLOW_MODE
+ * This function will be called at approximately 20 hz when the control mode is LINE_FOLLOW_MODE
  */
 void line_follow_loop(){
     led4 = 1;
     
-    int sensors[5];
-    float position = 0.0;
-    static float previous_position = 0.0;
-    static float integral = 0.0;
-    
-    //get sensors
-    pi.sensor_reading(sensors);
-    
-    //line position
-    position = pi.line_position();
-    float speed = (position - previous_position)*50.0;
-    integral += 0.02*position;
+    static double previous_position = 0.0;
+    static double integral = 0.0;
+       
+    double derivative = (line_position - previous_position);
+    integral += line_position;
+    previous_position = line_position;
 
-    float delta = k_p*position + k_i*integral + k_d*speed;
+    double delta = k_p*line_position + k_i*integral + k_d*derivative;
 
     leftspeed = speed+delta;
     rightspeed = speed-delta;
     
+    if(leftspeed > MAX){
+        leftspeed = MAX;
+    }
+    if(rightspeed > MAX){
+        rightspeed = MAX;
+    }
+    if(leftspeed < MIN){
+        leftspeed = MIN;
+    }
+    if(rightspeed < MIN){
+        rightspeed = MIN;
+    }
+    
     // Do your control calculations here
     ////////////////////////////////////
     
@@ -58,14 +65,9 @@
 void communicate()
 {
     led1 = 1;
-    __disable_irq();
-    pi.sensor_reading(sensors);
-    __enable_irq();
     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]);
-    __disable_irq();
-    xbee.printf("p:%f\n", pi.line_position());
-    __enable_irq();
+    xbee.printf("p:%f\n", line_position);
     xbee.printf("m:%d\n", mode);
     // send any other variables here
     ////////////////////////////////
@@ -94,23 +96,27 @@
         // mode
         }else if(cmd[0]=='c'){
             mode = atoi(&cmd[2]);
+            __disable_irq();
             xbee.printf("a:c:%d\n", mode);// acknowledge the mode change
+            __enable_irq();
             // xbee.printf("mode set to %d\n", mode);
         // gains
         }else if(cmd[0]=='g'){
+            __disable_irq();
             if(cmd[2]=='p'){
                 k_p = atof(&cmd[4]);
-                xbee.printf("k_p: %f", k_p);
+                xbee.printf("k_p: %f\n", k_p);
             }else if(cmd[2]=='i'){
                 k_i = atof(&cmd[4]);
-                xbee.printf("k_i: %f", k_i);
+                xbee.printf("k_i: %f\n", k_i);
             }else if(cmd[2]=='d'){
                 k_d = atof(&cmd[4]);
-                xbee.printf("k_d: %f", k_d);
+                xbee.printf("k_d: %f\n", k_d);
             }else if(cmd[2]=='s'){
                 speed = atof(&cmd[4]);
-                xbee.printf("speed: %f", speed);
+                xbee.printf("speed: %f\n", speed);
             }
+            __enable_irq();
             // xbee.printf("gains p:%f, i:%f, d:%f\n", k_p, k_i, k_d);
         // battery
         }else if(cmd[0]=='b'){
@@ -122,9 +128,11 @@
 
         ///////////////////////////////
         }else{
+            //XXX disabling this is disabling error checking
             xbee.printf("%s\n",cmd);
         }
     }else{
+        //XXX disabling this is disabling error checking
         xbee.printf("%s\n",cmd);
     }
     return 0;
@@ -141,7 +149,7 @@
             parse_command(received);
             r_index=0;
         } else {
-            if(r_index >= 80){
+            if(r_index >= 1024){
                 r_index=0;
             }
             received[r_index]=read;
@@ -153,6 +161,8 @@
 
 void control()
 {
+    pi.sensor_reading(sensors);
+    line_position = pi.line_position();
     if(mode==LINE_FOLLOW_MODE){
         line_follow_loop();
     }