Diff: main.cpp

Revision:

22:6e3218cf75f8

Parent:

21:167dacfe0b14

Child:

23:5cfc4789e00b

--- a/main.cpp	Tue Apr 09 19:24:31 2013 +0000
+++ b/main.cpp	Tue Apr 09 20:41:22 2013 +0000
@@ -12,6 +12,7 @@
 #include <algorithm>
 
 pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+Timer SystemTime;
 
 void motortest();
 void encodertest();
@@ -57,6 +58,7 @@
     Thread::wait(osWaitForever);
     */
     
+    SystemTime.start();
     
     InitSerial();
     //while(1)
@@ -68,6 +70,7 @@
     
     Kalman::start_predict_ticker(&predictthread);
     //Thread::wait(osWaitForever);
+    
     //feedbacktest();
     
     Thread::wait(3500);
@@ -105,29 +108,6 @@
     }
 }
 
-
-void feedbacktest(){
-    //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
-    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
-    
-    Kalman::State state;
-    
-    float Pgain = -0.01;
-    float fwdspeed = -400/3.0f;
-    Timer timer;
-    timer.start();
-    
-    while(true){
-        float expecdist = fwdspeed * timer.read();
-        state = Kalman::getState();
-        float errleft = left_encoder.getTicks() - (expecdist);
-        float errright = right_encoder.getTicks() - expecdist;
-        
-        mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
-        mright(max(min(errright*Pgain, 0.4f), -0.4f));
-    }
-}
-
 void cakesensortest(){
     wait(1);
     printf("cakesensortest");