Michael Ernst Peter / Mbed OS PM2_Example_ROME2_Robots

Example project

Dependencies:   PM2_Libary Eigen

Controller.cpp@41:7484471403aa, 2022-05-11 (annotated)

Committer:
pmic
Date:
Wed May 11 09:44:25 2022 +0200
Revision:
41:7484471403aa
Parent:
38:8aae5cbcf25f 
Delete StateMachine

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 37:698d6b73b50c 1 /*
pmic 37:698d6b73b50c 2 * Controller.cpp
pmic 37:698d6b73b50c 3 * Copyright (c) 2022, ZHAW
pmic 37:698d6b73b50c 4 * All rights reserved.
pmic 37:698d6b73b50c 5 */
pmic 37:698d6b73b50c 6
pmic 37:698d6b73b50c 7 #include "Controller.h"
pmic 37:698d6b73b50c 8
pmic 37:698d6b73b50c 9 using namespace std;
pmic 37:698d6b73b50c 10
pmic 37:698d6b73b50c 11 const float Controller::PERIOD = 0.001f; // period of control task, given in [s]
pmic 37:698d6b73b50c 12 const float Controller::M_PI = 3.14159265f; // the mathematical constant PI
pmic 37:698d6b73b50c 13 const float Controller::WHEEL_DISTANCE = 0.190f; // distance between wheels, given in [m]
pmic 37:698d6b73b50c 14 const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m]
pmic 37:698d6b73b50c 15 const float Controller::MAXIMUM_VELOCITY = 500.0; // maximum wheel velocity, given in [rpm]
pmic 37:698d6b73b50c 16 const float Controller::MAXIMUM_ACCELERATION = 1000.0; // maximum wheel acceleration, given in [rpm/s]
pmic 37:698d6b73b50c 17 const float Controller::COUNTS_PER_TURN = 1200.0f; // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
pmic 37:698d6b73b50c 18 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
pmic 37:698d6b73b50c 19 const float Controller::KN = 40.0f; // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
pmic 37:698d6b73b50c 20 const float Controller::KP = 0.15f; // speed control parameter
pmic 37:698d6b73b50c 21 const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
pmic 37:698d6b73b50c 22 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
pmic 37:698d6b73b50c 23 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
pmic 37:698d6b73b50c 24
pmic 37:698d6b73b50c 25 /**
pmic 37:698d6b73b50c 26 * Creates and initialises the robot controller.
pmic 37:698d6b73b50c 27 * @param pwmLeft a reference to the pwm output for the left motor.
pmic 37:698d6b73b50c 28 * @param pwmRight a reference to the pwm output for the right motor.
pmic 37:698d6b73b50c 29 * @param counterLeft a reference to the encoder counter of the left motor.
pmic 37:698d6b73b50c 30 * @param counterRight a reference to the encoder counter of the right motor.
pmic 37:698d6b73b50c 31 */
pmic 37:698d6b73b50c 32 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounterROME2& counterLeft, EncoderCounterROME2& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight), thread(osPriorityHigh, STACK_SIZE) {
pmic 37:698d6b73b50c 33
pmic 37:698d6b73b50c 34 // initialise pwm outputs
pmic 37:698d6b73b50c 35
pmic 37:698d6b73b50c 36 pwmLeft.period(0.00005f); // pwm period of 50 us
pmic 37:698d6b73b50c 37 pwmLeft = 0.5f; // duty-cycle of 50%
pmic 37:698d6b73b50c 38
pmic 37:698d6b73b50c 39 pwmRight.period(0.00005f); // pwm period of 50 us
pmic 37:698d6b73b50c 40 pwmRight = 0.5f; // duty-cycle of 50%
pmic 37:698d6b73b50c 41
pmic 37:698d6b73b50c 42 // initialise local variables
pmic 37:698d6b73b50c 43
pmic 37:698d6b73b50c 44 translationalVelocity = 0.0f;
pmic 37:698d6b73b50c 45 rotationalVelocity = 0.0f;
pmic 37:698d6b73b50c 46
pmic 37:698d6b73b50c 47 actualTranslationalVelocity = 0.0f;
pmic 37:698d6b73b50c 48 actualRotationalVelocity = 0.0f;
pmic 37:698d6b73b50c 49
pmic 37:698d6b73b50c 50 desiredSpeedLeft = 0.0f;
pmic 37:698d6b73b50c 51 desiredSpeedRight = 0.0f;
pmic 37:698d6b73b50c 52
pmic 37:698d6b73b50c 53 actualSpeedLeft = 0.0f;
pmic 37:698d6b73b50c 54 actualSpeedRight = 0.0f;
pmic 37:698d6b73b50c 55
pmic 37:698d6b73b50c 56 motionLeft.setProfileVelocity(MAXIMUM_VELOCITY);
pmic 37:698d6b73b50c 57 motionLeft.setProfileAcceleration(MAXIMUM_ACCELERATION);
pmic 37:698d6b73b50c 58 motionLeft.setProfileDeceleration(MAXIMUM_ACCELERATION);
pmic 37:698d6b73b50c 59
pmic 37:698d6b73b50c 60 motionRight.setProfileVelocity(MAXIMUM_VELOCITY);
pmic 37:698d6b73b50c 61 motionRight.setProfileAcceleration(MAXIMUM_ACCELERATION);
pmic 37:698d6b73b50c 62 motionRight.setProfileDeceleration(MAXIMUM_ACCELERATION);
pmic 37:698d6b73b50c 63
pmic 37:698d6b73b50c 64 previousValueCounterLeft = counterLeft.read();
pmic 37:698d6b73b50c 65 previousValueCounterRight = counterRight.read();
pmic 37:698d6b73b50c 66
pmic 37:698d6b73b50c 67 speedLeftFilter.setPeriod(PERIOD);
pmic 37:698d6b73b50c 68 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 37:698d6b73b50c 69
pmic 37:698d6b73b50c 70 speedRightFilter.setPeriod(PERIOD);
pmic 37:698d6b73b50c 71 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 37:698d6b73b50c 72
pmic 37:698d6b73b50c 73 // start thread and timer interrupt
pmic 37:698d6b73b50c 74
pmic 37:698d6b73b50c 75 thread.start(callback(this, &Controller::run));
pmic 38:8aae5cbcf25f 76 ticker.attach(callback(this, &Controller::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * PERIOD)});
pmic 37:698d6b73b50c 77 }
pmic 37:698d6b73b50c 78
pmic 37:698d6b73b50c 79 /**
pmic 37:698d6b73b50c 80 * Deletes this Controller object.
pmic 37:698d6b73b50c 81 */
pmic 37:698d6b73b50c 82 Controller::~Controller() {
pmic 37:698d6b73b50c 83
pmic 37:698d6b73b50c 84 ticker.detach(); // stop the timer interrupt
pmic 37:698d6b73b50c 85 }
pmic 37:698d6b73b50c 86
pmic 37:698d6b73b50c 87 /**
pmic 37:698d6b73b50c 88 * Sets the desired translational velocity of the robot.
pmic 37:698d6b73b50c 89 * @param velocity the desired translational velocity, given in [m/s].
pmic 37:698d6b73b50c 90 */
pmic 37:698d6b73b50c 91 void Controller::setTranslationalVelocity(float velocity) {
pmic 37:698d6b73b50c 92
pmic 37:698d6b73b50c 93 this->translationalVelocity = velocity;
pmic 37:698d6b73b50c 94 }
pmic 37:698d6b73b50c 95
pmic 37:698d6b73b50c 96 /**
pmic 37:698d6b73b50c 97 * Sets the desired rotational velocity of the robot.
pmic 37:698d6b73b50c 98 * @param velocity the desired rotational velocity, given in [rad/s].
pmic 37:698d6b73b50c 99 */
pmic 37:698d6b73b50c 100 void Controller::setRotationalVelocity(float velocity) {
pmic 37:698d6b73b50c 101
pmic 37:698d6b73b50c 102 this->rotationalVelocity = velocity;
pmic 37:698d6b73b50c 103 }
pmic 37:698d6b73b50c 104
pmic 37:698d6b73b50c 105 /**
pmic 37:698d6b73b50c 106 * Gets the actual translational velocity of the robot.
pmic 37:698d6b73b50c 107 * @return the actual translational velocity, given in [m/s].
pmic 37:698d6b73b50c 108 */
pmic 37:698d6b73b50c 109 float Controller::getActualTranslationalVelocity() {
pmic 37:698d6b73b50c 110
pmic 37:698d6b73b50c 111 return actualTranslationalVelocity;
pmic 37:698d6b73b50c 112 }
pmic 37:698d6b73b50c 113
pmic 37:698d6b73b50c 114 /**
pmic 37:698d6b73b50c 115 * Gets the actual rotational velocity of the robot.
pmic 37:698d6b73b50c 116 * @return the actual rotational velocity, given in [rad/s].
pmic 37:698d6b73b50c 117 */
pmic 37:698d6b73b50c 118 float Controller::getActualRotationalVelocity() {
pmic 37:698d6b73b50c 119
pmic 37:698d6b73b50c 120 return actualRotationalVelocity;
pmic 37:698d6b73b50c 121 }
pmic 37:698d6b73b50c 122
pmic 37:698d6b73b50c 123 /**
pmic 37:698d6b73b50c 124 * This method is called by the ticker timer interrupt service routine.
pmic 37:698d6b73b50c 125 * It sends a flag to the thread to make it run again.
pmic 37:698d6b73b50c 126 */
pmic 37:698d6b73b50c 127 void Controller::sendThreadFlag() {
pmic 37:698d6b73b50c 128
pmic 37:698d6b73b50c 129 thread.flags_set(threadFlag);
pmic 37:698d6b73b50c 130 }
pmic 37:698d6b73b50c 131
pmic 37:698d6b73b50c 132 /**
pmic 37:698d6b73b50c 133 * This is an internal method of the controller that is running periodically.
pmic 37:698d6b73b50c 134 */
pmic 37:698d6b73b50c 135 void Controller::run() {
pmic 37:698d6b73b50c 136
pmic 37:698d6b73b50c 137 while (true) {
pmic 37:698d6b73b50c 138
pmic 37:698d6b73b50c 139 // wait for the periodic thread flag
pmic 37:698d6b73b50c 140
pmic 37:698d6b73b50c 141 ThisThread::flags_wait_any(threadFlag);
pmic 37:698d6b73b50c 142
pmic 37:698d6b73b50c 143 // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
pmic 37:698d6b73b50c 144
pmic 37:698d6b73b50c 145 desiredSpeedLeft = (translationalVelocity-WHEEL_DISTANCE/2.0f*rotationalVelocity)/WHEEL_RADIUS*60.0f/2.0f/M_PI;
pmic 37:698d6b73b50c 146 desiredSpeedRight = -(translationalVelocity+WHEEL_DISTANCE/2.0f*rotationalVelocity)/WHEEL_RADIUS*60.0f/2.0f/M_PI;
pmic 37:698d6b73b50c 147
pmic 37:698d6b73b50c 148 // calculate planned speedLeft and speedRight values using the motion planner
pmic 37:698d6b73b50c 149
pmic 37:698d6b73b50c 150 motionLeft.incrementToVelocity(desiredSpeedLeft, PERIOD);
pmic 37:698d6b73b50c 151 motionRight.incrementToVelocity(desiredSpeedRight, PERIOD);
pmic 37:698d6b73b50c 152
pmic 37:698d6b73b50c 153 desiredSpeedLeft = motionLeft.getVelocity();
pmic 37:698d6b73b50c 154 desiredSpeedRight = motionRight.getVelocity();
pmic 37:698d6b73b50c 155
pmic 37:698d6b73b50c 156 // calculate the actual speed of the motors in [rpm]
pmic 37:698d6b73b50c 157
pmic 37:698d6b73b50c 158 short valueCounterLeft = counterLeft.read();
pmic 37:698d6b73b50c 159 short valueCounterRight = counterRight.read();
pmic 37:698d6b73b50c 160
pmic 37:698d6b73b50c 161 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
pmic 37:698d6b73b50c 162 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
pmic 37:698d6b73b50c 163
pmic 37:698d6b73b50c 164 previousValueCounterLeft = valueCounterLeft;
pmic 37:698d6b73b50c 165 previousValueCounterRight = valueCounterRight;
pmic 37:698d6b73b50c 166
pmic 37:698d6b73b50c 167 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
pmic 37:698d6b73b50c 168 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
pmic 37:698d6b73b50c 169
pmic 37:698d6b73b50c 170 // calculate desired motor voltages Uout
pmic 37:698d6b73b50c 171
pmic 37:698d6b73b50c 172 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
pmic 37:698d6b73b50c 173 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
pmic 37:698d6b73b50c 174
pmic 37:698d6b73b50c 175 // calculate, limit and set the duty-cycle
pmic 37:698d6b73b50c 176
pmic 37:698d6b73b50c 177 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
pmic 37:698d6b73b50c 178 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
pmic 37:698d6b73b50c 179 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
pmic 37:698d6b73b50c 180 pwmLeft = dutyCycleLeft;
pmic 37:698d6b73b50c 181
pmic 37:698d6b73b50c 182 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
pmic 37:698d6b73b50c 183 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
pmic 37:698d6b73b50c 184 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
pmic 37:698d6b73b50c 185 pwmRight = dutyCycleRight;
pmic 37:698d6b73b50c 186
pmic 37:698d6b73b50c 187 // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
pmic 37:698d6b73b50c 188
pmic 37:698d6b73b50c 189 actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*M_PI/60.0f*WHEEL_RADIUS/2.0f;
pmic 37:698d6b73b50c 190 actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*M_PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
pmic 37:698d6b73b50c 191 }
pmic 37:698d6b73b50c 192 }