Diff: main.cpp

Revision:

5:f9837617817b

Parent:

4:453d534b1c1d

Child:

6:32d9b855b90f

--- a/main.cpp	Sat Nov 14 00:15:41 2015 +0000
+++ b/main.cpp	Sat Nov 14 01:12:47 2015 +0000
@@ -68,16 +68,16 @@
 // (we can change the names later, 
 // i added line in after i realized that i already had the angular code)
 const float line_propo = 1;
-const float line_integ = 1;
-const float line_deriv = 1;
+const float line_integ = 0;
+const float line_deriv = 0;
 
 const float gyro_propo = 1;
-const float gyro_integ = 1;
-const float gyro_deriv = 1;
+const float gyro_integ = 0;
+const float gyro_deriv = 0;
 
 const float enco_propo = 1;
-const float enco_integ = 1;
-const float enco_deriv = 1;
+const float enco_integ = 0;
+const float enco_deriv = 0;
 
 const float spin_enco_weight = 0.5;
 const float spin_gyro_weight = 1 - spin_enco_weight;
@@ -117,6 +117,7 @@
 // we do have to calibrate constants though. 
 void systick()
 {
+    pc.printf("systick ran\r\n");
     if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
     {
         // do nothing? 
@@ -125,6 +126,7 @@
         offsetCalc();
         return;
     }
+    pc.printf("systick ran while state is FORWARD \r\n");
     
     int dt = timer.read_us(); // should be around 1 ms.
     timer.reset();
@@ -159,6 +161,10 @@
         float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
         left_speed += x_error + w_error;
         right_speed += x_error - w_error;
+        
+        pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
+        
+        moveForward();
         // offsetCalc();
     }
     if ( mouse_state == TURNING )
@@ -174,11 +180,14 @@
     line_accumulator /= line_decayFactor;
     enco_accumulator /= enco_decayFactor;
     gyro_accumulator /= gyro_decayFactor;
+    
+    reset();
 }
 
 // setup stuff. 
 void setup()
 {
+    pc.printf("Hello World\r\n");
     mouse_state = STOPPED;
     
     eRS = 0;
@@ -189,14 +198,15 @@
     myled = 1;
     
     // repeat this for some time frame. 
-    offsetCalc();
+    for ( int i = 0; i < 200; i++ )
+        offsetCalc();
     Systicker.attach_us(&systick, 1000);
 }
 
 void reset()
 {
-    l_pulses = 0;
-    r_pulses = 0;
+    l_enco.reset();
+    r_enco.reset();
 }
 
 
@@ -218,12 +228,19 @@
 int main()
 {
     setup();
-    while(1)
-    {
-        pc.printf("The left motor is going at speed: %d\r\n", left_speed);  
-        pc.printf("The left motor is going at speed: %d\r\n", right_speed);
-        wait(1);
-    }
+    mouse_state = FORWARD;
+    
+    wait(1.5);
+    stop();
+    
+    pc.printf("DONE\r\n");
+    
+    //while(1)
+//    {
+//        pc.printf("The left motor is going at speed: %d\r\n", left_speed);  
+//        pc.printf("The left motor is going at speed: %d\r\n", right_speed);
+//        wait(1);
+//    }
 }
 
 
@@ -259,10 +276,29 @@
 {
     mouse_state = STOPPED;
     
+    
     motor1_forward = 1.0;
     motor1_reverse = 1.0;
     motor2_forward = 1.0;
     motor2_reverse = 1.0;
+    
+    
 }
 
-void turn(int dir); // maybe split this into two functions?
\ No newline at end of file
+void turn()// maybe split this into two functions?
+{
+    mouse_state = TURNING; 
+    float angle = 0;
+    while (angle < 0.9)
+    {
+        float gyro_val = _gyro.read() - gyro_offset;
+        angle += gyro_val;
+        
+        pc.printf("%f\r\n", angle);
+        
+        motor1_forward = 0.5;
+        motor1_reverse = 0;
+        motor2_forward = 0;
+        motor2_reverse = 0.5;
+    }    
+}
\ No newline at end of file