BMT Module 9 Group 4 / Mbed 2 deprecated PES_tutorial_5

Motor control, feedback, PI controller, BiQuad filter

Dependencies:   FastPWM HIDScope MODSERIAL biquadFilter mbed QEI

main.cpp@24:f9dccbc1fc7e, 2018-10-31 (annotated)

Committer:
1856413
Date:
Wed Oct 31 21:11:28 2018 +0000
Revision:
24:f9dccbc1fc7e
Parent:
22:2a560f0f1671
Child:
25:039e2b6429ad
motor1Direction veranderd en de potmeter opnieuw gescaled

Who changed what in which revision?

UserRevisionLine numberNew contents of line
1856413 0:2e33035d4e86 1 #include "mbed.h"
1856413 20:16373bb9af42 2 #include "FastPWM.h"
1856413 2:34c14fb36b5d 3 #include "MODSERIAL.h"
lweersink 4:49c5fd62a192 4 #include "QEI.h"
1856413 12:1ecd11dc2c00 5 #include <math.h>
1856413 2:34c14fb36b5d 6 MODSERIAL pc(USBTX, USBRX);
1856413 13:0b51846cf9e3 7 DigitalOut motor1DirectionPin(D7);
1856413 20:16373bb9af42 8 DigitalOut motor2DirectionPin(D4);
nicollevanrijswijk 22:2a560f0f1671 9 DigitalOut Led(LED_GREEN);
1856413 20:16373bb9af42 10 FastPWM motor1MagnitudePin(D6);
1856413 20:16373bb9af42 11 FastPWM motor2MagnitudePin(D5);
1856413 8:ceb9abb5a4a8 12 AnalogIn potMeter1(A4);
1856413 12:1ecd11dc2c00 13 AnalogIn potMeter2(A5);
1856413 8:ceb9abb5a4a8 14 InterruptIn button2(D3);
1856413 20:16373bb9af42 15 QEI Encoder1 (D12, D13, NC, 64, QEI::X4_ENCODING);
1856413 20:16373bb9af42 16 QEI Encoder2 (D10, D11, NC, 64, QEI::X4_ENCODING);
1856413 12:1ecd11dc2c00 17
1856413 12:1ecd11dc2c00 18 //Tickers
1856413 20:16373bb9af42 19 Ticker MeasureControl1;
1856413 20:16373bb9af42 20 Ticker MeasureControl2;
lweersink 14:29236a33b5e4 21 Ticker print;
1856413 10:b002572e37fd 22
1856413 10:b002572e37fd 23 //Global variables
1856413 20:16373bb9af42 24 volatile double measuredPosition1 = 0.0;
1856413 20:16373bb9af42 25 volatile double measuredPosition2 = 0.0;
1856413 20:16373bb9af42 26 volatile double referencePosition1 = 0.0;
1856413 20:16373bb9af42 27 volatile double referencePosition2 = 0.0;
1856413 20:16373bb9af42 28 volatile double motorValue1 = 0.01;
1856413 20:16373bb9af42 29 volatile double motorValue2 = 0.01;
1856413 24:f9dccbc1fc7e 30 volatile double errorM1 = 0.0;
1856413 20:16373bb9af42 31 volatile double Kp = 0.34; //dit maken we variabel, dit zorgt voor een grote of kleine overshoot
1856413 20:16373bb9af42 32 volatile double Ki = 0.0; //dit moeten we bepalen met een plot bijvoorbeeld
lweersink 17:4a0912c93771 33 volatile double Kd = 0.0;
lweersink 15:c2cfab737a4c 34 volatile double Ts = 0.01;
nicollevanrijswijk 5:a1fb2d2fb2d0 35
1856413 13:0b51846cf9e3 36 //------------------------------------------------------------------------------
1856413 20:16373bb9af42 37
1856413 20:16373bb9af42 38 double GetReferencePosition1()
1856413 20:16373bb9af42 39 {
1856413 20:16373bb9af42 40 double potMeter1In = potMeter1.read();
1856413 24:f9dccbc1fc7e 41 referencePosition1 = 8.0*3.14*potMeter1In - 4.0*3.14 ; // Reference value y, scaled to -4 to 4 revolutions
1856413 20:16373bb9af42 42 return referencePosition1;
1856413 20:16373bb9af42 43 }
1856413 20:16373bb9af42 44
1856413 20:16373bb9af42 45 double GetReferencePosition2()
lweersink 19:1353ba4d94db 46 {
1856413 20:16373bb9af42 47 double potMeter2In = potMeter2.read();
1856413 24:f9dccbc1fc7e 48 referencePosition2 = 8.0*3.14*potMeter2In - 4.0*3.14 ;
1856413 20:16373bb9af42 49 return referencePosition2;
lweersink 19:1353ba4d94db 50 }
1856413 20:16373bb9af42 51
1856413 20:16373bb9af42 52 double GetMeasuredPosition1()
1856413 12:1ecd11dc2c00 53 {
1856413 20:16373bb9af42 54 int counts1 = Encoder1.getPulses();
1856413 20:16373bb9af42 55 double counts1d = counts1*1.0f;
1856413 20:16373bb9af42 56 measuredPosition1 = ( counts1d / (8400)) * 6.28; // Rotational position in radians
1856413 20:16373bb9af42 57 return measuredPosition1;
1856413 20:16373bb9af42 58 }
1856413 20:16373bb9af42 59
1856413 20:16373bb9af42 60 double GetMeasuredPosition2()
1856413 20:16373bb9af42 61 {
1856413 20:16373bb9af42 62 int counts2 = Encoder2.getPulses();
1856413 20:16373bb9af42 63 double counts2d = counts2*1.0f;
1856413 20:16373bb9af42 64 measuredPosition2 = ( counts2d / (8400)) * 6.28;
1856413 20:16373bb9af42 65 return measuredPosition2;
1856413 0:2e33035d4e86 66 }
nicollevanrijswijk 11:4e3ef6150a2e 67
1856413 20:16373bb9af42 68 double FeedbackControl1(double Error1)
nicollevanrijswijk 11:4e3ef6150a2e 69 {
1856413 20:16373bb9af42 70 static double Error_integral1 = 0;
1856413 20:16373bb9af42 71 static double Error_prev1 = Error1;
lweersink 17:4a0912c93771 72 //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
1856413 20:16373bb9af42 73 // Proportional part:
lweersink 17:4a0912c93771 74 //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
1856413 20:16373bb9af42 75 double u_k1 = Kp * Error1;
lweersink 15:c2cfab737a4c 76 // Integral part:
1856413 20:16373bb9af42 77 Error_integral1 = Error_integral1 + Error1 * Ts;
1856413 20:16373bb9af42 78 double u_i1 = Ki * Error_integral1;
lweersink 17:4a0912c93771 79 // Derivative part
1856413 20:16373bb9af42 80 double Error_derivative1 = (Error1 - Error_prev1)/Ts;
1856413 20:16373bb9af42 81 double u_d1 = Kd * Error_derivative1;
1856413 20:16373bb9af42 82 Error_prev1 = Error1;
1856413 20:16373bb9af42 83 // Sum all parts and return it
1856413 20:16373bb9af42 84 return u_k1 + u_i1 + u_d1; //motorValue
1856413 20:16373bb9af42 85 }
1856413 20:16373bb9af42 86
1856413 20:16373bb9af42 87 double FeedbackControl2(double Error2)
1856413 20:16373bb9af42 88 {
1856413 20:16373bb9af42 89 static double Error_integral2 = 0;
1856413 20:16373bb9af42 90 static double Error_prev2 = Error2;
1856413 20:16373bb9af42 91 //static BiQuad LowPassFilter(..., ..., ..., ..., ...)
1856413 20:16373bb9af42 92 // Proportional part:
1856413 20:16373bb9af42 93 //van 0 tot 20, waardes rond de 5 zijn het beste (minder overshoot + minder trilling motor beste combinatie hiervan)
1856413 20:16373bb9af42 94 double u_k2 = Kp * Error2;
1856413 20:16373bb9af42 95 // Integral part:
1856413 20:16373bb9af42 96 Error_integral2 = Error_integral2 + Error2 * Ts;
1856413 20:16373bb9af42 97 double u_i2 = Ki * Error_integral2;
1856413 20:16373bb9af42 98 // Derivative part
1856413 20:16373bb9af42 99 double Error_derivative2 = (Error2 - Error_prev2)/Ts;
1856413 20:16373bb9af42 100 double u_d2 = Kd * Error_derivative2;
1856413 20:16373bb9af42 101 Error_prev2 = Error2;
1856413 20:16373bb9af42 102 // Sum all parts and return it
1856413 20:16373bb9af42 103 return u_k2 + u_i2 + u_d2; //motorValue
1856413 20:16373bb9af42 104 }
1856413 20:16373bb9af42 105
1856413 20:16373bb9af42 106 void SetMotor1(double motorValue1)
1856413 12:1ecd11dc2c00 107 {
1856413 12:1ecd11dc2c00 108 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 12:1ecd11dc2c00 109 // bits for motor 1. Positive value makes motor rotating
1856413 12:1ecd11dc2c00 110 // clockwise. motorValues outside range are truncated to
1856413 12:1ecd11dc2c00 111 // within range
1856413 24:f9dccbc1fc7e 112 // 0 = clockwise motor direction
1856413 24:f9dccbc1fc7e 113 // 1 = counterclockwise motor direction
1856413 20:16373bb9af42 114 if (motorValue1 >=0) {
1856413 24:f9dccbc1fc7e 115 motor1DirectionPin=0;
1856413 24:f9dccbc1fc7e 116 } else {
1856413 12:1ecd11dc2c00 117 motor1DirectionPin=1;
1856413 12:1ecd11dc2c00 118 }
1856413 20:16373bb9af42 119 if (fabs(motorValue1)>1) {
1856413 12:1ecd11dc2c00 120 motor1MagnitudePin = 1;
1856413 20:16373bb9af42 121 } else {
1856413 20:16373bb9af42 122 motor1MagnitudePin = fabs(motorValue1);
1856413 12:1ecd11dc2c00 123 }
1856413 20:16373bb9af42 124 }
1856413 20:16373bb9af42 125
1856413 20:16373bb9af42 126 void SetMotor2(double motorValue2)
1856413 20:16373bb9af42 127 {
1856413 20:16373bb9af42 128 // Given -1<=motorValue<=1, this sets the PWM and direction
1856413 20:16373bb9af42 129 // bits for motor 1. Positive value makes motor rotating
1856413 20:16373bb9af42 130 // clockwise. motorValues outside range are truncated to
1856413 20:16373bb9af42 131 // within range
1856413 24:f9dccbc1fc7e 132 // 0 = counterclockwise motor direction
1856413 24:f9dccbc1fc7e 133 // 1 = clockwise motor direction
1856413 20:16373bb9af42 134 if (motorValue2 >=0) {
1856413 20:16373bb9af42 135 motor2DirectionPin=1;
1856413 20:16373bb9af42 136 } else {
1856413 20:16373bb9af42 137 motor2DirectionPin=0;
1856413 20:16373bb9af42 138 }
1856413 20:16373bb9af42 139 if (fabs(motorValue2)>1) {
1856413 20:16373bb9af42 140 motor2MagnitudePin = 1;
1856413 20:16373bb9af42 141 } else {
1856413 20:16373bb9af42 142 motor2MagnitudePin = fabs(motorValue2);
1856413 12:1ecd11dc2c00 143 }
1856413 12:1ecd11dc2c00 144 }
1856413 12:1ecd11dc2c00 145 //-----------------------------------------------------------------------------
lweersink 14:29236a33b5e4 146 // Tickers
1856413 20:16373bb9af42 147 void MeasureAndControl1(void)
1856413 12:1ecd11dc2c00 148 {
1856413 20:16373bb9af42 149 // This function determines the desired velocity, measures the
1856413 20:16373bb9af42 150 // actual velocity, and controls the motor with
1856413 12:1ecd11dc2c00 151 // a simple Feedback controller. Call this from a Ticker.
1856413 20:16373bb9af42 152 referencePosition1 = GetReferencePosition1();
1856413 20:16373bb9af42 153 measuredPosition1 = GetMeasuredPosition1();
1856413 24:f9dccbc1fc7e 154 errorM1 = referencePosition1 - measuredPosition1;
1856413 20:16373bb9af42 155 motorValue1 = FeedbackControl1(referencePosition1 - measuredPosition1);
1856413 20:16373bb9af42 156 SetMotor1(motorValue1);
1856413 13:0b51846cf9e3 157 }
1856413 12:1ecd11dc2c00 158
1856413 20:16373bb9af42 159 void MeasureAndControl2(void)
1856413 20:16373bb9af42 160 {
1856413 20:16373bb9af42 161 // This function determines the desired velocity, measures the
1856413 20:16373bb9af42 162 // actual velocity, and controls the motor with
1856413 20:16373bb9af42 163 // a simple Feedback controller. Call this from a Ticker.
1856413 20:16373bb9af42 164 referencePosition2 = GetReferencePosition2();
1856413 20:16373bb9af42 165 measuredPosition2 = GetMeasuredPosition2();
1856413 20:16373bb9af42 166 motorValue2 = FeedbackControl2(referencePosition2 - measuredPosition2);
1856413 20:16373bb9af42 167 SetMotor2(motorValue2);
1856413 20:16373bb9af42 168 }
lweersink 14:29236a33b5e4 169 void printen()
lweersink 14:29236a33b5e4 170 {
lweersink 14:29236a33b5e4 171 pc.baud (115200);
1856413 20:16373bb9af42 172 pc.printf("Referenceposition POT1 = %f \t Referenceposition POT2 = %f \r\n", referencePosition1, referencePosition2);
1856413 24:f9dccbc1fc7e 173 pc.printf("Measured position 1 = %f \t Measured position 2 = %f \r\n", measuredPosition1, measuredPosition2);
1856413 24:f9dccbc1fc7e 174 pc.printf("Error M1 = %f \r\n", errorM1);
1856413 24:f9dccbc1fc7e 175 pc.printf("Motorvalue M1 = %f \r\n", motorValue1);
1856413 20:16373bb9af42 176 //pc.printf("Proportional gain %f \r\n", Kp);
1856413 20:16373bb9af42 177 //pc.printf("Integral gain %f \r\n", Ki);
1856413 20:16373bb9af42 178 //pc.printf("Derivative gain %f \r\n", Kd);
lweersink 14:29236a33b5e4 179 }
1856413 12:1ecd11dc2c00 180 //-----------------------------------------------------------------------------
1856413 0:2e33035d4e86 181 int main()
1856413 0:2e33035d4e86 182 {
1856413 12:1ecd11dc2c00 183 //Initialize once
1856413 6:bd73804c8cec 184 pc.baud(115200);
1856413 13:0b51846cf9e3 185 motor1MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz.
1856413 20:16373bb9af42 186 motor2MagnitudePin.period_us(60.0); // 60 microseconds PWM period: 16.7 kHz.
1856413 20:16373bb9af42 187
1856413 20:16373bb9af42 188 MeasureControl1.attach(MeasureAndControl1, 0.01);
1856413 20:16373bb9af42 189 MeasureControl2.attach(MeasureAndControl2, 0.01);
nicollevanrijswijk 22:2a560f0f1671 190 print.attach(printen, 1.0);
1856413 20:16373bb9af42 191
1856413 12:1ecd11dc2c00 192 //Other initializations
1856413 20:16373bb9af42 193
1856413 20:16373bb9af42 194 while(true) {
nicollevanrijswijk 22:2a560f0f1671 195 Led = !Led;
nicollevanrijswijk 22:2a560f0f1671 196 wait(2);
nicollevanrijswijk 11:4e3ef6150a2e 197 }
nicollevanrijswijk 11:4e3ef6150a2e 198 }