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@22:2a560f0f1671, 2018-10-31 (annotated)

Committer:
nicollevanrijswijk
Date:
Wed Oct 31 19:42:55 2018 +0000
Revision:
22:2a560f0f1671
Parent:
21:9ba0ed42ee42
Child:
24:f9dccbc1fc7e
Toevoegen van Led om even te kijken of het nog werkt

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