Michael Ernst Peter / PM2_Libary

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:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

PositionController.cpp@29:335fb9b01ca7, 2022-05-13 (annotated)

Committer:
pmic
Date:
Fri May 13 20:53:33 2022 +0000
Revision:
29:335fb9b01ca7
Parent:
19:518ed284d98b
Child:
30:05abc1d2a2b9
Motion integrated into SpeedController and PositionController class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pmic 8:6b747ad59ff5 1 #include "PositionController.h"
pmic 8:6b747ad59ff5 2
pmic 8:6b747ad59ff5 3 const float PositionController::TS = 0.001f; // period of 1 ms
pmic 8:6b747ad59ff5 4 const float PositionController::LOWPASS_FILTER_FREQUENCY = 100.0f; // given in [rad/s]
pmic 19:518ed284d98b 5 const float PositionController::MIN_DUTY_CYCLE = 0.01f; // minimum duty-cycle
pmic 19:518ed284d98b 6 const float PositionController::MAX_DUTY_CYCLE = 0.99f; // maximum duty-cycle
pmic 8:6b747ad59ff5 7
pmic 8:6b747ad59ff5 8 PositionController::PositionController(float counts_per_turn, float kn, float max_voltage, FastPWM& pwm, EncoderCounter& encoderCounter) : pwm(pwm), encoderCounter(encoderCounter), thread(osPriorityHigh, 4096)
pmic 8:6b747ad59ff5 9 {
pmic 8:6b747ad59ff5 10 this->counts_per_turn = counts_per_turn;
pmic 10:fe74e8909d3f 11 setFeedForwardGain(kn);
pmic 10:fe74e8909d3f 12 setSpeedCntrlGain(0.1f);
pmic 8:6b747ad59ff5 13 this->max_voltage = max_voltage;
pmic 8:6b747ad59ff5 14 this->max_speed = kn*max_voltage;
pmic 29:335fb9b01ca7 15 setPositionCntrlGain(22.0f);
pmic 10:fe74e8909d3f 16
pmic 10:fe74e8909d3f 17 // initialise pwm
pmic 10:fe74e8909d3f 18 pwm.period(0.00005); // pwm period of 50 us
pmic 10:fe74e8909d3f 19 pwm.write(0.5); // duty-cycle of 50%
pmic 10:fe74e8909d3f 20
pmic 10:fe74e8909d3f 21 // initialise
pmic 10:fe74e8909d3f 22 previousValueCounter = encoderCounter.read();
pmic 10:fe74e8909d3f 23 speedFilter.setPeriod(TS);
pmic 10:fe74e8909d3f 24 speedFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
pmic 10:fe74e8909d3f 25 desiredSpeed = 0.0f;
pmic 10:fe74e8909d3f 26 actualSpeed = 0.0f;
pmic 10:fe74e8909d3f 27 float initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 10:fe74e8909d3f 28 this->initialRotation = initialRotation;
pmic 10:fe74e8909d3f 29 actualRotation = initialRotation;
pmic 10:fe74e8909d3f 30 desiredRotation = initialRotation;
pmic 29:335fb9b01ca7 31
pmic 29:335fb9b01ca7 32 motion.setProfileVelocity(max_voltage * kn);
pmic 29:335fb9b01ca7 33 float maxAcceleration = 22.0f * max_voltage * kn * 0.4f; // pmic, 13.05.2022, only 40%, based on simple measurement, max_voltage * kn is gearratio
pmic 29:335fb9b01ca7 34 motion.setProfileAcceleration(maxAcceleration);
pmic 29:335fb9b01ca7 35 motion.setProfileDeceleration(maxAcceleration);
pmic 8:6b747ad59ff5 36
pmic 10:fe74e8909d3f 37 // set up thread
pmic 8:6b747ad59ff5 38 thread.start(callback(this, &PositionController::run));
pmic 10:fe74e8909d3f 39 ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 8:6b747ad59ff5 40 }
pmic 8:6b747ad59ff5 41
pmic 8:6b747ad59ff5 42 PositionController::~PositionController()
pmic 8:6b747ad59ff5 43 {
pmic 10:fe74e8909d3f 44 ticker.detach();
pmic 8:6b747ad59ff5 45 }
pmic 8:6b747ad59ff5 46
pmic 10:fe74e8909d3f 47 /**
pmic 10:fe74e8909d3f 48 * Reads the speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 49 * @return actual speed in RPM.
pmic 10:fe74e8909d3f 50 */
pmic 8:6b747ad59ff5 51 float PositionController::getSpeedRPM()
pmic 8:6b747ad59ff5 52 {
pmic 8:6b747ad59ff5 53 return actualSpeed;
pmic 8:6b747ad59ff5 54 }
pmic 8:6b747ad59ff5 55
pmic 10:fe74e8909d3f 56 /**
pmic 10:fe74e8909d3f 57 * Reads the speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 58 * @return actual speed in RPS.
pmic 10:fe74e8909d3f 59 */
pmic 8:6b747ad59ff5 60 float PositionController::getSpeedRPS()
pmic 8:6b747ad59ff5 61 {
pmic 8:6b747ad59ff5 62 return actualSpeed/60.0f;
pmic 8:6b747ad59ff5 63 }
pmic 8:6b747ad59ff5 64
pmic 10:fe74e8909d3f 65 /**
pmic 10:fe74e8909d3f 66 * Sets desired rotation (1 corresponds to 360 deg).
pmic 10:fe74e8909d3f 67 */
pmic 8:6b747ad59ff5 68 void PositionController::setDesiredRotation(float desiredRotation)
pmic 8:6b747ad59ff5 69 {
pmic 8:6b747ad59ff5 70 this->desiredRotation = initialRotation + desiredRotation;
pmic 8:6b747ad59ff5 71 }
pmic 8:6b747ad59ff5 72
pmic 10:fe74e8909d3f 73 /**
pmic 10:fe74e8909d3f 74 * Reads the number of rotations (1 corresponds to 360 deg).
pmic 10:fe74e8909d3f 75 * @return actual rotations.
pmic 10:fe74e8909d3f 76 */
pmic 8:6b747ad59ff5 77 float PositionController::getRotation()
pmic 8:6b747ad59ff5 78 {
pmic 8:6b747ad59ff5 79 return actualRotation - initialRotation;
pmic 8:6b747ad59ff5 80 }
pmic 8:6b747ad59ff5 81
pmic 10:fe74e8909d3f 82 /**
pmic 10:fe74e8909d3f 83 * Sets the feed-forward gain.
pmic 10:fe74e8909d3f 84 */
pmic 10:fe74e8909d3f 85 void PositionController::setFeedForwardGain(float kn)
pmic 10:fe74e8909d3f 86 {
pmic 10:fe74e8909d3f 87 this->kn = kn;
pmic 10:fe74e8909d3f 88 }
pmic 10:fe74e8909d3f 89
pmic 10:fe74e8909d3f 90 /**
pmic 10:fe74e8909d3f 91 * Sets the gain of the speed controller (p-controller).
pmic 10:fe74e8909d3f 92 */
pmic 10:fe74e8909d3f 93 void PositionController::setSpeedCntrlGain(float kp)
pmic 10:fe74e8909d3f 94 {
pmic 10:fe74e8909d3f 95 this->kp = kp;
pmic 10:fe74e8909d3f 96 }
pmic 10:fe74e8909d3f 97
pmic 10:fe74e8909d3f 98 /**
pmic 10:fe74e8909d3f 99 * Sets the gain of the position controller (p-controller).
pmic 10:fe74e8909d3f 100 */
pmic 10:fe74e8909d3f 101 void PositionController::setPositionCntrlGain(float p)
pmic 10:fe74e8909d3f 102 {
pmic 10:fe74e8909d3f 103 this->p = p;
pmic 10:fe74e8909d3f 104 }
pmic 10:fe74e8909d3f 105
pmic 29:335fb9b01ca7 106 /**
pmic 29:335fb9b01ca7 107 * Sets the maximum Velocity in RPS.
pmic 29:335fb9b01ca7 108 */
pmic 29:335fb9b01ca7 109 void PositionController::setMaxVelocityRPS(float maxVelocityRPS)
pmic 29:335fb9b01ca7 110 {
pmic 29:335fb9b01ca7 111 if (maxVelocityRPS*60.0f <= max_speed)
pmic 29:335fb9b01ca7 112 motion.setProfileVelocity(maxVelocityRPS*60.0f);
pmic 29:335fb9b01ca7 113 else
pmic 29:335fb9b01ca7 114 motion.setProfileVelocity(max_speed);
pmic 29:335fb9b01ca7 115 }
pmic 29:335fb9b01ca7 116
pmic 29:335fb9b01ca7 117 /**
pmic 29:335fb9b01ca7 118 * Sets the maximum Velocity in RPM.
pmic 29:335fb9b01ca7 119 */
pmic 29:335fb9b01ca7 120 void PositionController::setMaxVelocityRPM(float maxVelocityRPM)
pmic 29:335fb9b01ca7 121 {
pmic 29:335fb9b01ca7 122 if (maxVelocityRPM <= max_speed)
pmic 29:335fb9b01ca7 123 motion.setProfileVelocity(maxVelocityRPM);
pmic 29:335fb9b01ca7 124 else
pmic 29:335fb9b01ca7 125 motion.setProfileVelocity(max_speed);
pmic 29:335fb9b01ca7 126 }
pmic 29:335fb9b01ca7 127
pmic 29:335fb9b01ca7 128 /**
pmic 29:335fb9b01ca7 129 * Sets the maximum Acceleration in RPS/sec.
pmic 29:335fb9b01ca7 130 */
pmic 29:335fb9b01ca7 131 void PositionController::setMaxAccelerationRPS(float maxAccelerationRPS)
pmic 29:335fb9b01ca7 132 {
pmic 29:335fb9b01ca7 133 motion.setProfileAcceleration(maxAccelerationRPS*60.0f);
pmic 29:335fb9b01ca7 134 motion.setProfileDeceleration(maxAccelerationRPS*60.0f);
pmic 29:335fb9b01ca7 135 }
pmic 29:335fb9b01ca7 136
pmic 29:335fb9b01ca7 137 /**
pmic 29:335fb9b01ca7 138 * Sets the maximum Acceleration in RPM/sec.
pmic 29:335fb9b01ca7 139 */
pmic 29:335fb9b01ca7 140 void PositionController::setMaxAccelerationRPM(float maxAccelerationRPM)
pmic 29:335fb9b01ca7 141 {
pmic 29:335fb9b01ca7 142 motion.setProfileAcceleration(maxAccelerationRPM);
pmic 29:335fb9b01ca7 143 motion.setProfileDeceleration(maxAccelerationRPM);
pmic 29:335fb9b01ca7 144 }
pmic 29:335fb9b01ca7 145
pmic 8:6b747ad59ff5 146 void PositionController::run()
pmic 8:6b747ad59ff5 147 {
pmic 8:6b747ad59ff5 148 while(true) {
pmic 8:6b747ad59ff5 149 // wait for the periodic signal
pmic 8:6b747ad59ff5 150 ThisThread::flags_wait_any(threadFlag);
pmic 8:6b747ad59ff5 151
pmic 8:6b747ad59ff5 152 // calculate actual speed of motors in [rpm]
pmic 8:6b747ad59ff5 153 short valueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 154 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 8:6b747ad59ff5 155 previousValueCounter = valueCounter;
pmic 8:6b747ad59ff5 156 actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
pmic 8:6b747ad59ff5 157 actualRotation = actualRotation + actualSpeed/60.0f*TS;
pmic 8:6b747ad59ff5 158
pmic 29:335fb9b01ca7 159 motion.incrementToPosition(60.0f * desiredRotation, TS);
pmic 29:335fb9b01ca7 160 float desiredRotationMotion = motion.getPosition();
pmic 29:335fb9b01ca7 161 float desiredVelocityMotion = motion.getVelocity();
pmic 29:335fb9b01ca7 162
pmic 8:6b747ad59ff5 163 // calculate motor phase voltages
pmic 29:335fb9b01ca7 164 desiredSpeed = p *(desiredRotationMotion - 60.0f * actualRotation) + 0.0f * desiredVelocityMotion;
pmic 29:335fb9b01ca7 165
pmic 8:6b747ad59ff5 166 if (desiredSpeed < -max_speed) desiredSpeed = -max_speed;
pmic 8:6b747ad59ff5 167 else if (desiredSpeed > max_speed) desiredSpeed = max_speed;
pmic 29:335fb9b01ca7 168
pmic 8:6b747ad59ff5 169 float voltage = kp*(desiredSpeed - actualSpeed) + desiredSpeed/kn;
pmic 8:6b747ad59ff5 170 // calculate, limit and set duty cycles
pmic 8:6b747ad59ff5 171 float dutyCycle = 0.5f + 0.5f*voltage/max_voltage;
pmic 8:6b747ad59ff5 172 if (dutyCycle < MIN_DUTY_CYCLE) dutyCycle = MIN_DUTY_CYCLE;
pmic 8:6b747ad59ff5 173 else if (dutyCycle > MAX_DUTY_CYCLE) dutyCycle = MAX_DUTY_CYCLE;
pmic 8:6b747ad59ff5 174 pwm.write(static_cast<double>(dutyCycle));
pmic 8:6b747ad59ff5 175 }
pmic 8:6b747ad59ff5 176 }
pmic 8:6b747ad59ff5 177
pmic 8:6b747ad59ff5 178 void PositionController::sendThreadFlag()
pmic 8:6b747ad59ff5 179 {
pmic 8:6b747ad59ff5 180 thread.flags_set(threadFlag);
pmic 8:6b747ad59ff5 181 }