ROME2 SM
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Sat Jul 23 2022 17:01:00 by
1.7.2