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@25:039e2b6429ad, 2018-11-02 (annotated)

Committer:
1856413
Date:
Fri Nov 02 11:26:18 2018 +0000
Revision:
25:039e2b6429ad
Parent:
24:f9dccbc1fc7e 
Use this script for DEMOMODE

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