/ Mbed 2 deprecated Rome_P1

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@2:f381fc3a8eaf, 2020-02-26 (annotated)

Committer:
wengefa1
Date:
Wed Feb 26 14:20:16 2020 +0000
Revision:
2:f381fc3a8eaf
Motor Controller added

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wengefa1 2:f381fc3a8eaf 1 /*
wengefa1 2:f381fc3a8eaf 2 * Controller.cpp
wengefa1 2:f381fc3a8eaf 3 * Copyright (c) 2020, ZHAW
wengefa1 2:f381fc3a8eaf 4 * All rights reserved.
wengefa1 2:f381fc3a8eaf 5 */
wengefa1 2:f381fc3a8eaf 6
wengefa1 2:f381fc3a8eaf 7 #include "Controller.h"
wengefa1 2:f381fc3a8eaf 8
wengefa1 2:f381fc3a8eaf 9 using namespace std;
wengefa1 2:f381fc3a8eaf 10
wengefa1 2:f381fc3a8eaf 11 const float Controller::PERIOD = 0.001f; // period of 1 ms
wengefa1 2:f381fc3a8eaf 12 const float Controller::COUNTS_PER_TURN = 86016.0f; // encoder resolution (pololu motors: 1200.0f, maxon motors: 86016.0f)
wengefa1 2:f381fc3a8eaf 13 const float Controller::LOWPASS_FILTER_FREQUENCY = 300.0f; // given in [rad/s]
wengefa1 2:f381fc3a8eaf 14 const float Controller::KN = 45.0f; // speed constant in [rpm/V] (pololu motors: 40.0f, maxon motors: 45.0f)
wengefa1 2:f381fc3a8eaf 15 const float Controller::KP = 0.12f; // speed control parameter
wengefa1 2:f381fc3a8eaf 16 const float Controller::MAX_VOLTAGE = 12.0f; // battery voltage in [V]
wengefa1 2:f381fc3a8eaf 17 const float Controller::MIN_DUTY_CYCLE = 0.02f; // minimum duty-cycle
wengefa1 2:f381fc3a8eaf 18 const float Controller::MAX_DUTY_CYCLE = 0.98f; // maximum duty-cycle
wengefa1 2:f381fc3a8eaf 19
wengefa1 2:f381fc3a8eaf 20 /**
wengefa1 2:f381fc3a8eaf 21 * Creates and initialises the robot controller.
wengefa1 2:f381fc3a8eaf 22 * @param pwmLeft a reference to the pwm output for the left motor.
wengefa1 2:f381fc3a8eaf 23 * @param pwmRight a reference to the pwm output for the right motor.
wengefa1 2:f381fc3a8eaf 24 * @param counterLeft a reference to the encoder counter of the left motor.
wengefa1 2:f381fc3a8eaf 25 * @param counterRight a reference to the encoder counter of the right motor.
wengefa1 2:f381fc3a8eaf 26 */
wengefa1 2:f381fc3a8eaf 27 Controller::Controller(PwmOut& pwmLeft, PwmOut& pwmRight, EncoderCounter& counterLeft, EncoderCounter& counterRight) : pwmLeft(pwmLeft), pwmRight(pwmRight), counterLeft(counterLeft), counterRight(counterRight) {
wengefa1 2:f381fc3a8eaf 28
wengefa1 2:f381fc3a8eaf 29 // initialise pwm outputs
wengefa1 2:f381fc3a8eaf 30
wengefa1 2:f381fc3a8eaf 31 pwmLeft.period(0.00005f); // pwm period of 50 us
wengefa1 2:f381fc3a8eaf 32 pwmLeft = 0.5f; // duty-cycle of 50%
wengefa1 2:f381fc3a8eaf 33
wengefa1 2:f381fc3a8eaf 34 pwmRight.period(0.00005f); // pwm period of 50 us
wengefa1 2:f381fc3a8eaf 35 pwmRight = 0.5f; // duty-cycle of 50%
wengefa1 2:f381fc3a8eaf 36
wengefa1 2:f381fc3a8eaf 37 // initialise local variables
wengefa1 2:f381fc3a8eaf 38
wengefa1 2:f381fc3a8eaf 39 previousValueCounterLeft = counterLeft.read();
wengefa1 2:f381fc3a8eaf 40 previousValueCounterRight = counterRight.read();
wengefa1 2:f381fc3a8eaf 41
wengefa1 2:f381fc3a8eaf 42 speedLeftFilter.setPeriod(PERIOD);
wengefa1 2:f381fc3a8eaf 43 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
wengefa1 2:f381fc3a8eaf 44
wengefa1 2:f381fc3a8eaf 45 speedRightFilter.setPeriod(PERIOD);
wengefa1 2:f381fc3a8eaf 46 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
wengefa1 2:f381fc3a8eaf 47
wengefa1 2:f381fc3a8eaf 48 desiredSpeedLeft = 0.0f;
wengefa1 2:f381fc3a8eaf 49 desiredSpeedRight = 0.0f;
wengefa1 2:f381fc3a8eaf 50
wengefa1 2:f381fc3a8eaf 51 actualSpeedLeft = 0.0f;
wengefa1 2:f381fc3a8eaf 52 actualSpeedRight = 0.0f;
wengefa1 2:f381fc3a8eaf 53
wengefa1 2:f381fc3a8eaf 54 // start the periodic task
wengefa1 2:f381fc3a8eaf 55
wengefa1 2:f381fc3a8eaf 56 ticker.attach(callback(this, &Controller::run), PERIOD);
wengefa1 2:f381fc3a8eaf 57 }
wengefa1 2:f381fc3a8eaf 58
wengefa1 2:f381fc3a8eaf 59 /**
wengefa1 2:f381fc3a8eaf 60 * Deletes this Controller object.
wengefa1 2:f381fc3a8eaf 61 */
wengefa1 2:f381fc3a8eaf 62 Controller::~Controller() {
wengefa1 2:f381fc3a8eaf 63
wengefa1 2:f381fc3a8eaf 64 ticker.detach(); // stop the periodic task
wengefa1 2:f381fc3a8eaf 65 }
wengefa1 2:f381fc3a8eaf 66
wengefa1 2:f381fc3a8eaf 67 /**
wengefa1 2:f381fc3a8eaf 68 * Sets the desired speed of the left motor.
wengefa1 2:f381fc3a8eaf 69 * @param desiredSpeedLeft desired speed given in [rpm].
wengefa1 2:f381fc3a8eaf 70 */
wengefa1 2:f381fc3a8eaf 71 void Controller::setDesiredSpeedLeft(float desiredSpeedLeft) {
wengefa1 2:f381fc3a8eaf 72
wengefa1 2:f381fc3a8eaf 73 this->desiredSpeedLeft = desiredSpeedLeft;
wengefa1 2:f381fc3a8eaf 74 }
wengefa1 2:f381fc3a8eaf 75
wengefa1 2:f381fc3a8eaf 76 /**
wengefa1 2:f381fc3a8eaf 77 * Sets the desired speed of the right motor.
wengefa1 2:f381fc3a8eaf 78 * @param desiredSpeedRight desired speed given in [rpm].
wengefa1 2:f381fc3a8eaf 79 */
wengefa1 2:f381fc3a8eaf 80 void Controller::setDesiredSpeedRight(float desiredSpeedRight) {
wengefa1 2:f381fc3a8eaf 81
wengefa1 2:f381fc3a8eaf 82 this->desiredSpeedRight = desiredSpeedRight;
wengefa1 2:f381fc3a8eaf 83 }
wengefa1 2:f381fc3a8eaf 84
wengefa1 2:f381fc3a8eaf 85 /**
wengefa1 2:f381fc3a8eaf 86 * This is an internal method of the controller that is running periodically.
wengefa1 2:f381fc3a8eaf 87 */
wengefa1 2:f381fc3a8eaf 88 void Controller::run() {
wengefa1 2:f381fc3a8eaf 89
wengefa1 2:f381fc3a8eaf 90 // calculate the actual speed of the motors in [rpm]
wengefa1 2:f381fc3a8eaf 91
wengefa1 2:f381fc3a8eaf 92 short valueCounterLeft = counterLeft.read();
wengefa1 2:f381fc3a8eaf 93 short valueCounterRight = counterRight.read();
wengefa1 2:f381fc3a8eaf 94
wengefa1 2:f381fc3a8eaf 95 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
wengefa1 2:f381fc3a8eaf 96 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
wengefa1 2:f381fc3a8eaf 97
wengefa1 2:f381fc3a8eaf 98 previousValueCounterLeft = valueCounterLeft;
wengefa1 2:f381fc3a8eaf 99 previousValueCounterRight = valueCounterRight;
wengefa1 2:f381fc3a8eaf 100
wengefa1 2:f381fc3a8eaf 101 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft/COUNTS_PER_TURN/PERIOD*60.0f);
wengefa1 2:f381fc3a8eaf 102 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight/COUNTS_PER_TURN/PERIOD*60.0f);
wengefa1 2:f381fc3a8eaf 103
wengefa1 2:f381fc3a8eaf 104 // calculate desired motor voltages Uout
wengefa1 2:f381fc3a8eaf 105 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
wengefa1 2:f381fc3a8eaf 106 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
wengefa1 2:f381fc3a8eaf 107
wengefa1 2:f381fc3a8eaf 108 // calculate, limit and set the duty-cycle
wengefa1 2:f381fc3a8eaf 109
wengefa1 2:f381fc3a8eaf 110 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
wengefa1 2:f381fc3a8eaf 111 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
wengefa1 2:f381fc3a8eaf 112 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
wengefa1 2:f381fc3a8eaf 113 pwmLeft = dutyCycleLeft;
wengefa1 2:f381fc3a8eaf 114
wengefa1 2:f381fc3a8eaf 115 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
wengefa1 2:f381fc3a8eaf 116 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
wengefa1 2:f381fc3a8eaf 117 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
wengefa1 2:f381fc3a8eaf 118 pwmRight = dutyCycleRight;
wengefa1 2:f381fc3a8eaf 119 }
wengefa1 2:f381fc3a8eaf 120
wengefa1 2:f381fc3a8eaf 121 float Controller::getSpeedLeft(){
wengefa1 2:f381fc3a8eaf 122 return actualSpeedLeft;
wengefa1 2:f381fc3a8eaf 123 }
wengefa1 2:f381fc3a8eaf 124
wengefa1 2:f381fc3a8eaf 125 float Controller::getSpeedRight(){
wengefa1 2:f381fc3a8eaf 126 return actualSpeedRight;
wengefa1 2:f381fc3a8eaf 127 }
wengefa1 2:f381fc3a8eaf 128
wengefa1 2:f381fc3a8eaf 129 float Controller::getDesSpeedLeft(){
wengefa1 2:f381fc3a8eaf 130 return desiredSpeedLeft;
wengefa1 2:f381fc3a8eaf 131 }
wengefa1 2:f381fc3a8eaf 132
wengefa1 2:f381fc3a8eaf 133 float Controller::getDesSpeedRight(){
wengefa1 2:f381fc3a8eaf 134 return desiredSpeedRight;
wengefa1 2:f381fc3a8eaf 135 }