Controller.cpp

00001 /*
00002  * Controller.cpp
00003  * Copyright (c) 2020, ZHAW
00004  * All rights reserved.
00005  */
00006 
00007 #include "Controller.h"
00008 
00009 using namespace std;
00010 
00011 const float Controller::PERIOD = 0.001f;                    // period of 1 ms
00012 const float Controller::PI = 3.14159265f;                   // the constant PI
00013 const float Controller::WHEEL_DISTANCE = 0.185f;            // distance between wheels, given in [m]
00014 const float Controller::WHEEL_RADIUS = 0.038f;              // radius of wheels, given in [m]
00015 const float Controller::COUNTS_PER_TURN = 86016.0f;         // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
00016 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f;  // given in [rad/s]
00017 const float Controller::KN = 45.0f;                         // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
00018 const float Controller::KP = 0.1f;                          // speed control parameter
00019 const float Controller::MAX_VOLTAGE = 12.0f;                // battery voltage in [V]
00020 const float Controller::MIN_DUTY_CYCLE = 0.02f;             // minimum duty-cycle
00021 const float Controller::MAX_DUTY_CYCLE = 0.98f;             // maximum duty-cycle
00022 
00023 /**
00024  * Creates and initialises the robot controller.
00025  * @param pwmLeft a reference to the pwm output for the left motor.
00026  * @param pwmRight a reference to the pwm output for the right motor.
00027  * @param counterLeft a reference to the encoder counter of the left motor.
00028  * @param counterRight a reference to the encoder counter of the right motor.
00029  */
00030 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
00031     
00032     // initialise pwm outputs
00033     
00034     pwmLeft.period(0.00005f);  // pwm period of 50 us
00035     pwmLeft = 0.5f;            // duty-cycle of 50%
00036     
00037     pwmRight.period(0.00005f); // pwm period of 50 us
00038     pwmRight = 0.5f;           // duty-cycle of 50%
00039     
00040     // initialise local variables
00041     
00042     translationalMotion.setProfileVelocity(2.0f);
00043     translationalMotion.setProfileAcceleration(2.0f);
00044     translationalMotion.setProfileDeceleration(4.0f);
00045     
00046     rotationalMotion.setProfileVelocity(6.0f);
00047     rotationalMotion.setProfileAcceleration(12.0f);
00048     rotationalMotion.setProfileDeceleration(12.0f);
00049     
00050     translationalVelocity = 0.0f;
00051     rotationalVelocity = 0.0f;
00052     actualTranslationalVelocity = 0.0f;
00053     actualRotationalVelocity = 0.0f;
00054     
00055     previousValueCounterLeft = counterLeft.read();
00056     previousValueCounterRight = counterRight.read();
00057     
00058     speedLeftFilter.setPeriod(PERIOD);
00059     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00060     
00061     speedRightFilter.setPeriod(PERIOD);
00062     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00063     
00064     desiredSpeedLeft = 0.0f;
00065     desiredSpeedRight = 0.0f;
00066     
00067     actualSpeedLeft = 0.0f;
00068     actualSpeedRight = 0.0f;
00069     
00070     x = 0.0f;
00071     y = 0.0f;
00072     alpha = 0.0f;
00073     
00074     // start the periodic task
00075     
00076     ticker.attach(callback(this, &Controller::run), PERIOD);
00077 }
00078 
00079 /**
00080  * Deletes this Controller object.
00081  */
00082 Controller::~Controller() {
00083     
00084     ticker.detach(); // stop the periodic task
00085 }
00086 
00087 /**
00088  * Sets the desired translational velocity of the robot.
00089  * @param velocity the desired translational velocity, given in [m/s].
00090  */
00091 void Controller::setTranslationalVelocity(float velocity) {
00092     
00093     this->translationalVelocity = velocity;
00094 }
00095 
00096 /**
00097  * Sets the desired rotational velocity of the robot.
00098  * @param velocity the desired rotational velocity, given in [rad/s].
00099  */
00100 void Controller::setRotationalVelocity(float velocity) {
00101     
00102     this->rotationalVelocity = velocity;
00103 }
00104 
00105 /**
00106  * Gets the actual translational velocity of the robot.
00107  * @return the actual translational velocity, given in [m/s].
00108  */
00109 float Controller::getActualTranslationalVelocity() {
00110     
00111     return actualTranslationalVelocity;
00112 }
00113 
00114 /**
00115  * Gets the actual rotational velocity of the robot.
00116  * @return the actual rotational velocity, given in [rad/s].
00117  */
00118 float Controller::getActualRotationalVelocity() {
00119     
00120     return actualRotationalVelocity;
00121 }
00122 
00123 /**
00124  * Sets the desired speed of the left motor.
00125  * @param desiredSpeedLeft desired speed given in [rpm].
00126  */
00127 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
00128 
00129     this->desiredSpeedLeft = desiredSpeedLeft;
00130 }
00131 
00132 /**
00133  * Sets the desired speed of the right motor.
00134  * @param desiredSpeedRight desired speed given in [rpm].
00135  */
00136 void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
00137 
00138     this->desiredSpeedRight = desiredSpeedRight;
00139 }
00140 
00141 /**
00142  * Gets the actual speed of the left motor.
00143  * @return the actual speed given in [rpm].
00144  */
00145 float Controller::getActualSpeedLeft() {
00146 
00147     return actualSpeedLeft;
00148 }
00149 
00150 /**
00151  * Gets the actual speed of the right motor.
00152  * @return the actual speed given in [rpm].
00153  */
00154 float Controller::getActualSpeedRight() {
00155 
00156     return actualSpeedRight;
00157 }
00158 
00159 /**
00160  * Sets the actual x coordinate of the robots position.
00161  * @param x the x coordinate of the position, given in [m].
00162  */
00163 void Controller::setX(float x) {
00164     
00165     this->x = x;
00166 }
00167 
00168 /**
00169  * Gets the actual x coordinate of the robots position.
00170  * @return the x coordinate of the position, given in [m].
00171  */
00172 float Controller::getX() {
00173     
00174     return x;
00175 }
00176 
00177 /**
00178  * Sets the actual y coordinate of the robots position.
00179  * @param y the y coordinate of the position, given in [m].
00180  */
00181 void Controller::setY(float y) {
00182     
00183     this->y = y;
00184 }
00185 
00186 /**
00187  * Gets the actual y coordinate of the robots position.
00188  * @return the y coordinate of the position, given in [m].
00189  */
00190 float Controller::getY() {
00191     
00192     return y;
00193 }
00194 
00195 /**
00196  * Sets the actual orientation of the robot.
00197  * @param alpha the orientation, given in [rad].
00198  */
00199 void Controller::setAlpha(float alpha) {
00200     
00201     this->alpha = alpha;
00202 }
00203 
00204 /**
00205  * Gets the actual orientation of the robot.
00206  * @return the orientation, given in [rad].
00207  */
00208 float Controller::getAlpha() {
00209     
00210     return alpha;
00211 }
00212 
00213 /**
00214  * This is an internal method of the controller that is running periodically.
00215  */
00216 void Controller::run() {
00217     
00218     // calculate the planned velocities using the motion planner
00219     
00220     translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
00221     rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
00222     
00223     // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
00224     
00225     desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
00226     desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
00227     
00228     // calculate the actual speed of the motors in [rpm]
00229 
00230     short valueCounterLeft = counterLeft.read();
00231     short valueCounterRight = counterRight.read();
00232 
00233     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00234     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00235 
00236     previousValueCounterLeft = valueCounterLeft;
00237     previousValueCounterRight = valueCounterRight;
00238 
00239     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
00240     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
00241 
00242     // calculate desired motor voltages Uout
00243 
00244     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00245     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
00246 
00247     // calculate, limit and set the duty-cycle
00248 
00249     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00250     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00251     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00252     pwmLeft = dutyCycleLeft;
00253 
00254     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00255     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00256     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00257     pwmRight = dutyCycleRight;
00258     
00259     // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
00260     
00261     actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
00262     actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
00263     
00264     // calculate the actual robot pose
00265     
00266     float deltaTranslation = translationalMotion.velocity*PERIOD; // with a real robot: actualTranslationalVelocity*PERIOD
00267     float deltaOrientation = rotationalMotion.velocity*PERIOD;    // with a real robot: actualRotationalVelocity*PERIOD
00268     
00269     float sinAlpha = sin(alpha+deltaOrientation);
00270     float cosAlpha = cos(alpha+deltaOrientation);
00271     
00272     x += cosAlpha*deltaTranslation;
00273     y += sinAlpha*deltaTranslation;
00274     float alpha = this->alpha+deltaOrientation;
00275     
00276     while (alpha > PI) alpha -= 2.0f*PI;
00277     while (alpha < -PI) alpha += 2.0f*PI;
00278     
00279     this->alpha = alpha;
00280 }
00281