Diff: main.cpp

Revision:

5:f704940c9c7e

Parent:

4:112f3d35bd2d

Child:

6:61b503990cd6

--- a/main.cpp	Tue Dec 01 19:38:27 2015 +0000
+++ b/main.cpp	Wed Dec 02 01:53:48 2015 +0000
@@ -96,6 +96,7 @@
 const float irKi = 0; //constants
 volatile float leftD;
 volatile float rightD;
+
 void irPID()       
 {    
  eLS = 1;
@@ -148,7 +149,8 @@
 {
     STOPPED, 
     FORWARD, 
-    TURNING, 
+    TURNING,
+    FINISHED, 
     UNKNOWN
 } STATE;
 volatile STATE mouse_state;
@@ -157,7 +159,7 @@
 void offsetCalc();
 void stop();
 
-void reset()
+void encoder_reset()
 {
     l_enco.reset();
     r_enco.reset();
@@ -165,49 +167,51 @@
 
 void systick()
 {
-    watchOut();
+    //watchOut();
     if ( mouse_state == STOPPED )
     {
         offsetCalc();
         stop();
         return;
+    } else if (mouse_state == FORWARD) {
+        watchOut();
+        
+        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_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 = 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;
+        
+        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();
     }
-    
-    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_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 = 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;
-    
-    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
@@ -234,19 +238,102 @@
     
 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;
+    //if (mouse_state == FORWARD) {
+        eRF = 1;
+        float right = rRF.read();
+        eRF = 0;
+        eLF = 1;
+        float left = rLF.read();
+        eLF = 0;
+        if(left > frontWall || right > frontWall)
+        {
+            mouse_state = STOPPED;
+        }
+    //}
+}
+
+
+typedef enum 
+{
+    LEFT, 
+    RIGHT
+} TURN_DIRECTION;
+volatile TURN_DIRECTION turn_direction;
+
+volatile int turn_counter;
+volatile int forward_counter;
+volatile int right_turn_count;
+volatile int examined_left;
+volatile int examined_right;
+
+//time to make a 90 degree turn and move forward one cell length, repectively
+//should be replaced with encoder counts
+volatile int max_turn_count = 100;
+volatile int cell_length_count = 100;
+
+void turn() {
+    if (mouse_state == TURNING) {
+        if (turn_counter < max_turn_count) {
+            int left_pulses = l_enco.getPulses();
+            int right_pulses = r_enco.getPulses();
+            if (left_pulses < 0) left_pulses = -left_pulses;
+            if (right_pulses < 0) right_pulses = -right_pulses;
+            turn_counter = (left_pulses + right_pulses)/2;
+        } else {
+            if (turn_direction == LEFT) {
+                examined_left = 1;
+            } else {
+                if (right_turn_count > 1) {
+                    examined_right = 1;
+                } else {
+                    right_turn_count++;
+                    mouse_state = STOPPED;
+                    return;
+                }
+            }
+            encoder_reset();
+            mouse_state = FORWARD;
+        }
+    }
+}
+    
+void find_opening() {
+    if (mouse_state == STOPPED) {
+        encoder_reset();
+        if (!examined_left && !examined_right) {
+            turn_direction = LEFT;
+            left_forward = 0;
+            left_reverse = set_speed / left_max_speed;
+            right_forward = set_speed / right_max_speed;
+            right_reverse = 0;
+            mouse_state = TURNING;
+        } else if (examined_left && !examined_right) {
+            turn_direction = RIGHT;
+            left_forward = set_speed / left_max_speed;
+            left_reverse = 0;
+            right_forward = 0;
+            right_reverse = set_speed / right_max_speed;
+            mouse_state = TURNING;
+        } else if (!examined_left && examined_right) {
+            //this should never happen
+            mouse_state = FINISHED;
+        } else {
+            //we are done
+            mouse_state = FINISHED;
+        }
+    } else if (mouse_state == FORWARD) {
+        if (forward_counter > cell_length_count) {
+            examined_left = 0;
+            examined_right = 0;
+            right_turn_count = 0;
+            forward_counter = 0;
+        }
+        forward_counter = (l_enco.getPulses() + r_enco.getPulses())/2;
     }
 }
 
-
+Ticker turn_ticker;
+Ticker algorithm_ticker;
 
 void setup()
 {
@@ -275,6 +362,9 @@
     
     // repeat this for some time frame. 
     Systicker.attach_us(&systick, 1000);
+    //ir_ticker.attach_us(&irPID, 100000);
+    algorithm_ticker.attach_us(&find_opening, 1000);
+    turn_ticker.attach_us(&turn, 1000);
 }