Revision 62:78d99b781f02, committed 2013-04-14

Comitter:

madcowswe

Date:

Sun Apr 14 12:57:04 2013 +0000

Parent:

61:a7782a35ce4f

Child:

63:c2c6269767b8

Commit message:

Basic fwd dc power feed forward working

Changed in this revision

--- a/Processes/MotorControl/MotorControl.cpp	Sun Apr 14 10:49:51 2013 +0000
+++ b/Processes/MotorControl/MotorControl.cpp	Sun Apr 14 12:57:04 2013 +0000
@@ -4,6 +4,7 @@
 #include "globals.h"
 #include <algorithm>
 #include "system.h"
+#include "supportfuncs.h"
 
 namespace MotorControl
 {
@@ -11,12 +12,16 @@
 volatile float fwdcmd = 0;
 volatile float omegacmd = 0;
 
+volatile float mfwdpowdbg = 0;
+volatile float mrotpowdbg = 0;
+
+MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
+
 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;
+    const float power_per_dc_m_per_s = 1.64f;
+    const float hysteresis_pwr = 0.16f;
 
     float testspeed = 0.2;
     float Fcrit = 1.75;
@@ -48,6 +53,7 @@
 
     float currtime = SystemTime.read();
     float dt = currtime - lastT;
+    dt = 0.05; //TODO: HACK!
     lastT = currtime;
 
     thetafiltstate = MOTORCONTROLLER_FILTER_K * thetafiltstate + (1-MOTORCONTROLLER_FILTER_K) * ((dright-dleft)/(dt*ENCODER_WHEELBASE));
@@ -61,7 +67,7 @@
     fwdIstate += errfwd;
     rotIstate += errtheta;
     
-    float actuatefwd = errfwd*Pgain + fwdIstate*Igain + power_per_dc_m_per_s*fwdcmd;
+    float actuatefwd = errfwd*Pgain + fwdIstate*Igain + max(power_per_dc_m_per_s*abs(fwdcmd), hysteresis_pwr)*sgn(fwdcmd);
     float actuaterot = errtheta*Pgain_rot + rotIstate*Igain_rot;
 
     float actuateleft = actuatefwd - (actuaterot*ENCODER_WHEELBASE/2.0f);
@@ -69,6 +75,9 @@
 
     mleft(max(min(actuateleft, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
     mright(max(min(actuateright, MOTOR_MAX_POWER), -MOTOR_MAX_POWER));
+    
+    mfwdpowdbg = fwdIstate*Igain;
+    mrotpowdbg = rotIstate*Igain_rot;
 
 }
 

--- a/Processes/MotorControl/MotorControl.h	Sun Apr 14 10:49:51 2013 +0000
+++ b/Processes/MotorControl/MotorControl.h	Sun Apr 14 12:57:04 2013 +0000
@@ -4,8 +4,11 @@
 
 namespace MotorControl{
     
-    extern float fwdcmd;
-    extern float omegacmd;
+    extern volatile float fwdcmd;
+    extern volatile float omegacmd;
+    
+    extern volatile float mfwdpowdbg;
+    extern volatile float mrotpowdbg;
     
     inline void set_fwdcmd(float infwdcmd){
         fwdcmd = infwdcmd;

--- a/Processes/Printing/Printing.h	Sun Apr 14 10:49:51 2013 +0000
+++ b/Processes/Printing/Printing.h	Sun Apr 14 12:57:04 2013 +0000
@@ -1,7 +1,7 @@
 
 // Eurobot13 Printing.h
 
-//#define PRINTINGOFF
+#define PRINTINGOFF
 
 #include "mbed.h"
 #include "rtos.h"

--- a/globals.h	Sun Apr 14 10:49:51 2013 +0000
+++ b/globals.h	Sun Apr 14 12:57:04 2013 +0000
@@ -8,7 +8,7 @@
 
 #define ENABLE_GLOBAL_ENCODERS
 
-const float ENCODER_M_PER_TICK = 1.0f/11980.0f;
+const float ENCODER_M_PER_TICK = 1.0f/1198.0f;
 const float ENCODER_WHEELBASE = 0.068;
 const float TURRET_FWD_PLACEMENT = -0.042;
 

--- a/main.cpp	Sun Apr 14 10:49:51 2013 +0000
+++ b/main.cpp	Sun Apr 14 12:57:04 2013 +0000
@@ -73,19 +73,34 @@
     Ticker motorcontroltestticker;
     motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
     // ai layer thread
-    Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
+    //Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
     // motion layer periodic callback
-    RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
-    motion_timer.start(50);
+    //RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
+    //motion_timer.start(50);
 
-    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+    //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
+    
+    MotorControl::set_omegacmd(0);
+    for(float spd = 0.02; spd <= 0.5; spd *= 1.4){
+    
+        MotorControl::set_fwdcmd(spd);
+        
+        Thread::wait(3000);
+        
+        float f = MotorControl::mfwdpowdbg;
+        float r = MotorControl::mrotpowdbg;
+        MotorControl::set_fwdcmd(0);
+        printf("targetspd:%f, fwd:%f, rot:%f\r\n", spd, f, r);
+        Thread::wait(5000);
+    }
+    
     Thread::wait(osWaitForever);
    
 }

--- a/supportfuncs.h	Sun Apr 14 10:49:51 2013 +0000
+++ b/supportfuncs.h	Sun Apr 14 12:57:04 2013 +0000
@@ -5,6 +5,10 @@
 #include "globals.h"
 #include "tvmet/Matrix.h"
 
+template <typename T> int sgn(T val) {
+    return (T(0) < val) - (val < T(0));
+}
+
 //Constrains agles to +/- pi
 inline float constrainAngle(float x){
     x = fmod(x + PI, 2*PI);