Diff: Controller.cpp

Revision:

1:31b1c5cefd64

Parent:

0:e360940c4b88

--- a/Controller.cpp	Fri Mar 16 13:32:47 2018 +0000
+++ b/Controller.cpp	Fri Mar 16 14:40:52 2018 +0000
@@ -7,8 +7,10 @@
 #include <cmath>
 #include "Controller.h"
 
+
 using namespace std;
 
+
 const float Controller::PERIOD = 0.001f;                    // period of control task, given in [s]
 const float Controller::COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
@@ -17,7 +19,9 @@
 const float Controller::MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
 const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
 const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
-
+const float Controller::PI = 3.14159265f;                   // the constant PI
+const float Controller::WHEEL_DISTANCE = 0.170f;            // distance between wheels, given in [m]
+const float Controller::WHEEL_RADIUS = 0.0375f;             // radius of wheels, given in [m]
 /**
  * Creates and initializes a Controller object.
  * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
@@ -120,12 +124,15 @@
  */
 void Controller::run() {
     
-    // calculate the planned velocities using the motion planner
+        // calculate the planned velocities using the motion planner
     
-    
+    translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
+    rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
     
     // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
     
+    desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
+    desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
     
     
     // calculate actual speed of motors in [rpm]
@@ -160,8 +167,10 @@
     pwmRight.write(dutyCycleRight);
     
     // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
+
     
-    
+    actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
+    actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
     
 }