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 const float Controller::SIGMA_TRANSLATION = 0.0001;         // standard deviation of estimated translation per period, given in [m]
00024 const float Controller::SIGMA_ORIENTATION = 0.0002;         // standard deviation of estimated orientation per period, given in [rad]
00025 const float Controller::SIGMA_DISTANCE = 0.01;              // standard deviation of distance measurement, given in [m]
00026 const float Controller::SIGMA_GAMMA = 0.03;                 // standard deviation of angle measurement, given in [rad]
00027 
00028 /**
00029  * Creates and initializes a Controller object.
00030  * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
00031  * @param pwmRight a pwm output object to set the duty cycle for the right motor.
00032  * @param counterLeft an encoder counter object to read the position of the left motor.
00033  * @param counterRight an encoder counter object to read the position of the right motor.
00034  */
00035 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
00036     
00037     // initialize periphery drivers
00038     
00039     pwmLeft.period(0.00005f);
00040     pwmLeft.write(0.5f);
00041     
00042     pwmRight.period(0.00005f);
00043     pwmRight.write(0.5f);
00044     
00045     // initialize local variables
00046     
00047     translationalMotion.setProfileVelocity(1.5f);
00048     translationalMotion.setProfileAcceleration(1.5f);
00049     translationalMotion.setProfileDeceleration(3.0f);
00050     
00051     rotationalMotion.setProfileVelocity(3.0f);
00052     rotationalMotion.setProfileAcceleration(15.0f);
00053     rotationalMotion.setProfileDeceleration(15.0f);
00054     
00055     translationalVelocity = 0.0f;
00056     rotationalVelocity = 0.0f;
00057     actualTranslationalVelocity = 0.0f;
00058     actualRotationalVelocity = 0.0f;
00059     
00060     previousValueCounterLeft = counterLeft.read();
00061     previousValueCounterRight = counterRight.read();
00062     
00063     speedLeftFilter.setPeriod(PERIOD);
00064     speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00065     
00066     speedRightFilter.setPeriod(PERIOD);
00067     speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
00068     
00069     desiredSpeedLeft = 0.0f;
00070     desiredSpeedRight = 0.0f;
00071     
00072     actualSpeedLeft = 0.0f;
00073     actualSpeedRight = 0.0f;
00074     
00075     x = 0.0f;
00076     y = 0.0f;
00077     alpha = 0.0f;
00078     
00079     p[0][0] = 0.001f;
00080     p[0][1] = 0.0f;
00081     p[0][2] = 0.0f;
00082     p[1][0] = 0.0f;
00083     p[1][1] = 0.001f;
00084     p[1][2] = 0.0f;
00085     p[2][0] = 0.0f;
00086     p[2][1] = 0.0f;
00087     p[2][2] = 0.001f;
00088     
00089     // start periodic task
00090     
00091     ticker.attach(callback(this, &Controller::run), PERIOD);
00092 }
00093 
00094 /**
00095  * Deletes the Controller object and releases all allocated resources.
00096  */
00097 Controller::~Controller() {
00098     
00099     ticker.detach();
00100 }
00101 
00102 /**
00103  * Sets the desired translational velocity of the robot.
00104  * @param velocity the desired translational velocity, given in [m/s].
00105  */
00106 void Controller::setTranslationalVelocity(float velocity) {
00107     
00108     this->translationalVelocity = velocity;
00109 }
00110 
00111 /**
00112  * Sets the desired rotational velocity of the robot.
00113  * @param velocity the desired rotational velocity, given in [rad/s].
00114  */
00115 void Controller::setRotationalVelocity(float velocity) {
00116     
00117     this->rotationalVelocity = velocity;
00118 }
00119 
00120 /**
00121  * Gets the actual translational velocity of the robot.
00122  * @return the actual translational velocity, given in [m/s].
00123  */
00124 float Controller::getActualTranslationalVelocity() {
00125     
00126     return actualTranslationalVelocity;
00127 }
00128 
00129 /**
00130  * Gets the actual rotational velocity of the robot.
00131  * @return the actual rotational velocity, given in [rad/s].
00132  */
00133 float Controller::getActualRotationalVelocity() {
00134     
00135     return actualRotationalVelocity;
00136 }
00137 
00138 /**
00139  * Sets the actual x coordinate of the robots position.
00140  * @param x the x coordinate of the position, given in [m].
00141  */
00142 void Controller::setX(float x) {
00143     
00144     this->x = x;
00145 }
00146 
00147 /**
00148  * Gets the actual x coordinate of the robots position.
00149  * @return the x coordinate of the position, given in [m].
00150  */
00151 float Controller::getX() {
00152     
00153     return x;
00154 }
00155 
00156 /**
00157  * Sets the actual y coordinate of the robots position.
00158  * @param y the y coordinate of the position, given in [m].
00159  */
00160 void Controller::setY(float y) {
00161     
00162     this->y = y;
00163 }
00164 
00165 /**
00166  * Gets the actual y coordinate of the robots position.
00167  * @return the y coordinate of the position, given in [m].
00168  */
00169 float Controller::getY() {
00170     
00171     return y;
00172 }
00173 
00174 /**
00175  * Sets the actual orientation of the robot.
00176  * @param alpha the orientation, given in [rad].
00177  */
00178 void Controller::setAlpha(float alpha) {
00179     
00180     this->alpha = alpha;
00181 }
00182 
00183 /**
00184  * Gets the actual orientation of the robot.
00185  * @return the orientation, given in [rad].
00186  */
00187 float Controller::getAlpha() {
00188     
00189     return alpha;
00190 }
00191 
00192 /**
00193  * Correct the pose with given actual and measured coordinates of a beacon.
00194  * @param xActual the actual x coordinate of the beacon, given in [m].
00195  * @param yActual the actual y coordinate of the beacon, given in [m].
00196  * @param xMeasured the x coordinate of the beacon measured with a sensor(i.e. a laser scanner), given in [m].
00197  * @param yMeasured the y coordinate of the beacon measured with a sensor(i.e. a laser scanner), given in [m].
00198  */
00199 void Controller::correctPoseWithBeacon(float xActual, float yActual, float xMeasured, float yMeasured) {
00200     
00201     // create copies of current state and covariance matrix for Kalman filter P
00202     
00203     float x = this->x;
00204     float y = this->y;
00205     float alpha = this->alpha;
00206     
00207     float p[3][3];
00208     
00209     for (int i = 0; i < 3; i++) {
00210         for (int j = 0; j < 3; j++) {
00211             p[i][j] = this->p[i][j];
00212         }
00213     }
00214     
00215     // calculate covariance matrix of innovation S
00216     
00217     float s[2][2];
00218     float r = sqrt((xActual-x)*(xActual-x)+(yActual-y)*(yActual-y));
00219     
00220     s[0][0] = 1.0f/r/r*(p[1][0]*xActual*yActual+p[1][1]*yActual*yActual+r*r*SIGMA_DISTANCE*SIGMA_DISTANCE+p[0][0]*(xActual-x)*(xActual-x)-p[1][0]*yActual*x+p[0][1]*(xActual-x)*(yActual-y)-p[1][0]*xActual*y-2.0f*p[1][1]*yActual*y+p[1][0]*x*y+p[1][1]*y*y);
00221     s[0][1] = -(1.0f/r/r/r*(-p[1][1]*xActual*yActual+p[1][0]*yActual*yActual-p[0][2]*xActual*r*r-p[1][2]*yActual*r*r-p[0][1]*(xActual-x)*(xActual-x)+p[1][1]*yActual*x+p[0][2]*r*r*x+p[0][0]*(xActual-x)*(yActual-y)+p[1][1]*xActual*y-2.0f*p[1][0]*yActual*y+p[1][2]*r*r*y-p[1][1]*x*y+p[1][0]*y*y));
00222     s[1][0] = ((xActual-x)*(p[2][0]*r*r+p[1][0]*(xActual-x)+p[0][0]*(-yActual+y))+(yActual-y)*(p[2][1]*r*r+p[1][1]*(xActual-x)+p[0][1]*(-yActual+y)))/r/r/r;
00223     s[1][1] = p[2][2]+SIGMA_GAMMA*SIGMA_GAMMA+p[1][2]*(xActual-x)/r/r+p[0][2]*(-yActual+y)/r/r-(yActual-y)*(p[2][0]*r*r+p[1][0]*(xActual-x)+p[0][0]*(-yActual+y))/r/r/r/r+(xActual-x)*(p[2][1]*r*r+p[1][1]*(xActual-x)+p[0][1]*(-yActual+y))/r/r/r/r;
00224     
00225     // calculate Kalman matrix K
00226     
00227     float k[3][2];
00228     
00229     k[0][0] = -((s[1][0]*(-p[0][2]+(p[0][1]*(-xActual+x))/r/r+(p[0][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[0][0]*(-xActual+x))/r+(p[0][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
00230     k[0][1] = (s[0][0]*(-p[0][2]+(p[0][1]*(-xActual+x))/r/r+(p[0][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[0][0]*(-xActual+x))/r+(p[0][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
00231     k[1][0] = -((s[1][0]*(-p[1][2]+(p[1][1]*(-xActual+x))/r/r+(p[1][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[1][0]*(-xActual+x))/r+(p[1][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
00232     k[1][1] = (s[0][0]*(-p[1][2]+(p[1][1]*(-xActual+x))/r/r+(p[1][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[1][0]*(-xActual+x))/r+(p[1][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
00233     k[2][0] = -((s[1][0]*(-p[2][2]+(p[2][1]*(-xActual+x))/r/r+(p[2][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]))+(s[1][1]*((p[2][0]*(-xActual+x))/r+(p[2][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
00234     k[2][1] = (s[0][0]*(-p[2][2]+(p[2][1]*(-xActual+x))/r/r+(p[2][0]*(yActual-y))/r/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1])-(s[0][1]*((p[2][0]*(-xActual+x))/r+(p[2][1]*(-yActual+y))/r))/(-(s[0][1]*s[1][0])+s[0][0]*s[1][1]);
00235     
00236     // calculate pose correction
00237     
00238     float distanceMeasured = sqrt((xMeasured-x)*(xMeasured-x)+(yMeasured-y)*(yMeasured-y));
00239     float gammaMeasured = atan2(yMeasured-y, xMeasured-x)-alpha;
00240     
00241     if (gammaMeasured > PI) gammaMeasured -= 2.0f*PI;
00242     else if (gammaMeasured < -PI) gammaMeasured += 2.0f*PI;
00243     
00244     float distanceEstimated = sqrt((xActual-x)*(xActual-x)+(yActual-y)*(yActual-y));
00245     float gammaEstimated = atan2(yActual-y, xActual-x)-alpha;
00246     
00247     if (gammaEstimated > PI) gammaEstimated -= 2.0f*PI;
00248     else if (gammaEstimated < -PI) gammaEstimated += 2.0f*PI;
00249     
00250     x += k[0][0]*(distanceMeasured-distanceEstimated)+k[0][1]*(gammaMeasured-gammaEstimated);
00251     y += k[1][0]*(distanceMeasured-distanceEstimated)+k[1][1]*(gammaMeasured-gammaEstimated);
00252     alpha += k[2][0]*(distanceMeasured-distanceEstimated)+k[2][1]*(gammaMeasured-gammaEstimated);
00253     
00254     this->x = x;
00255     this->y = y;
00256     this->alpha = alpha;
00257     
00258     // calculate correction of covariance matrix for Kalman filter P
00259     
00260     p[0][0] = k[0][1]*p[2][0]+p[0][0]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][0]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
00261     p[0][1] = k[0][1]*p[2][1]+p[0][1]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][1]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
00262     p[0][2] = k[0][1]*p[2][2]+p[0][2]*(1-(k[0][0]*(-xActual+x))/r-(k[0][1]*(yActual-y))/r/r)+p[1][2]*(-((k[0][1]*(-xActual+x))/r/r)-(k[0][0]*(-yActual+y))/r);
00263     
00264     p[1][0] = k[1][1]*p[2][0]+p[0][0]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][0]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
00265     p[1][1] = k[1][1]*p[2][1]+p[0][1]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][1]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
00266     p[1][2] = k[1][1]*p[2][2]+p[0][2]*(-((k[1][0]*(-xActual+x))/r)-(k[1][1]*(yActual-y))/r/r)+p[1][2]*(1-(k[1][1]*(-xActual+x))/r/r-(k[1][0]*(-yActual+y))/r);
00267     
00268     p[2][0] = (1+k[2][1])*p[2][0]+p[0][0]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][0]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
00269     p[2][1] = (1+k[2][1])*p[2][1]+p[0][1]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][1]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
00270     p[2][2] = (1+k[2][1])*p[2][2]+p[0][2]*(-((k[2][0]*(-xActual+x))/r)-(k[2][1]*(yActual-y))/r/r)+p[1][2]*(-((k[2][1]*(-xActual+x))/r/r)-(k[2][0]*(-yActual+y))/r);
00271     
00272     for (int i = 0; i < 3; i++) {
00273         for (int j = 0; j < 3; j++) {
00274             this->p[i][j] = p[i][j];
00275         }
00276     }
00277 }
00278 
00279 /**
00280  * This method is called periodically by the ticker object and contains the
00281  * algorithm of the speed controller.
00282  */
00283 void Controller::run() {
00284     
00285     // calculate the planned velocities using the motion planner
00286     
00287     translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
00288     rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
00289     
00290     // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
00291     
00292     desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
00293     desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
00294     
00295     // calculate actual speed of motors in [rpm]
00296     
00297     short valueCounterLeft = counterLeft.read();
00298     short valueCounterRight = counterRight.read();
00299     
00300     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00301     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00302     
00303     previousValueCounterLeft = valueCounterLeft;
00304     previousValueCounterRight = valueCounterRight;
00305     
00306     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
00307     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
00308     
00309     // calculate motor phase voltages
00310     
00311     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00312     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
00313     
00314     // calculate, limit and set duty cycles
00315     
00316     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00317     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00318     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00319     pwmLeft.write(dutyCycleLeft);
00320     
00321     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00322     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00323     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00324     pwmRight.write(dutyCycleRight);
00325     
00326     // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
00327     
00328     actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
00329     actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
00330     
00331     // calculate the actual robot pose
00332     
00333     float deltaTranslation = actualTranslationalVelocity*PERIOD;
00334     float deltaOrientation = actualRotationalVelocity*PERIOD;
00335     
00336     float sinAlpha = sin(alpha+deltaOrientation);
00337     float cosAlpha = cos(alpha+deltaOrientation);
00338     
00339     x += cosAlpha*deltaTranslation;
00340     y += sinAlpha*deltaTranslation;
00341     float alpha = this->alpha+deltaOrientation;
00342     
00343     while (alpha > PI) alpha -= 2.0f*PI;
00344     while (alpha < -PI) alpha += 2.0f*PI;
00345     
00346     this->alpha = alpha;
00347     
00348     // calculate covariance matrix for Kalman filter P
00349     
00350     p[0][0] = p[0][0]+SIGMA_TRANSLATION*SIGMA_TRANSLATION*cosAlpha*cosAlpha+deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha*sinAlpha-deltaTranslation*(p[0][2]+p[2][0])*sinAlpha;
00351     p[0][1] = p[0][1]-deltaTranslation*p[2][1]*sinAlpha+cosAlpha*(deltaTranslation*p[0][2]+(SIGMA_TRANSLATION*SIGMA_TRANSLATION-deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2]))*sinAlpha);
00352     p[0][2] = p[0][2]-deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha;
00353     
00354     p[1][0] = p[1][0]-deltaTranslation*p[1][2]*sinAlpha+cosAlpha*(deltaTranslation*p[2][0]+(SIGMA_TRANSLATION*SIGMA_TRANSLATION-deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2]))*sinAlpha);
00355     p[1][1] = p[1][1]+deltaTranslation*deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha*cosAlpha+deltaTranslation*(p[1][2]+p[2][1])*cosAlpha+SIGMA_TRANSLATION*SIGMA_TRANSLATION*sinAlpha*sinAlpha;
00356     p[1][2] = p[1][2]+deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha;
00357     
00358     p[2][0] = p[2][0]-deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*sinAlpha;
00359     p[2][1] = p[2][1]+deltaTranslation*(SIGMA_ORIENTATION*SIGMA_ORIENTATION+p[2][2])*cosAlpha;
00360     p[2][2] = p[2][2]+SIGMA_ORIENTATION*SIGMA_ORIENTATION;
00361 }
00362