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 = 1200.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 = 40.0f;                         // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
00018 const float Controller::KP = 0.25f;                          // 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(1.0f);
00043     translationalMotion.setProfileAcceleration(2.0f);
00044     translationalMotion.setProfileDeceleration(3.0f);
00045     
00046     rotationalMotion.setProfileVelocity(1.5f);
00047     rotationalMotion.setProfileAcceleration(20.0f);
00048     rotationalMotion.setProfileDeceleration(20.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     // start the periodic task
00071     
00072     ticker.attach(callback(this, &Controller::run), PERIOD);
00073 }
00074 
00075 /**
00076  * Deletes this Controller object.
00077  */
00078 Controller::~Controller() {
00079     
00080     ticker.detach(); // stop the periodic task
00081 }
00082 
00083 /**
00084  * Sets the desired translational velocity of the robot.
00085  * @param velocity the desired translational velocity, given in [m/s].
00086  */
00087 void Controller::setTranslationalVelocity(float velocity) {
00088     
00089     this->translationalVelocity = velocity;
00090 }
00091 
00092 /**
00093  * Sets the desired rotational velocity of the robot.
00094  * @param velocity the desired rotational velocity, given in [rad/s].
00095  */
00096 void Controller::setRotationalVelocity(float velocity) {
00097     
00098     this->rotationalVelocity = velocity;
00099 }
00100 
00101 /**
00102  * Gets the actual translational velocity of the robot.
00103  * @return the actual translational velocity, given in [m/s].
00104  */
00105 float Controller::getActualTranslationalVelocity() {
00106     
00107     return actualTranslationalVelocity;
00108 }
00109 
00110 /**
00111  * Gets the actual rotational velocity of the robot.
00112  * @return the actual rotational velocity, given in [rad/s].
00113  */
00114 float Controller::getActualRotationalVelocity() {
00115     
00116     return actualRotationalVelocity;
00117 }
00118 
00119 /**
00120  * Sets the desired speed of the left motor.
00121  * @param desiredSpeedLeft desired speed given in [rpm].
00122  */
00123 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
00124 
00125     this->desiredSpeedLeft = desiredSpeedLeft;
00126 }
00127 
00128 /**
00129  * Sets the desired speed of the right motor.
00130  * @param desiredSpeedRight desired speed given in [rpm].
00131  */
00132 void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
00133 
00134     this->desiredSpeedRight = desiredSpeedRight;
00135 }
00136 
00137 /**
00138  * Gets the actual speed of the left motor.
00139  * @return the actual speed given in [rpm].
00140  */
00141 float Controller::getActualSpeedLeft() {
00142 
00143     return actualSpeedLeft;
00144 }
00145 
00146 /**
00147  * Gets the actual speed of the right motor.
00148  * @return the actual speed given in [rpm].
00149  */
00150 float Controller::getActualSpeedRight() {
00151 
00152     return actualSpeedRight;
00153 }
00154 
00155 /**
00156  * This is an internal method of the controller that is running periodically.
00157  */
00158 void Controller::run() {
00159  
00160     // calculate the planned velocities using the motion planner
00161     translationalMotion.incrementToVelocity(translationalVelocity, PERIOD); // Schnelligkeit 0.5 m/s, Periode 0.001 Sek.
00162     rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
00163     
00164     // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
00165     desiredSpeedLeft = translationalVelocity - (WHEEL_DISTANCE/2)*rotationalVelocity;
00166     desiredSpeedRight = translationalVelocity + (WHEEL_DISTANCE/2)*rotationalVelocity;
00167     
00168     // calculate the actual speed of the motors in [rpm]
00169     desiredSpeedLeft = (60/(2*PI*WHEEL_RADIUS))*desiredSpeedLeft;
00170     desiredSpeedRight = (-60/(2*PI*WHEEL_RADIUS))*desiredSpeedRight;
00171     
00172     short valueCounterLeft = counterLeft.read();
00173     short valueCounterRight = counterRight.read();
00174 
00175     short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
00176     short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
00177 
00178     previousValueCounterLeft = valueCounterLeft;
00179     previousValueCounterRight = valueCounterRight;
00180 
00181     actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
00182     actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
00183 
00184     // calculate desired motor voltages Uout
00185 
00186     float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
00187     float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
00188 
00189     // calculate, limit and set the duty-cycle
00190 
00191     float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
00192     if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
00193     else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
00194     pwmLeft = dutyCycleLeft;
00195 
00196     float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
00197     if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
00198     else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
00199     pwmRight = dutyCycleRight;
00200     
00201     // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
00202     actualTranslationalVelocity = 0.5*(actualSpeedLeft+actualSpeedRight);
00203     actualRotationalVelocity = 1/(WHEEL_DISTANCE)*(actualSpeedRight-actualSpeedLeft);
00204 }
00205