Revision 23:6e3218cf75f8, committed 2013-04-09

Comitter:

madcowswe

Date:

Tue Apr 09 20:41:22 2013 +0000

Parent:

22:167dacfe0b14

Child:

24:5cfc4789e00b

Commit message:

MotorControl compiles

Changed in this revision

--- a/Processes/Kalman/Kalman.cpp	Tue Apr 09 19:24:31 2013 +0000
+++ b/Processes/Kalman/Kalman.cpp	Tue Apr 09 20:41:22 2013 +0000
@@ -28,7 +28,7 @@
 Mutex statelock;
 
 float RawReadings[maxmeasure+1];
-float IRpahseOffset;
+float IRphaseOffset;
 
 bool Kalman_inited = 0;
 
@@ -89,10 +89,10 @@
         fromb0offset += constrainAngle(IR_Offsets[i] - IR_Offsets[0]);
     }
 
-    IRpahseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
+    IRphaseOffset = constrainAngle(IR_Offsets[0] + fromb0offset/3);
 
     //debug
-    printf("Offsets IR: %0.4f\r\n",IRpahseOffset*180/PI);
+    printf("Offsets IR: %0.4f\r\n",IRphaseOffset*180/PI);
 
     statelock.lock();
     X(0,0) = x_coor;
@@ -224,7 +224,7 @@
     } else {
 
         if (type >= IR0 && type <= IR2)
-            RawReadings[type] = value - IRpahseOffset;
+            RawReadings[type] = value - IRphaseOffset;
         else
             RawReadings[type] = value;
 

--- a/Processes/Kalman/Kalman.h	Tue Apr 09 19:24:31 2013 +0000
+++ b/Processes/Kalman/Kalman.h	Tue Apr 09 20:41:22 2013 +0000
@@ -29,7 +29,7 @@
 void runupdate(measurement_t type, float value, float variance);
 
 extern float RawReadings[maxmeasure+1];
-extern float IRpahseOffset;
+extern float IRphaseOffset;
 
 extern bool Kalman_inited;
 

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Tue Apr 09 20:41:22 2013 +0000
@@ -0,0 +1,51 @@
+
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "globals.h"
+#include <algorithm>
+
+namespace MotorControl{
+
+float fwdcmd = 0;
+float thetacmd = 0;
+
+void motor_control_loop(){
+    MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+    
+    float Pgain = -0.01;
+    static float lastT = SystemTime;
+    static float lastright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
+    static float lastleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+    
+    while(true){
+        
+        static float thetafiltstate = 0;
+        static float fwdfiltstate = 0;
+        
+        float currright = right_encoder.getTicks() * ENCODER_M_PER_TICK;
+        float dright = currright - lastright;
+        lastright = currright;
+        
+        float currleft = left_encoder.getTicks() * ENCODER_M_PER_TICK;
+        float dleft = currleft - lastleft;
+        currleft = currleft;
+        
+        float currtime = SystemTime.read();
+        float dt = currtime - lastT;
+        lastT = currtime;
+        
+        thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
+        fwdfiltstate = MOTORCONTROLLER_FILTER_K * fwdfiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright+dleft)/(2.0f*dt));
+        
+        float errfwd = fwdfiltstate - fwdcmd;
+        float errtheta = thetafiltstate - thetacmd;
+        
+        float errleft = errfwd - (errtheta*ENCODER_WHEELBASE/2.0f);
+        float errright = errfwd + (errtheta*ENCODER_WHEELBASE/2.0f);
+        
+        mleft(max(min(errleft*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+        mright(max(min(errright*Pgain, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    }
+}
+
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Processes/MotorControl/MotorControl.h	Tue Apr 09 20:41:22 2013 +0000
@@ -0,0 +1,20 @@
+
+#ifndef MOTOR_CONTROL_H
+#define MOTOR_CONTROL_H
+
+namespace MotorControl{
+    
+    extern float fwdcmd;
+    extern float thetacmd;
+    
+    inline void set_fwdcmd(float infwdcmd){
+        fwdcmd = infwdcmd;
+    }
+    
+    inline void set_thetacmd(float inthetacmd){
+        thetacmd = inthetacmd;
+    }
+    
+}
+
+#endif //MOTOR_CONTROL_H
\ No newline at end of file

--- a/globals.h	Tue Apr 09 19:24:31 2013 +0000
+++ b/globals.h	Tue Apr 09 20:41:22 2013 +0000
@@ -18,6 +18,11 @@
 const float xyvarpertime = 0.0005; //(very poorly) accounts for hitting things
 const float angvarpertime = 0.001;
 
+extern Timer SystemTime;
+
+const float MOTORCONTROLLER_FILTER_K = 0.9;
+const float MOTOR_MAX_POWER = 0.4f;
+
 /*
 PINOUT Sensors
 5:  RF:SDI

--- 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");