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@27:4d7ca91e2e64, 2015-10-15 (annotated)

Committer:
Rvs94
Date:
Thu Oct 15 17:36:25 2015 +0000
Revision:
27:4d7ca91e2e64
Parent:
25:ae908de29943 
Motor werkt niet, snap niet waarom. Biquad toegevoegd op een andere manier en nu werkt er niks meer. Terug naar vorig script.

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 27:4d7ca91e2e64 48 const double m2_Kp = 0.1,m2_Ki = 0.001, m2_Kd = 1;
Rvs94 25:ae908de29943 49 const double m1_Kp = 5,m1_Ki = 0.05, m1_Kd = 2;
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 27:4d7ca91e2e64 54 const double BiGainm2f1 = 0.959332;
Rvs94 27:4d7ca91e2e64 55 const double m2_f1_a1 = -1.55576653052, m2_f1_a2 = 0.61374320375, m2_f1_b0 = 1.0*BiGainm2f1, m2_f1_b1 = -0.90928276835*BiGainm2f1, m2_f1_b2 = 1.0*BiGainm2f1;
Rvs94 27:4d7ca91e2e64 56
Rvs94 27:4d7ca91e2e64 57
Rvs94 27:4d7ca91e2e64 58 //Biquads
Rvs94 27:4d7ca91e2e64 59 biquadFilter f_Motor2 (m2_f1_a1, m2_f1_a2, m2_f1_b0, m2_f1_b1, m2_f1_b2); // creates the low pass filter
Rvs94 20:f5091e29cd26 60
Rvs94 20:f5091e29cd26 61 // Filter variables
Rvs94 25:ae908de29943 62 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 25:ae908de29943 63 double m1_f_v1 = 0, m1_f_v2 = 0;
Rvs94 27:4d7ca91e2e64 64 double m2_f2_v1 = 0, m2_f2_v2 = 0;
Rvs94 20:f5091e29cd26 65
Rvs94 25:ae908de29943 66 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 67 // General Functions
Rvs94 25:ae908de29943 68 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 69
Rvs94 20:f5091e29cd26 70 //HIDScope
Rvs94 25:ae908de29943 71 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 72 {
Rvs94 25:ae908de29943 73 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 74 scope.set(1, position2);
Rvs94 27:4d7ca91e2e64 75 scope.set(2, reference2);
Rvs94 27:4d7ca91e2e64 76 scope.set(3, position2);
Rvs94 25:ae908de29943 77 scope.send();
Rvs94 1:48aba8d5610a 78
Rvs94 27:4d7ca91e2e64 79 }
Rvs94 12:69ab81cf5b7d 80
Rvs94 12:69ab81cf5b7d 81 // Reusable PID controller
Rvs94 27:4d7ca91e2e64 82 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
Rvs94 25:ae908de29943 83 {
Rvs94 12:69ab81cf5b7d 84 // Derivative
Rvs94 25:ae908de29943 85 double e_der = (e-e_prev)/Ts;
Rvs94 27:4d7ca91e2e64 86 //e_der = f_motor2(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 25:ae908de29943 87 e_prev = e;
Rvs94 12:69ab81cf5b7d 88 // Integral
Rvs94 25:ae908de29943 89 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 90 // PID
Rvs94 25:ae908de29943 91 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 92 }
Rvs94 25:ae908de29943 93 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 94 // Motor control functions
Rvs94 25:ae908de29943 95 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 96
Margreeth95 18:6f71bb91b8bd 97 // Motor2 control
Rvs94 25:ae908de29943 98 void motor2_Controller()
Rvs94 9:774fc3c6a39e 99 {
Rvs94 25:ae908de29943 100 // Setpoint motor 2
Rvs94 25:ae908de29943 101 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 102 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 103 // Speed control
Rvs94 27:4d7ca91e2e64 104 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
Rvs94 27:4d7ca91e2e64 105 double m2_P2 = f_Motor2.step(m2_P1);//(m2_P1, m2_f2_v1, m2_f2_v2, m2_f2_a1, m2_f2_a2,m2_f2_b0, m2_f2_b1, m2_f2_b2); // Filter of motorspeed input
Rvs94 25:ae908de29943 106 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 107 // Direction control
Rvs94 25:ae908de29943 108 if(m2_P2 > 0)
Rvs94 25:ae908de29943 109 {
Rvs94 25:ae908de29943 110 motor2direction = 0;
Rvs94 25:ae908de29943 111 }
Rvs94 25:ae908de29943 112 else
Rvs94 25:ae908de29943 113 {
Rvs94 25:ae908de29943 114 motor2direction = 1;
Rvs94 25:ae908de29943 115 }
Rvs94 25:ae908de29943 116 }
Rvs94 25:ae908de29943 117
Rvs94 25:ae908de29943 118 // Motor1 control
Rvs94 25:ae908de29943 119 void motor1_Controller()
Rvs94 25:ae908de29943 120 {
Rvs94 27:4d7ca91e2e64 121
Rvs94 9:774fc3c6a39e 122 }
Rvs94 3:687729d7996e 123
Rvs94 25:ae908de29943 124 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 125 // Main function
Rvs94 25:ae908de29943 126 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 127 int main()
Rvs94 25:ae908de29943 128 {
Rvs94 25:ae908de29943 129 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 130 // Initalizing
Rvs94 25:ae908de29943 131 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 132 //LEDs OFF
Rvs94 25:ae908de29943 133 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 134
Rvs94 25:ae908de29943 135 //PC connection & check
Rvs94 25:ae908de29943 136 pc.baud(115200);
Rvs94 25:ae908de29943 137 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 138
Rvs94 25:ae908de29943 139 // Tickers
Rvs94 25:ae908de29943 140 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 25:ae908de29943 141 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 25:ae908de29943 142 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Rvs94 25:ae908de29943 143
Rvs94 25:ae908de29943 144 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 145 // Control Program
Rvs94 25:ae908de29943 146 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 147 while(true)
Rvs94 20:f5091e29cd26 148 {
Rvs94 20:f5091e29cd26 149 char c = pc.getc();
Rvs94 25:ae908de29943 150 // 1 Program UP
Rvs94 25:ae908de29943 151 if(c == 'e')
Rvs94 20:f5091e29cd26 152 {
Rvs94 20:f5091e29cd26 153 count = count + 1;
Rvs94 20:f5091e29cd26 154 if(count > 2)
Rvs94 20:f5091e29cd26 155 {
Rvs94 20:f5091e29cd26 156 count = 2;
Rvs94 20:f5091e29cd26 157 }
Rvs94 20:f5091e29cd26 158
Rvs94 20:f5091e29cd26 159 }
Rvs94 25:ae908de29943 160 // 1 Program DOWN
Rvs94 25:ae908de29943 161 if(c == 'd')
Rvs94 20:f5091e29cd26 162 {
Rvs94 20:f5091e29cd26 163 count = count - 1;
Rvs94 20:f5091e29cd26 164 if(count < 0)
Rvs94 20:f5091e29cd26 165 {
Rvs94 20:f5091e29cd26 166 count = 0;
Rvs94 20:f5091e29cd26 167 }
Rvs94 25:ae908de29943 168 }
Rvs94 25:ae908de29943 169 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 170 if(count == 0)
Rvs94 20:f5091e29cd26 171 {
Rvs94 20:f5091e29cd26 172
Rvs94 20:f5091e29cd26 173 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 174 LedG = 0;
Margreeth95 19:9417d2011e8b 175 if(c == 'r')
Margreeth95 19:9417d2011e8b 176 {
Rvs94 24:d0af4b2be295 177 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 178 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 179 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 180 {
Rvs94 24:d0af4b2be295 181 m2_ref = Grens2;
Rvs94 25:ae908de29943 182 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 183 }
Margreeth95 19:9417d2011e8b 184 }
Margreeth95 19:9417d2011e8b 185 if(c == 'f')
Margreeth95 19:9417d2011e8b 186 {
Rvs94 24:d0af4b2be295 187 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 188 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 189 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 190 {
Rvs94 24:d0af4b2be295 191 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 192 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 193 }
Margreeth95 19:9417d2011e8b 194 }
Rvs94 20:f5091e29cd26 195 }
Rvs94 25:ae908de29943 196 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 197 if(count == 1)
Rvs94 20:f5091e29cd26 198 {
Rvs94 20:f5091e29cd26 199 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 200 LedR = 0;
Rvs94 24:d0af4b2be295 201 if(c == 't')
Rvs94 24:d0af4b2be295 202 {
Rvs94 24:d0af4b2be295 203 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 204 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 205 {
Rvs94 24:d0af4b2be295 206 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 207 }
Rvs94 24:d0af4b2be295 208 }
Rvs94 24:d0af4b2be295 209 if(c == 'g')
Rvs94 24:d0af4b2be295 210 {
Rvs94 24:d0af4b2be295 211 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 212 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 213 {
Rvs94 24:d0af4b2be295 214 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 215 }
Rvs94 24:d0af4b2be295 216 }
Rvs94 20:f5091e29cd26 217 }
Rvs94 25:ae908de29943 218 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 219 if(count == 2)
Rvs94 20:f5091e29cd26 220 {
Rvs94 20:f5091e29cd26 221
Rvs94 20:f5091e29cd26 222 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 223 LedB = 0;
Rvs94 25:ae908de29943 224 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 225 wait(1);
Rvs94 24:d0af4b2be295 226 m2_ref = 0;
Rvs94 24:d0af4b2be295 227 m1_ref = 0;
Rvs94 20:f5091e29cd26 228 }
Margreeth95 0:284ed397e046 229 }
Rvs94 9:774fc3c6a39e 230
Margreeth95 0:284ed397e046 231 }