Diff: main.cpp

Revision:

10:d2907773f9a3

Parent:

9:ad6f60953086

Child:

11:cde87eaf3f0f

--- a/main.cpp	Fri Dec 04 07:23:38 2015 +0000
+++ b/main.cpp	Fri Dec 04 09:38:25 2015 +0000
@@ -84,6 +84,10 @@
 const float wall_left = 0;
 const float wall_right = 0;
 
+volatile float lf_val;
+volatile float rf_val;
+volatile float ls_val;
+volatile float rs_val;
 
 volatile float enco_error;
 volatile float enco_pid;
@@ -142,54 +146,51 @@
 }
 
 //combining detect opening and watch out
-void Scan()
+bool forward_wall_detected()
+{
+    return lf_val > thresh_LF;
+}
+
+bool side_wall_detected()
+{
+    if (rs_val < open_right) 
+    {
+        mouse_state = MOVECELL;
+        turn_direction = RIGHT;
+        return true;
+    } 
+    else if (ls_val < open_left) 
+    {
+        mouse_state = MOVECELL;
+        turn_direction = LEFT;
+        return true;
+    }
+        
+    return false;
+}
+
+// updates PID values.
+void scan()
 {
     eRS = 1;
     wait_us(50);
-    float right_side = rRS.read();
+    rs_val = rRS.read();
     eRS = 0;
     
     eLS  = 1;
     wait_us(50);
-    float left_side = rLS.read();
+    ls_val = rLS.read();
     eLS = 0;
-    //eRF = 1;
-//        wait_us(50);
-//        float right_front = rRF.read();   //right front not working so screw it
-//        eRF = 0;
+    
     eLF = 1;
     wait_us(50);
-    float left_front = rLF.read();
-    eLF = 0;
-    
-    if ( left_front > thresh_LF )
-    {
-        mouse_state = STOPPED;
-    }
-    
-    /*if (right_side < open_right) {
-        mouse_state = MOVECELL;
-        opening_right_ahead = 1; 
-    } else if (left_side < open_left) {
-        mouse_state = MOVECELL;
-        opening_left_ahead = 1; 
-    }
-        
-        if(left_front > thresh_LF )
-        {
-            mouse_state = STOPPED; // does this actually do anything tho
-            wall_in_front = 1; //I don't think we're using this and it's not getting reset?
-            
-            encoder_reset();
-            mouse_state = TURNING;
-            turn_direction = LEFT;
-        } */
+    lf_val = rLF.read();
 }
 
 
 void systick()
 {
-    Scan(); // get the IR values.
+    scan(); // get the IR values.
             // these values are useful regardless of what mouse_state we are in.
     switch (mouse_state)
     {
@@ -198,6 +199,18 @@
             encoder_reset();
             break;
         case FORWARD:
+            if ( forward_wall_detected() )
+            {
+                mouse_state = STOPPED;
+                turn_direction = LEFT;
+                return;
+            }
+            
+//            if ( side_wall_detected() )
+//            {
+//                return;
+//            }
+            
             enco_error = l_enco.getPulses() - r_enco.getPulses();
             gyro_error = _gyro.read() - gyro_offset;
             
@@ -294,10 +307,9 @@
     }
 }
 
-
-
+// TODO move_cell is not complete. this is copied code from systick.
 void move_cell() {
-    //out of business moved to algorithm
+    //bacl in business boys.
     if (mouse_state == MOVECELL) 
     {
         forward_counter = 0.5 * (r_enco.getPulses() + l_enco.getPulses());
@@ -363,6 +375,45 @@
     //algorithm_ticker.attach_us(&algorithm, 1000);       let's try to stick with one ticker
 }
 
+int main() {
+    setup();
+    while(1) {
+        switch (mouse_state)
+        {
+        case STOPPED:
+            stop();
+            wait(1);
+            if ( turn_direction == INVALID )
+            {
+                mouse_state = FORWARD;
+            }
+            else
+            {
+                mouse_state = TURNING;
+            }
+            break;
+        case FORWARD:
+            left_forward = left_speed / left_max_speed;
+            left_reverse = 0;
+            right_forward = right_speed / right_max_speed;
+            right_reverse = 0;
+            break;
+        case TURNING:
+            turn();
+            mouse_state = STOPPED;
+            turn_direction = INVALID;
+            break;
+        case MOVECELL:
+            move_cell();
+            break;
+        default: 
+            // idk lmao. show some error message?
+            break;
+        }
+    }
+}
+
+
 void test_IR_sensors()
 {
     eRS = 1;
@@ -385,28 +436,15 @@
     wait(1);
 }
 
-int main() {
-    setup();
-    while(1) {
-        switch (mouse_state)
-        {
-            case STOPPED:
-                stop();
-                break;
-            case FORWARD:
-                left_forward = left_speed / left_max_speed;
-                left_reverse = 0;
-                right_forward = right_speed / right_max_speed;
-                right_reverse = 0;
-                break;
-            case TURNING:
-                turn();
-                break;
-            case MOVECELL:
-                break;
-            default: 
-                // idk lmao
-                break;
-        }
-    }
-}
+void test_motors()
+{
+    encoder_reset();
+    
+    right_forward = 1;
+    left_forward = 1;
+    right_reverse = 0;
+    left_reverse = 0;
+    
+    wait(10);
+    pc.printf("left pulses: %d, right pulses: %d", l_enco.getPulses(), r_enco.getPulses());
+}
\ No newline at end of file