Zürcher Eliteeinheit / Mbed 2 deprecated ROME2_P2

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Controller.cpp@3:9433326c2248, 2018-03-16 (annotated)

Committer:
Appalco
Date:
Fri Mar 16 19:44:20 2018 +0000
Revision:
3:9433326c2248
Parent:
1:31b1c5cefd64 
added commented untested fwd  detection

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Appalco 0:e360940c4b88 1 /*
Appalco 0:e360940c4b88 2 * Controller.cpp
Appalco 0:e360940c4b88 3 * Copyright (c) 2018, ZHAW
Appalco 0:e360940c4b88 4 * All rights reserved.
Appalco 0:e360940c4b88 5 */
Appalco 0:e360940c4b88 6
Appalco 0:e360940c4b88 7 #include <cmath>
Appalco 0:e360940c4b88 8 #include "Controller.h"
Appalco 0:e360940c4b88 9
Appalco 1:31b1c5cefd64 10
Appalco 0:e360940c4b88 11 using namespace std;
Appalco 0:e360940c4b88 12
Appalco 1:31b1c5cefd64 13
Appalco 0:e360940c4b88 14 const float Controller::PERIOD = 0.001f; // period of control task, given in [s]
Appalco 0:e360940c4b88 15 const float Controller::COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
Appalco 0:e360940c4b88 16 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
Appalco 0:e360940c4b88 17 const float Controller::KN = 40.0f; // speed constant of motor, given in [rpm/V]
Appalco 0:e360940c4b88 18 const float Controller::KP = 0.2f; // speed controller gain, given in [V/rpm]
Appalco 0:e360940c4b88 19 const float Controller::MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
Appalco 0:e360940c4b88 20 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
Appalco 0:e360940c4b88 21 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
Appalco 1:31b1c5cefd64 22 const float Controller::PI = 3.14159265f; // the constant PI
Appalco 1:31b1c5cefd64 23 const float Controller::WHEEL_DISTANCE = 0.170f; // distance between wheels, given in [m]
Appalco 1:31b1c5cefd64 24 const float Controller::WHEEL_RADIUS = 0.0375f; // radius of wheels, given in [m]
Appalco 0:e360940c4b88 25 /**
Appalco 0:e360940c4b88 26 * Creates and initializes a Controller object.
Appalco 0:e360940c4b88 27 * @param pwmLeft a pwm output object to set the duty cycle for the left motor.
Appalco 0:e360940c4b88 28 * @param pwmRight a pwm output object to set the duty cycle for the right motor.
Appalco 0:e360940c4b88 29 * @param counterLeft an encoder counter object to read the position of the left motor.
Appalco 0:e360940c4b88 30 * @param counterRight an encoder counter object to read the position of the right motor.
Appalco 0:e360940c4b88 31 */
Appalco 0:e360940c4b88 32 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
Appalco 0:e360940c4b88 33
Appalco 0:e360940c4b88 34 // initialize periphery drivers
Appalco 0:e360940c4b88 35
Appalco 0:e360940c4b88 36 pwmLeft.period(0.00005f);
Appalco 0:e360940c4b88 37 pwmLeft.write(0.5f);
Appalco 0:e360940c4b88 38
Appalco 0:e360940c4b88 39 pwmRight.period(0.00005f);
Appalco 0:e360940c4b88 40 pwmRight.write(0.5f);
Appalco 0:e360940c4b88 41
Appalco 0:e360940c4b88 42 // initialize local variables
Appalco 0:e360940c4b88 43
Appalco 0:e360940c4b88 44 translationalMotion.setProfileVelocity(0.5f);
Appalco 0:e360940c4b88 45 translationalMotion.setProfileAcceleration(1.0f);
Appalco 0:e360940c4b88 46 translationalMotion.setProfileDeceleration(1.0f);
Appalco 0:e360940c4b88 47
Appalco 0:e360940c4b88 48 rotationalMotion.setProfileVelocity(1.0f);
Appalco 0:e360940c4b88 49 rotationalMotion.setProfileAcceleration(2.0f);
Appalco 0:e360940c4b88 50 rotationalMotion.setProfileDeceleration(2.0f);
Appalco 0:e360940c4b88 51
Appalco 0:e360940c4b88 52 translationalVelocity = 0.0f;
Appalco 0:e360940c4b88 53 rotationalVelocity = 0.0f;
Appalco 0:e360940c4b88 54 actualTranslationalVelocity = 0.0f;
Appalco 0:e360940c4b88 55 actualRotationalVelocity = 0.0f;
Appalco 0:e360940c4b88 56
Appalco 0:e360940c4b88 57 previousValueCounterLeft = counterLeft.read();
Appalco 0:e360940c4b88 58 previousValueCounterRight = counterRight.read();
Appalco 0:e360940c4b88 59
Appalco 0:e360940c4b88 60 speedLeftFilter.setPeriod(PERIOD);
Appalco 0:e360940c4b88 61 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
Appalco 0:e360940c4b88 62
Appalco 0:e360940c4b88 63 speedRightFilter.setPeriod(PERIOD);
Appalco 0:e360940c4b88 64 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
Appalco 0:e360940c4b88 65
Appalco 0:e360940c4b88 66 desiredSpeedLeft = 0.0f;
Appalco 0:e360940c4b88 67 desiredSpeedRight = 0.0f;
Appalco 0:e360940c4b88 68
Appalco 0:e360940c4b88 69 actualSpeedLeft = 0.0f;
Appalco 0:e360940c4b88 70 actualSpeedRight = 0.0f;
Appalco 0:e360940c4b88 71
Appalco 0:e360940c4b88 72 // start periodic task
Appalco 0:e360940c4b88 73
Appalco 0:e360940c4b88 74 ticker.attach(callback(this, &Controller::run), PERIOD);
Appalco 0:e360940c4b88 75 }
Appalco 0:e360940c4b88 76
Appalco 0:e360940c4b88 77 /**
Appalco 0:e360940c4b88 78 * Deletes the Controller object and releases all allocated resources.
Appalco 0:e360940c4b88 79 */
Appalco 0:e360940c4b88 80 Controller::~Controller() {
Appalco 0:e360940c4b88 81
Appalco 0:e360940c4b88 82 ticker.detach();
Appalco 0:e360940c4b88 83 }
Appalco 0:e360940c4b88 84
Appalco 0:e360940c4b88 85 /**
Appalco 0:e360940c4b88 86 * Sets the desired translational velocity of the robot.
Appalco 0:e360940c4b88 87 * @param velocity the desired translational velocity, given in [m/s].
Appalco 0:e360940c4b88 88 */
Appalco 0:e360940c4b88 89 void Controller::setTranslationalVelocity(float velocity) {
Appalco 0:e360940c4b88 90
Appalco 0:e360940c4b88 91 this->translationalVelocity = velocity;
Appalco 0:e360940c4b88 92 }
Appalco 0:e360940c4b88 93
Appalco 0:e360940c4b88 94 /**
Appalco 0:e360940c4b88 95 * Sets the desired rotational velocity of the robot.
Appalco 0:e360940c4b88 96 * @param velocity the desired rotational velocity, given in [rad/s].
Appalco 0:e360940c4b88 97 */
Appalco 0:e360940c4b88 98 void Controller::setRotationalVelocity(float velocity) {
Appalco 0:e360940c4b88 99
Appalco 0:e360940c4b88 100 this->rotationalVelocity = velocity;
Appalco 0:e360940c4b88 101 }
Appalco 0:e360940c4b88 102
Appalco 0:e360940c4b88 103 /**
Appalco 0:e360940c4b88 104 * Gets the actual translational velocity of the robot.
Appalco 0:e360940c4b88 105 * @return the actual translational velocity, given in [m/s].
Appalco 0:e360940c4b88 106 */
Appalco 0:e360940c4b88 107 float Controller::getActualTranslationalVelocity() {
Appalco 0:e360940c4b88 108
Appalco 0:e360940c4b88 109 return actualTranslationalVelocity;
Appalco 0:e360940c4b88 110 }
Appalco 0:e360940c4b88 111
Appalco 0:e360940c4b88 112 /**
Appalco 0:e360940c4b88 113 * Gets the actual rotational velocity of the robot.
Appalco 0:e360940c4b88 114 * @return the actual rotational velocity, given in [rad/s].
Appalco 0:e360940c4b88 115 */
Appalco 0:e360940c4b88 116 float Controller::getActualRotationalVelocity() {
Appalco 0:e360940c4b88 117
Appalco 0:e360940c4b88 118 return actualRotationalVelocity;
Appalco 0:e360940c4b88 119 }
Appalco 0:e360940c4b88 120
Appalco 0:e360940c4b88 121 /**
Appalco 0:e360940c4b88 122 * This method is called periodically by the ticker object and contains the
Appalco 0:e360940c4b88 123 * algorithm of the speed controller.
Appalco 0:e360940c4b88 124 */
Appalco 0:e360940c4b88 125 void Controller::run() {
Appalco 0:e360940c4b88 126
Appalco 1:31b1c5cefd64 127 // calculate the planned velocities using the motion planner
Appalco 0:e360940c4b88 128
Appalco 1:31b1c5cefd64 129 translationalMotion.incrementToVelocity(translationalVelocity, PERIOD);
Appalco 1:31b1c5cefd64 130 rotationalMotion.incrementToVelocity(rotationalVelocity, PERIOD);
Appalco 0:e360940c4b88 131
Appalco 0:e360940c4b88 132 // calculate the values 'desiredSpeedLeft' and 'desiredSpeedRight' using the kinematic model
Appalco 0:e360940c4b88 133
Appalco 1:31b1c5cefd64 134 desiredSpeedLeft = (translationalMotion.velocity-WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
Appalco 1:31b1c5cefd64 135 desiredSpeedRight = -(translationalMotion.velocity+WHEEL_DISTANCE/2.0f*rotationalMotion.velocity)/WHEEL_RADIUS*60.0f/2.0f/PI;
Appalco 0:e360940c4b88 136
Appalco 0:e360940c4b88 137
Appalco 0:e360940c4b88 138 // calculate actual speed of motors in [rpm]
Appalco 0:e360940c4b88 139
Appalco 0:e360940c4b88 140 short valueCounterLeft = counterLeft.read();
Appalco 0:e360940c4b88 141 short valueCounterRight = counterRight.read();
Appalco 0:e360940c4b88 142
Appalco 0:e360940c4b88 143 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
Appalco 0:e360940c4b88 144 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
Appalco 0:e360940c4b88 145
Appalco 0:e360940c4b88 146 previousValueCounterLeft = valueCounterLeft;
Appalco 0:e360940c4b88 147 previousValueCounterRight = valueCounterRight;
Appalco 0:e360940c4b88 148
Appalco 0:e360940c4b88 149 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
Appalco 0:e360940c4b88 150 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
Appalco 0:e360940c4b88 151
Appalco 0:e360940c4b88 152 // calculate motor phase voltages
Appalco 0:e360940c4b88 153
Appalco 0:e360940c4b88 154 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
Appalco 0:e360940c4b88 155 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
Appalco 0:e360940c4b88 156
Appalco 0:e360940c4b88 157 // calculate, limit and set duty cycles
Appalco 0:e360940c4b88 158
Appalco 0:e360940c4b88 159 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
Appalco 0:e360940c4b88 160 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
Appalco 0:e360940c4b88 161 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
Appalco 0:e360940c4b88 162 pwmLeft.write(dutyCycleLeft);
Appalco 0:e360940c4b88 163
Appalco 0:e360940c4b88 164 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
Appalco 0:e360940c4b88 165 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
Appalco 0:e360940c4b88 166 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
Appalco 0:e360940c4b88 167 pwmRight.write(dutyCycleRight);
Appalco 0:e360940c4b88 168
Appalco 0:e360940c4b88 169 // calculate the values 'actualTranslationalVelocity' and 'actualRotationalVelocity' using the kinematic model
Appalco 1:31b1c5cefd64 170
Appalco 0:e360940c4b88 171
Appalco 1:31b1c5cefd64 172 actualTranslationalVelocity = (actualSpeedLeft-actualSpeedRight)*2.0f*PI/60.0f*WHEEL_RADIUS/2.0f;
Appalco 1:31b1c5cefd64 173 actualRotationalVelocity = (-actualSpeedRight-actualSpeedLeft)*2.0f*PI/60.0f*WHEEL_RADIUS/WHEEL_DISTANCE;
Appalco 0:e360940c4b88 174
Appalco 0:e360940c4b88 175 }
Appalco 0:e360940c4b88 176