Revision 60:5058465904e0, committed 2013-04-13

Comitter:

madcowswe

Date:

Sat Apr 13 22:41:00 2013 +0000

Parent:

55:0c8897da6b3a

Child:

61:a7782a35ce4f

Commit message:

Added feed forward. Maybe positive feedback

Changed in this revision

--- a/Processes/MotorControl/MotorControl.cpp	Fri Apr 12 22:00:32 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Sat Apr 13 22:41:00 2013 +0000
@@ -8,13 +8,15 @@
 namespace MotorControl
 {
 
-float fwdcmd = 0;
-float omegacmd = 0;
+volatile float fwdcmd = 0;
+volatile float omegacmd = 0;
 
 void motor_control_isr()
 {
 
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+    
+    const float power_per_dc_m_per_s = 1.0f/0.6f;
 
     float testspeed = 0.2;
     float Fcrit = 1.75;
@@ -51,15 +53,15 @@
     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 - omegacmd;
+    float errfwd = fwdcmd - fwdfiltstate;
+    float errtheta = omegacmd - thetafiltstate;
     
     static float fwdIstate = 0;
     static float rotIstate = 0;
     fwdIstate += errfwd;
     rotIstate += errtheta;
     
-    float actuatefwd = errfwd*Pgain + fwdIstate*Igain;
+    float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*fwdcmd;
     float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
 
     float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);

--- a/globals.h	Fri Apr 12 22:00:32 2013 +0000
+++ b/globals.h	Sat Apr 13 22:41:00 2013 +0000
@@ -8,7 +8,7 @@
 
 #define ENABLE_GLOBAL_ENCODERS
 
-const float ENCODER_M_PER_TICK = 0.00084;
+const float ENCODER_M_PER_TICK = 1.0f/11980.0f;
 const float ENCODER_WHEELBASE = 0.068;
 const float TURRET_FWD_PLACEMENT = -0.042;
 
@@ -65,10 +65,10 @@
 const PinName P_COLOR_SENSOR_IN_UPPER = p20;
 const PinName P_COLOR_SENSOR_IN_LOWER = p19;
 
-const PinName P_MOT_LEFT_A     = p21;
-const PinName P_MOT_LEFT_B     = p22;
-const PinName P_MOT_RIGHT_A      = p23;
-const PinName P_MOT_RIGHT_B      = p24;
+const PinName P_MOT_LEFT_A     = p22;
+const PinName P_MOT_LEFT_B     = p21;
+const PinName P_MOT_RIGHT_A      = p24;
+const PinName P_MOT_RIGHT_B      = p23;
 
 const PinName P_ENC_RIGHT_A     = p26;
 const PinName P_ENC_RIGHT_B     = p25;

--- a/main.cpp	Fri Apr 12 22:00:32 2013 +0000
+++ b/main.cpp	Sat Apr 13 22:41:00 2013 +0000
@@ -61,7 +61,6 @@
     
     Serial pc(USBTX, USBRX);
     pc.baud(115200);
-    wait(2);
     InitSerial();
     wait(3);
     Kalman::KalmanInit();
@@ -81,8 +80,12 @@
     motion_timer.start(50);
 
     Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+    
+    //while(1){
+    //    printf("r:%f, l:%f, t:%f\r\n", right_encoder.getTicks()*ENCODER_M_PER_TICK, left_encoder.getTicks()*ENCODER_M_PER_TICK, SystemTime.read());
+    //}
 
-    measureCPUidle(); //repurpose thread for idle measurement
+    //measureCPUidle(); //repurpose thread for idle measurement
     Thread::wait(osWaitForever);
    
 }