Marco Oehler / Mbed 2 deprecated ROME2

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:7ee4c6416e08, 2020-02-24 (annotated)

Committer:
oehlemar
Date:
Mon Feb 24 16:05:50 2020 +0000
Revision:
0:7ee4c6416e08
ROME2 P1

Who changed what in which revision?

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