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@0:e360940c4b88, 2018-03-16 (annotated)

Committer:
Appalco
Date:
Fri Mar 16 13:32:47 2018 +0000
Revision:
0:e360940c4b88
Child:
1:31b1c5cefd64
import

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