TeamSurface / Library

P2 halbfertig

Fork of Library by St Knz

Controller.cpp@1:f38485404dbe, 2018-03-09 (annotated)

Committer:
kueenste
Date:
Fri Mar 09 16:00:42 2018 +0000
Revision:
1:f38485404dbe
Parent:
0:bb408887ab78
Child:
2:6dd39662e6e5
PT2 funktioniert einigermassen;

Who changed what in which revision?

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