Diff: main.cpp

Revision:

29:6aa49bba0d81

Parent:

28:15b95329a064

Child:

30:ea511cd81f43

--- a/main.cpp	Thu Aug 06 02:16:53 2015 +0000
+++ b/main.cpp	Wed Aug 05 19:42:25 2015 -0700
@@ -10,17 +10,20 @@
     int sensors[5];
     float position = 0.0;
     static float previous_position = 0.0;
-    static float integral;
+    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;
+
+    float delta = k_p*position + k_i*integral + k_d*speed;
+
+    leftspeed = speed+delta;
+    rightspeed = speed-delta;
     
     // Do your control calculations here
     ////////////////////////////////////
@@ -42,8 +45,8 @@
 // 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
-// g:<p|i|d>:<float>
-//              change gains
+// g:<p|i|d|s>:<float>
+//              change gains/speed
 // b:           check battery
 
 // OUTPUT MESSAGES
@@ -64,6 +67,10 @@
     xbee.printf("p:%f\n", pi.line_position());
     __enable_irq();
     xbee.printf("m:%d\n", mode);
+    // send any other variables here
+    ////////////////////////////////
+    
+    ////////////////////////////////
     led1 = 0;
 }
 
@@ -93,18 +100,27 @@
         }else if(cmd[0]=='g'){
             if(cmd[2]=='p'){
                 k_p = atof(&cmd[4]);
+                xbee.printf("k_p: %f", k_p);
             }else if(cmd[2]=='i'){
                 k_i = atof(&cmd[4]);
+                xbee.printf("k_i: %f", k_i);
             }else if(cmd[2]=='d'){
                 k_d = atof(&cmd[4]);
+                xbee.printf("k_d: %f", k_d);
+            }else if(cmd[2]=='s'){
+                speed = atof(&cmd[4]);
+                xbee.printf("speed: %f", speed);
             }
-            xbee.printf("gains p:%f, i:%f, d:%f\n", k_p, k_i, k_d);
-  
+            // xbee.printf("gains p:%f, i:%f, d:%f\n", k_p, k_i, k_d);
         // battery
         }else if(cmd[0]=='b'){
             __disable_irq();
             xbee.printf("battery voltage: %f\n", pi.battery());
             __enable_irq();
+        // parse your own commands here
+        ///////////////////////////////
+
+        ///////////////////////////////
         }else{
             xbee.printf("%s\n",cmd);
         }
@@ -115,10 +131,10 @@
 }
 
 void check_incoming(){
-    led2 = 1;
     char read;
 
     while(xbee.readable()){
+        led2 = 1;
         read = xbee.getc();
         if(read=='\n'){
             received[r_index]='\0'; // put a null character at the end