Diff: Controller.cpp

Revision:

3:27100cbaaa6e

Parent:

2:3f836b662104

Child:

4:75df35ef4fb6

--- a/Controller.cpp	Thu Mar 29 12:19:53 2018 +0000
+++ b/Controller.cpp	Thu Mar 29 14:42:59 2018 +0000
@@ -13,13 +13,16 @@
 using namespace std;
 
 const float Controller::PERIOD = 0.001f;                        // the period of the timer interrupt, given in [s]
-const float Controller::ALPHA = 0.785398163397448f;         // alpha = 45° in [rad]
+const float Controller::ALPHA = 0.785398163397448f;             // alpha = 45° in [rad]
 const float Controller::RB = 0.095f;                            // Ball Radius in [m]
 const float Controller::RW = 0.024f;                            // Wheel Radius in [m]
 const float Controller::PI = 3.141592653589793f;                // PI
 const float Controller::SQRT_3 = 1.732050807568877f;            // Square root of 3
 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;      // frequency of lowpass filter for actual speed values, given in [rad/s]
 const float Controller::COUNTS_PER_TURN = 1024.0f;
+const float Controller::KI = 58.5f;                             // torque constant [mNm/A]
+const float Controller::MIN_DUTY_CYCLE = 0.1f;                  // minimum allowed value for duty cycle (10%)
+const float Controller::MAX_DUTY_CYCLE = 0.9f;                  // maximum allowed value for duty cycle (90%)
 
 /**
  * Create and initialize a robot controller object.
@@ -269,13 +272,26 @@
             M3 = M3 + K[2][i]*x[i][1];
         };
 
-        // Calculate duty cycles from desired Torque
-        
+        // Calculate duty cycles from desired Torque, limit and set duty cycles
+        float I1 = M1*1000.0f/KI;
+        float I2 = M2*1000.0f/KI;
+        float I3 = M3*1000.0f/KI;
+                
+        float dutyCycle1 = 0.5f/6.0f*I1 + 0.5f;
+        if (dutyCycle1 < MIN_DUTY_CYCLE) dutyCycle1 = MIN_DUTY_CYCLE;
+        else if (dutyCycle1 > MAX_DUTY_CYCLE) dutyCycle1 = MAX_DUTY_CYCLE;
+        //pwm0.write(dutyCycle1);
         
-        //pwm1.write(dutyCycle1);
-        //pwm2.write(dutyCycle2);
-        //pwm3.write(dutyCycle3);
+        float dutyCycle2 = 0.5f/6.0f*I2 + 0.5f;
+        if (dutyCycle2 < MIN_DUTY_CYCLE) dutyCycle2 = MIN_DUTY_CYCLE;
+        else if (dutyCycle2 > MAX_DUTY_CYCLE) dutyCycle2 = MAX_DUTY_CYCLE;
+        //pwm1.write(dutyCycle2);
         
+        float dutyCycle3 = 0.5f/6.0f*I3 + 0.5f;
+        if (dutyCycle3 < MIN_DUTY_CYCLE) dutyCycle3 = MIN_DUTY_CYCLE;
+        else if (dutyCycle3 > MAX_DUTY_CYCLE) dutyCycle3 = MAX_DUTY_CYCLE;
+        //pwm2.write(dutyCycle3);
+                
         // actual robot pose
         this->x = phiY*RB;
         this->y = phiX*RB;