Michael Ernst Peter / PM2_Libary

Dependencies:   LSM9DS1 RangeFinder FastPWM

Dependents:   PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board PM2_Example_PES_board ... more

Committer:
pmic
Date:
Sat May 14 08:19:21 2022 +0000
Revision:
30:05abc1d2a2b9
Parent:
29:335fb9b01ca7
Updated PM2_Libary

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 30:05abc1d2a2b9 27 this->initialRotation = (float)encoderCounter.read()/counts_per_turn;
pmic 10:fe74e8909d3f 28 actualRotation = initialRotation;
pmic 10:fe74e8909d3f 29 desiredRotation = initialRotation;
pmic 29:335fb9b01ca7 30
pmic 29:335fb9b01ca7 31 motion.setProfileVelocity(max_voltage * kn);
pmic 29:335fb9b01ca7 32 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 33 motion.setProfileAcceleration(maxAcceleration);
pmic 29:335fb9b01ca7 34 motion.setProfileDeceleration(maxAcceleration);
pmic 8:6b747ad59ff5 35
pmic 10:fe74e8909d3f 36 // set up thread
pmic 8:6b747ad59ff5 37 thread.start(callback(this, &PositionController::run));
pmic 10:fe74e8909d3f 38 ticker.attach(callback(this, &PositionController::sendThreadFlag), std::chrono::microseconds{static_cast<long int>(1.0e6f * TS)});
pmic 8:6b747ad59ff5 39 }
pmic 8:6b747ad59ff5 40
pmic 8:6b747ad59ff5 41 PositionController::~PositionController()
pmic 8:6b747ad59ff5 42 {
pmic 10:fe74e8909d3f 43 ticker.detach();
pmic 8:6b747ad59ff5 44 }
pmic 8:6b747ad59ff5 45
pmic 10:fe74e8909d3f 46 /**
pmic 10:fe74e8909d3f 47 * Reads the speed in RPM (rotations per minute).
pmic 10:fe74e8909d3f 48 * @return actual speed in RPM.
pmic 10:fe74e8909d3f 49 */
pmic 8:6b747ad59ff5 50 float PositionController::getSpeedRPM()
pmic 8:6b747ad59ff5 51 {
pmic 8:6b747ad59ff5 52 return actualSpeed;
pmic 8:6b747ad59ff5 53 }
pmic 8:6b747ad59ff5 54
pmic 10:fe74e8909d3f 55 /**
pmic 10:fe74e8909d3f 56 * Reads the speed in RPS (rotations per second).
pmic 10:fe74e8909d3f 57 * @return actual speed in RPS.
pmic 10:fe74e8909d3f 58 */
pmic 8:6b747ad59ff5 59 float PositionController::getSpeedRPS()
pmic 8:6b747ad59ff5 60 {
pmic 8:6b747ad59ff5 61 return actualSpeed/60.0f;
pmic 8:6b747ad59ff5 62 }
pmic 8:6b747ad59ff5 63
pmic 10:fe74e8909d3f 64 /**
pmic 10:fe74e8909d3f 65 * Sets desired rotation (1 corresponds to 360 deg).
pmic 10:fe74e8909d3f 66 */
pmic 8:6b747ad59ff5 67 void PositionController::setDesiredRotation(float desiredRotation)
pmic 8:6b747ad59ff5 68 {
pmic 8:6b747ad59ff5 69 this->desiredRotation = initialRotation + desiredRotation;
pmic 8:6b747ad59ff5 70 }
pmic 8:6b747ad59ff5 71
pmic 10:fe74e8909d3f 72 /**
pmic 10:fe74e8909d3f 73 * Reads the number of rotations (1 corresponds to 360 deg).
pmic 10:fe74e8909d3f 74 * @return actual rotations.
pmic 10:fe74e8909d3f 75 */
pmic 8:6b747ad59ff5 76 float PositionController::getRotation()
pmic 8:6b747ad59ff5 77 {
pmic 8:6b747ad59ff5 78 return actualRotation - initialRotation;
pmic 8:6b747ad59ff5 79 }
pmic 8:6b747ad59ff5 80
pmic 10:fe74e8909d3f 81 /**
pmic 10:fe74e8909d3f 82 * Sets the feed-forward gain.
pmic 10:fe74e8909d3f 83 */
pmic 10:fe74e8909d3f 84 void PositionController::setFeedForwardGain(float kn)
pmic 10:fe74e8909d3f 85 {
pmic 10:fe74e8909d3f 86 this->kn = kn;
pmic 10:fe74e8909d3f 87 }
pmic 10:fe74e8909d3f 88
pmic 10:fe74e8909d3f 89 /**
pmic 10:fe74e8909d3f 90 * Sets the gain of the speed controller (p-controller).
pmic 10:fe74e8909d3f 91 */
pmic 10:fe74e8909d3f 92 void PositionController::setSpeedCntrlGain(float kp)
pmic 10:fe74e8909d3f 93 {
pmic 10:fe74e8909d3f 94 this->kp = kp;
pmic 10:fe74e8909d3f 95 }
pmic 10:fe74e8909d3f 96
pmic 10:fe74e8909d3f 97 /**
pmic 10:fe74e8909d3f 98 * Sets the gain of the position controller (p-controller).
pmic 10:fe74e8909d3f 99 */
pmic 10:fe74e8909d3f 100 void PositionController::setPositionCntrlGain(float p)
pmic 10:fe74e8909d3f 101 {
pmic 10:fe74e8909d3f 102 this->p = p;
pmic 10:fe74e8909d3f 103 }
pmic 10:fe74e8909d3f 104
pmic 29:335fb9b01ca7 105 /**
pmic 29:335fb9b01ca7 106 * Sets the maximum Velocity in RPS.
pmic 29:335fb9b01ca7 107 */
pmic 29:335fb9b01ca7 108 void PositionController::setMaxVelocityRPS(float maxVelocityRPS)
pmic 29:335fb9b01ca7 109 {
pmic 29:335fb9b01ca7 110 if (maxVelocityRPS*60.0f <= max_speed)
pmic 29:335fb9b01ca7 111 motion.setProfileVelocity(maxVelocityRPS*60.0f);
pmic 29:335fb9b01ca7 112 else
pmic 29:335fb9b01ca7 113 motion.setProfileVelocity(max_speed);
pmic 29:335fb9b01ca7 114 }
pmic 29:335fb9b01ca7 115
pmic 29:335fb9b01ca7 116 /**
pmic 29:335fb9b01ca7 117 * Sets the maximum Velocity in RPM.
pmic 29:335fb9b01ca7 118 */
pmic 29:335fb9b01ca7 119 void PositionController::setMaxVelocityRPM(float maxVelocityRPM)
pmic 29:335fb9b01ca7 120 {
pmic 29:335fb9b01ca7 121 if (maxVelocityRPM <= max_speed)
pmic 29:335fb9b01ca7 122 motion.setProfileVelocity(maxVelocityRPM);
pmic 29:335fb9b01ca7 123 else
pmic 29:335fb9b01ca7 124 motion.setProfileVelocity(max_speed);
pmic 29:335fb9b01ca7 125 }
pmic 29:335fb9b01ca7 126
pmic 29:335fb9b01ca7 127 /**
pmic 29:335fb9b01ca7 128 * Sets the maximum Acceleration in RPS/sec.
pmic 29:335fb9b01ca7 129 */
pmic 29:335fb9b01ca7 130 void PositionController::setMaxAccelerationRPS(float maxAccelerationRPS)
pmic 29:335fb9b01ca7 131 {
pmic 29:335fb9b01ca7 132 motion.setProfileAcceleration(maxAccelerationRPS*60.0f);
pmic 29:335fb9b01ca7 133 motion.setProfileDeceleration(maxAccelerationRPS*60.0f);
pmic 29:335fb9b01ca7 134 }
pmic 29:335fb9b01ca7 135
pmic 29:335fb9b01ca7 136 /**
pmic 29:335fb9b01ca7 137 * Sets the maximum Acceleration in RPM/sec.
pmic 29:335fb9b01ca7 138 */
pmic 29:335fb9b01ca7 139 void PositionController::setMaxAccelerationRPM(float maxAccelerationRPM)
pmic 29:335fb9b01ca7 140 {
pmic 29:335fb9b01ca7 141 motion.setProfileAcceleration(maxAccelerationRPM);
pmic 29:335fb9b01ca7 142 motion.setProfileDeceleration(maxAccelerationRPM);
pmic 29:335fb9b01ca7 143 }
pmic 29:335fb9b01ca7 144
pmic 8:6b747ad59ff5 145 void PositionController::run()
pmic 8:6b747ad59ff5 146 {
pmic 8:6b747ad59ff5 147 while(true) {
pmic 8:6b747ad59ff5 148 // wait for the periodic signal
pmic 8:6b747ad59ff5 149 ThisThread::flags_wait_any(threadFlag);
pmic 8:6b747ad59ff5 150
pmic 8:6b747ad59ff5 151 // calculate actual speed of motors in [rpm]
pmic 8:6b747ad59ff5 152 short valueCounter = encoderCounter.read();
pmic 8:6b747ad59ff5 153 short countsInPastPeriod = valueCounter - previousValueCounter;
pmic 8:6b747ad59ff5 154 previousValueCounter = valueCounter;
pmic 8:6b747ad59ff5 155 actualSpeed = speedFilter.filter((float)countsInPastPeriod/counts_per_turn/TS*60.0f);
pmic 8:6b747ad59ff5 156 actualRotation = actualRotation + actualSpeed/60.0f*TS;
pmic 8:6b747ad59ff5 157
pmic 30:05abc1d2a2b9 158 // trajectory needs to be calculated in rotations*60 so that all units are in RPM
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 30:05abc1d2a2b9 164 desiredSpeed = p *(desiredRotationMotion - 60.0f * actualRotation) + 0.0f * desiredVelocityMotion; // using velocity feedforward here will lead to overshoot
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 }