BMT M9 groep 7 / Mbed 2 deprecated frdm_Motor_V2_3

Werkend aansturingsscript voor 2 motoren, incl werkende program switch. Motoren oscilleren nog iets. Vuur mechanisme ontbreekt nog.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Robert Schulte

main.cpp@26:58c3d24b29d2, 2015-10-14 (annotated)

Committer:
Rvs94
Date:
Wed Oct 14 10:27:54 2015 +0000
Revision:
26:58c3d24b29d2
Parent:
25:ae908de29943
Child:
28:2eb768b9cb3b
Gevarieerd met coeffs en filters, maar systeem reageert niet zoals t hoort. Terug naar oude setting

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Rvs94 25:ae908de29943 1 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 2 // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
Rvs94 25:ae908de29943 3 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 4 // Libraries
Rvs94 25:ae908de29943 5 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 6 #include "mbed.h"
Margreeth95 0:284ed397e046 7 #include "MODSERIAL.h"
Margreeth95 0:284ed397e046 8 #include "HIDScope.h"
Margreeth95 0:284ed397e046 9 #include "QEI.h"
Rvs94 12:69ab81cf5b7d 10 #include "biquadFilter.h"
Rvs94 25:ae908de29943 11
Rvs94 25:ae908de29943 12 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 13 // Constanten/Inputs/Outputs
Rvs94 25:ae908de29943 14 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 15 MODSERIAL pc(USBTX, USBRX); // To/From PC
Rvs94 25:ae908de29943 16 QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2
Rvs94 25:ae908de29943 17 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1
Rvs94 25:ae908de29943 18 HIDScope scope(4); // Scope, 4 channels
Rvs94 20:f5091e29cd26 19
Rvs94 25:ae908de29943 20 // LEDs
Rvs94 25:ae908de29943 21 DigitalOut LedR(LED_RED);
Rvs94 25:ae908de29943 22 DigitalOut LedG(LED_GREEN);
Rvs94 25:ae908de29943 23 DigitalOut LedB(LED_BLUE);
Rvs94 20:f5091e29cd26 24
Rvs94 25:ae908de29943 25 // Motor
Rvs94 25:ae908de29943 26 DigitalOut motor1direction(D7); // Motor 1, Direction & Speed
Rvs94 25:ae908de29943 27 PwmOut motor1speed(D6);
Rvs94 25:ae908de29943 28 DigitalOut motor2direction(D4); // Motor 2, Direction & Speed
Rvs94 25:ae908de29943 29 PwmOut motor2speed(D5);
Rvs94 7:67b50d4fb03c 30
Rvs94 25:ae908de29943 31 // Tickers
Rvs94 25:ae908de29943 32 Ticker ScopeTime;
Rvs94 25:ae908de29943 33 Ticker myControllerTicker2;
Rvs94 25:ae908de29943 34 Ticker myControllerTicker1;
Rvs94 25:ae908de29943 35
Rvs94 25:ae908de29943 36 // Constants
Rvs94 25:ae908de29943 37 double reference2, reference1;
Rvs94 25:ae908de29943 38 double position2 = 0, position1 = 0;
Rvs94 25:ae908de29943 39 double m2_ref = 0, m1_ref = 0;
Rvs94 25:ae908de29943 40 int count = 0;
Rvs94 25:ae908de29943 41 double Grens2 = 90, Grens1 = 90;
Rvs94 25:ae908de29943 42 double Stapgrootte = 5;
Rvs94 2:099da0fc31b6 43
Rvs94 25:ae908de29943 44 //Sample time (motor-step)
Rvs94 25:ae908de29943 45 const double m2_Ts = 0.01, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 46
Rvs94 25:ae908de29943 47 //Controller gain Motor 2 & 1
Rvs94 26:58c3d24b29d2 48 const double m2_Kp = 5,m2_Ki = 0.05, m2_Kd = 5;
Rvs94 26:58c3d24b29d2 49 const double m1_Kp = 5,m1_Ki = 0, m1_Kd = 1;
Rvs94 25:ae908de29943 50 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 25:ae908de29943 51 double m1_err_int = 0, m1_prev_err = 0;
Rvs94 20:f5091e29cd26 52
Rvs94 25:ae908de29943 53 //Derivative filter coeffs Motor 2 & 1
Rvs94 26:58c3d24b29d2 54 const double BiGain2 = 0.010681;
Rvs94 26:58c3d24b29d2 55 const double m2_f_a1 = -1.78114551442*BiGain2, m2_f_a2 = 0.79497571585*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = -0.64542853817*BiGain2, m2_f_b2 = 1.0*BiGain2;
Rvs94 26:58c3d24b29d2 56 const double m1_f_a1 = m2_f_a1, m1_f_a2 = m2_f_a2, m1_f_b0 = m2_f_b0, m1_f_b1 = m2_f_b1, m1_f_b2 = m2_f_b2;
Rvs94 20:f5091e29cd26 57
Rvs94 20:f5091e29cd26 58 // Filter variables
Rvs94 25:ae908de29943 59 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 25:ae908de29943 60 double m1_f_v1 = 0, m1_f_v2 = 0;
Rvs94 20:f5091e29cd26 61
Rvs94 25:ae908de29943 62 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 63 // General Functions
Rvs94 25:ae908de29943 64 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 65
Rvs94 20:f5091e29cd26 66 //HIDScope
Rvs94 25:ae908de29943 67 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 68 {
Rvs94 25:ae908de29943 69 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 70 scope.set(1, position2);
Rvs94 25:ae908de29943 71 scope.set(2, reference1 - position1);
Rvs94 25:ae908de29943 72 scope.set(3, position1);
Rvs94 25:ae908de29943 73 scope.send();
Rvs94 1:48aba8d5610a 74
Rvs94 25:ae908de29943 75 }
Rvs94 12:69ab81cf5b7d 76
Rvs94 12:69ab81cf5b7d 77 // Biquad filter
Rvs94 25:ae908de29943 78 double biquad( double u, double &v1, double &v2, const double a1, const double a2, const double b0, const double b1, const double b2 )
Rvs94 25:ae908de29943 79 {
Rvs94 25:ae908de29943 80 double v = u - a1*v1 - a2*v2;
Rvs94 25:ae908de29943 81 double y = b0*v + b1*v1 + b2*v2;
Rvs94 25:ae908de29943 82 v2 = v1; v1 = v;
Rvs94 25:ae908de29943 83 return y;
Rvs94 25:ae908de29943 84 }
Rvs94 12:69ab81cf5b7d 85
Rvs94 12:69ab81cf5b7d 86
Rvs94 12:69ab81cf5b7d 87 // Reusable PID controller
Rvs94 25:ae908de29943 88 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2,
Rvs94 25:ae908de29943 89 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 25:ae908de29943 90 {
Rvs94 12:69ab81cf5b7d 91 // Derivative
Rvs94 25:ae908de29943 92 double e_der = (e-e_prev)/Ts;
Rvs94 25:ae908de29943 93 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 25:ae908de29943 94 e_prev = e;
Rvs94 12:69ab81cf5b7d 95 // Integral
Rvs94 25:ae908de29943 96 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 97 // PID
Rvs94 25:ae908de29943 98 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 99 }
Rvs94 25:ae908de29943 100 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 101 // Motor control functions
Rvs94 25:ae908de29943 102 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 103
Margreeth95 18:6f71bb91b8bd 104 // Motor2 control
Rvs94 25:ae908de29943 105 void motor2_Controller()
Rvs94 9:774fc3c6a39e 106 {
Rvs94 25:ae908de29943 107 // Setpoint motor 2
Rvs94 25:ae908de29943 108 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 109 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 110 // Speed control
Rvs94 25:ae908de29943 111 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,
Rvs94 25:ae908de29943 112 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 25:ae908de29943 113 double m2_P2 = biquad(m2_P1, m2_f_v1, m2_f_v2, m2_f_a1, m2_f_a2,m2_f_b0, m2_f_b1, m2_f_b2); // Filter of motorspeed input
Rvs94 25:ae908de29943 114 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 115 // Direction control
Rvs94 25:ae908de29943 116 if(m2_P2 > 0)
Rvs94 25:ae908de29943 117 {
Rvs94 25:ae908de29943 118 motor2direction = 0;
Rvs94 25:ae908de29943 119 }
Rvs94 25:ae908de29943 120 else
Rvs94 25:ae908de29943 121 {
Rvs94 25:ae908de29943 122 motor2direction = 1;
Rvs94 25:ae908de29943 123 }
Rvs94 25:ae908de29943 124 }
Rvs94 25:ae908de29943 125
Rvs94 25:ae908de29943 126 // Motor1 control
Rvs94 25:ae908de29943 127 void motor1_Controller()
Rvs94 25:ae908de29943 128 {
Rvs94 25:ae908de29943 129 // Setpoint Motor 1
Rvs94 25:ae908de29943 130 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 131 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 132 // Speed control
Rvs94 25:ae908de29943 133 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2,
Rvs94 25:ae908de29943 134 m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 135 double m1_P2 = biquad(m1_P1, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 136 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 137 // Direction control
Rvs94 25:ae908de29943 138 if(m1_P2 > 0)
Rvs94 25:ae908de29943 139 {
Rvs94 25:ae908de29943 140 motor1direction = 1;
Rvs94 25:ae908de29943 141 }
Rvs94 25:ae908de29943 142 else
Rvs94 25:ae908de29943 143 {
Rvs94 25:ae908de29943 144 motor1direction = 0;
Rvs94 25:ae908de29943 145 }
Rvs94 9:774fc3c6a39e 146 }
Rvs94 3:687729d7996e 147
Rvs94 25:ae908de29943 148 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 149 // Main function
Rvs94 25:ae908de29943 150 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 151 int main()
Rvs94 25:ae908de29943 152 {
Rvs94 25:ae908de29943 153 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 154 // Initalizing
Rvs94 25:ae908de29943 155 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 156 //LEDs OFF
Rvs94 25:ae908de29943 157 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 158
Rvs94 25:ae908de29943 159 //PC connection & check
Rvs94 25:ae908de29943 160 pc.baud(115200);
Rvs94 25:ae908de29943 161 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 162
Rvs94 25:ae908de29943 163 // Tickers
Rvs94 25:ae908de29943 164 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 25:ae908de29943 165 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 25:ae908de29943 166 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Rvs94 25:ae908de29943 167
Rvs94 25:ae908de29943 168 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 169 // Control Program
Rvs94 25:ae908de29943 170 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 171 while(true)
Rvs94 20:f5091e29cd26 172 {
Rvs94 20:f5091e29cd26 173 char c = pc.getc();
Rvs94 25:ae908de29943 174 // 1 Program UP
Rvs94 25:ae908de29943 175 if(c == 'e')
Rvs94 20:f5091e29cd26 176 {
Rvs94 20:f5091e29cd26 177 count = count + 1;
Rvs94 20:f5091e29cd26 178 if(count > 2)
Rvs94 20:f5091e29cd26 179 {
Rvs94 20:f5091e29cd26 180 count = 2;
Rvs94 20:f5091e29cd26 181 }
Rvs94 20:f5091e29cd26 182
Rvs94 20:f5091e29cd26 183 }
Rvs94 25:ae908de29943 184 // 1 Program DOWN
Rvs94 25:ae908de29943 185 if(c == 'd')
Rvs94 20:f5091e29cd26 186 {
Rvs94 20:f5091e29cd26 187 count = count - 1;
Rvs94 20:f5091e29cd26 188 if(count < 0)
Rvs94 20:f5091e29cd26 189 {
Rvs94 20:f5091e29cd26 190 count = 0;
Rvs94 20:f5091e29cd26 191 }
Rvs94 25:ae908de29943 192 }
Rvs94 25:ae908de29943 193 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 194 if(count == 0)
Rvs94 20:f5091e29cd26 195 {
Rvs94 20:f5091e29cd26 196
Rvs94 20:f5091e29cd26 197 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 198 LedG = 0;
Margreeth95 19:9417d2011e8b 199 if(c == 'r')
Margreeth95 19:9417d2011e8b 200 {
Rvs94 24:d0af4b2be295 201 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 202 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 203 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 204 {
Rvs94 24:d0af4b2be295 205 m2_ref = Grens2;
Rvs94 25:ae908de29943 206 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 207 }
Margreeth95 19:9417d2011e8b 208 }
Margreeth95 19:9417d2011e8b 209 if(c == 'f')
Margreeth95 19:9417d2011e8b 210 {
Rvs94 24:d0af4b2be295 211 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 212 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 213 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 214 {
Rvs94 24:d0af4b2be295 215 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 216 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 217 }
Margreeth95 19:9417d2011e8b 218 }
Rvs94 20:f5091e29cd26 219 }
Rvs94 25:ae908de29943 220 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 221 if(count == 1)
Rvs94 20:f5091e29cd26 222 {
Rvs94 20:f5091e29cd26 223 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 224 LedR = 0;
Rvs94 24:d0af4b2be295 225 if(c == 't')
Rvs94 24:d0af4b2be295 226 {
Rvs94 24:d0af4b2be295 227 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 228 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 229 {
Rvs94 24:d0af4b2be295 230 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 231 }
Rvs94 24:d0af4b2be295 232 }
Rvs94 24:d0af4b2be295 233 if(c == 'g')
Rvs94 24:d0af4b2be295 234 {
Rvs94 24:d0af4b2be295 235 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 236 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 237 {
Rvs94 24:d0af4b2be295 238 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 239 }
Rvs94 24:d0af4b2be295 240 }
Rvs94 20:f5091e29cd26 241 }
Rvs94 25:ae908de29943 242 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 243 if(count == 2)
Rvs94 20:f5091e29cd26 244 {
Rvs94 20:f5091e29cd26 245
Rvs94 20:f5091e29cd26 246 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 247 LedB = 0;
Rvs94 25:ae908de29943 248 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 249 wait(1);
Rvs94 24:d0af4b2be295 250 m2_ref = 0;
Rvs94 24:d0af4b2be295 251 m1_ref = 0;
Rvs94 20:f5091e29cd26 252 }
Margreeth95 0:284ed397e046 253 }
Rvs94 9:774fc3c6a39e 254
Margreeth95 0:284ed397e046 255 }