Diff: main.cpp

Revision:

12:0849b16c2672

Parent:

11:cde87eaf3f0f

--- a/main.cpp	Fri Dec 04 22:53:53 2015 +0000
+++ b/main.cpp	Sat Dec 05 00:26:26 2015 +0000
@@ -48,7 +48,7 @@
 volatile float enco_accumulator = 0;
 volatile float enco_decayFactor = 1;
 volatile float gyro_accumulator = 0;
-volatile float gyro_decayFactor = 1;
+volatile float gyro_decayFactor = 1.1;
 
 volatile float set_speed = .75;
 volatile float left_speed = .75;
@@ -73,7 +73,7 @@
 const float base_RF = 0.002;
 const float base_LF = 0.15;
 
-const float thresh_LF = 0.25;
+const float thresh_LF = 0.3;
 const float thresh_RF = 0.0045; // TODO: CAUTION USING THIS ALONE. 
 
 const float open_left = 0.22;     //calibrate these two
@@ -109,7 +109,8 @@
 //should be replaced with encoder counts
 const int lmax_turn_count = 620; //I think we need different constants for left/right
 const int rmax_turn_count = 0;
-const int cell_length_count = 800;
+const int cell_length_count = 875;
+const float gyro_90 = 0.595;
     
 volatile int forward_counter = 0;
 
@@ -141,6 +142,9 @@
 {
     l_enco.reset();
     r_enco.reset();
+    
+    enco_accumulator = 0;
+    gyro_accumulator = 0;
 }
 
 //combining detect opening and watch out
@@ -269,41 +273,67 @@
 void turn() {
     // reopen for business. 
     
-    int turn_counter = 0;
-    int base_left_pulses = l_enco.getPulses();
-    int base_right_pulses = r_enco.getPulses();
-    
-    int left_pulses;
-    int right_pulses;
+    float angle_change = 0;
     
-    while ( turn_counter < lmax_turn_count )
+    switch(turn_direction)
     {
-        left_pulses = l_enco.getPulses();
-        right_pulses = r_enco.getPulses();
-        
-        turn_counter = ((left_pulses - base_left_pulses) + (right_pulses - base_right_pulses)) / 2;
-        
-        switch(turn_direction)
+    case LEFT:
+        while (angle_change < gyro_90)
+        {
+            left_forward = 0;
+            right_forward = set_speed / right_max_speed;
+            left_reverse = set_speed / left_max_speed;
+            right_reverse = 0;
+            
+            pc.printf("gyro read: %f\r\n", _gyro.read());
+            angle_change += _gyro.read() - gyro_offset;
+        }
+        break;
+    case RIGHT:
+        while (angle_change < gyro_90)
         {
-            case LEFT: 
-                left_forward = 0;
-                right_forward = set_speed / right_max_speed;
-                left_reverse = set_speed / left_max_speed;
-                right_reverse = 0;
-                break;
-            case RIGHT:
-                left_forward = set_speed / left_max_speed;
-                right_forward = 0;
-                left_reverse = 0;
-                right_reverse = set_speed / right_max_speed;
-                break;
-            default: 
-                // invalid state. did not set a turn direction.
-                mouse_state = STOPPED;
-                break;
+            left_forward = set_speed / left_max_speed;
+            right_forward = 0;
+            left_reverse = 0;
+            right_forward = set_speed / right_max_speed;
+            
+            pc.printf("gyro read: %f\r\n", _gyro.read());
+            angle_change += gyro_offset - _gyro.read();
         }
-        
+        break;
+    default:
+        // error state.
+        break;
     }
+    
+//    while ( turn_counter < lmax_turn_count )
+//    {
+//        left_pulses = l_enco.getPulses();
+//        right_pulses = r_enco.getPulses();
+//        
+//        turn_counter = ((left_pulses - base_left_pulses) + (right_pulses - base_right_pulses)) / 2;
+//        
+//        switch(turn_direction)
+//        {
+//            case LEFT: 
+//                left_forward = 0;
+//                right_forward = set_speed / right_max_speed;
+//                left_reverse = set_speed / left_max_speed;
+//                right_reverse = 0;
+//                break;
+//            case RIGHT:
+//                left_forward = set_speed / left_max_speed;
+//                right_forward = 0;
+//                left_reverse = 0;
+//                right_reverse = set_speed / right_max_speed;
+//                break;
+//            default: 
+//                // invalid state. did not set a turn direction.
+//                mouse_state = STOPPED;
+//                break;
+//        }
+//        
+//    }
 }
 
 // TODO move_cell is not complete. this is copied code from systick.
@@ -379,6 +409,7 @@
             break;
         case MOVECELL:
             move_cell();
+            mouse_state = STOPPED;
             break;
         default: 
             // idk lmao. show some error message?