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@21:9ba0ed42ee42, 2018-10-31 (annotated)

Committer:
s1682253
Date:
Wed Oct 31 15:48:53 2018 +0000
Revision:
21:9ba0ed42ee42
Parent:
20:16373bb9af42
Child:
22:2a560f0f1671
Beide motoren werken met potmeter zoals we willen. Fysieke encoder pins omgedraaid.

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