Diff: main.cpp

Revision:

1:3cacc3c50e68

Parent:

0:13d8a77fb1d7

--- a/main.cpp	Sat Nov 21 03:20:07 2015 +0000
+++ b/main.cpp	Sat Nov 21 05:32:35 2015 +0000
@@ -47,33 +47,53 @@
 volatile float line_accumulator = 0;
 volatile float line_decayFactor = 1;
 volatile float enco_accumulator = 0;
-volatile float enco_decayFactor = 1;
+volatile float enco_decayFactor = 1.2;
 volatile float gyro_accumulator = 0;
-volatile float gyro_decayFactor = 1;
+volatile float gyro_decayFactor = 1.2;
 
+volatile float set_speed = 0.5;
 volatile float left_speed = 0.5;
 volatile float right_speed = 0.5;
 
 const float left_max_speed = 6; // max speed is 6 encoder pulses per ms.
 const float right_max_speed = 6.2; 
 
-const float gyro_propo = 0.75;
+const float gyro_propo = 6.5;
 const float gyro_integ = 0;
-const float gyro_deriv = 1;
+const float gyro_deriv = 10;
 
-const float enco_propo = 0.05;
+const float enco_propo = .0005;
 const float enco_integ = 0;
-const float enco_deriv = 5;
+const float enco_deriv = .0002;
 
-const float spin_enco_weight = 0.5;
+const float spin_enco_weight = 0.75;
 const float spin_gyro_weight = 1 - spin_enco_weight;
 
+const float frontWall = 0.7; //need to calibrate this threshold to a value where mouse can stop in time
+//something like this may be useful 
+
 volatile float enco_error;
 volatile float enco_pid;
 volatile float gyro_error;
 volatile float gyro_pid;
 volatile float w_error;
 
+// this is just so that we can maintain what state our mouse is in. 
+// currently this has no real use, but it may in the future. 
+// or we could just remove this entirely. 
+typedef enum 
+{
+    STOPPED, 
+    FORWARD, 
+    TURNING, 
+    UNKNOWN
+} STATE;
+volatile STATE mouse_state;
+
+void watchOut();
+void offsetCalc();
+void stop();
+
 void reset()
 {
     l_enco.reset();
@@ -82,27 +102,49 @@
 
 void systick()
 {
+    watchOut();
+    if ( mouse_state == STOPPED )
+    {
+        offsetCalc();
+        stop();
+        return;
+    }
+    
     enco_error = l_enco.getPulses() - r_enco.getPulses();
     gyro_error = _gyro.read() - gyro_offset;
     
+    enco_accumulator += enco_error;
+    gyro_accumulator += gyro_error;
+    
     enco_pid = 0;
-    //enco_pid += enco_propo * enco_error;
-    //enco_pid += enco_deriv * (enco_error - enco_prevError);
+    enco_pid += enco_propo * enco_error;
+    enco_pid += enco_integ * enco_accumulator;
+    enco_pid += enco_deriv * (enco_error - enco_prevError);
     
     gyro_pid = 0;
     gyro_pid += gyro_propo * gyro_error;
+    gyro_pid += gyro_integ * gyro_accumulator;
     gyro_pid += gyro_deriv * (gyro_error - gyro_prevError);
     
     w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
-    left_speed += w_error;
-    right_speed -= w_error;
+    left_speed = set_speed + w_error;
+    right_speed = set_speed - w_error;
     
     left_forward = left_speed / left_max_speed;
     left_reverse = 0;
     right_forward = right_speed / right_max_speed;
     right_reverse = 0;
     
-    reset();
+    enco_prevError = enco_error;
+    gyro_prevError = gyro_error;
+    
+    enco_accumulator += enco_error;
+    gyro_accumulator += gyro_error;
+    
+    enco_accumulator /= enco_decayFactor;
+    gyro_accumulator /= gyro_decayFactor;
+    
+    //reset();
 }
 
 // computes gyro_offset
@@ -119,6 +161,29 @@
 }
 
 
+void stop()
+{
+    left_forward = 1;
+    left_reverse = 1;
+    right_forward = 1;
+    right_reverse = 1;
+}
+    
+void watchOut()
+{
+    eRF = 1;
+    float right = rRF.read();
+    eRF = 0;
+    eLF = 1;
+    float left = rLF.read();
+    eLF = 0;
+    if(left > frontWall || right > frontWall)
+    {
+        mouse_state = STOPPED;
+    }
+}
+
+
 void setup()
 {
     pc.printf("Hello World\r\n");
@@ -128,6 +193,8 @@
     eLS = 0;
     eLF = 0;
     
+    mouse_state = FORWARD;
+    
     myled = 1;
     
     for ( int i = 0; i < 1000; i++ )
@@ -151,7 +218,7 @@
     setup();
     while(1) {
         
-        pc.printf("enco_error: %f, gyro_error: %f, w_error: %f\r\n", enco_error, gyro_error, w_error);
+        pc.printf("enco_pid: %f, gyro_pid: %f, w_error: %f\r\n", enco_pid, gyro_pid, w_error);
         //wait(1);
     }
 }