Controller.cpp

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