Margreeth de Breij / Mbed 2 deprecated frdm_MotorEMG

Motor programma met EMG

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of frdm_Motor_V2_3 by Margreeth de Breij

main.cpp@26:cd1db85359aa, 2015-10-05 (annotated)

Committer:
Margreeth95
Date:
Mon Oct 05 16:58:58 2015 +0000
Revision:
26:cd1db85359aa
Parent:
25:ae908de29943
Child:
27:3392f03bfada
2 motoren, switches tussen programma's. Opzet voor EMG in comments. Nog wel oscilaties

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;
Margreeth95 26:cd1db85359aa 35 Ticker myEMG1;
Margreeth95 26:cd1db85359aa 36 Ticker myEMG2;
Rvs94 25:ae908de29943 37
Rvs94 25:ae908de29943 38 // Constants
Rvs94 25:ae908de29943 39 double reference2, reference1;
Rvs94 25:ae908de29943 40 double position2 = 0, position1 = 0;
Rvs94 25:ae908de29943 41 double m2_ref = 0, m1_ref = 0;
Rvs94 25:ae908de29943 42 int count = 0;
Rvs94 25:ae908de29943 43 double Grens2 = 90, Grens1 = 90;
Rvs94 25:ae908de29943 44 double Stapgrootte = 5;
Margreeth95 26:cd1db85359aa 45 double Threshold;
Rvs94 2:099da0fc31b6 46
Rvs94 25:ae908de29943 47 //Sample time (motor-step)
Rvs94 25:ae908de29943 48 const double m2_Ts = 0.01, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 49
Rvs94 25:ae908de29943 50 //Controller gain Motor 2 & 1
Margreeth95 26:cd1db85359aa 51 const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
Margreeth95 26:cd1db85359aa 52 const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
Rvs94 25:ae908de29943 53 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 25:ae908de29943 54 double m1_err_int = 0, m1_prev_err = 0;
Rvs94 20:f5091e29cd26 55
Rvs94 25:ae908de29943 56 //Derivative filter coeffs Motor 2 & 1
Margreeth95 26:cd1db85359aa 57 const double BiGain2 = 0.012, BiGain1 = 0.016955;
Rvs94 25:ae908de29943 58 const double m2_f_a1 = -0.96608908283*BiGain2, m2_f_a2 = 0.0*BiGain2, m2_f_b0 = 1.0*BiGain2, m2_f_b1 = 1.0*BiGain2, m2_f_b2 = 0.0*BiGain2;
Rvs94 25:ae908de29943 59 const double m1_f_a1 = -0.96608908283*BiGain1, m1_f_a2 = 0.0*BiGain1, m1_f_b0 = 1.0*BiGain1, m1_f_b1 = 1.0*BiGain1, m1_f_b2 = 0.0*BiGain1;
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 20:f5091e29cd26 64
Rvs94 25:ae908de29943 65 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 66 // General Functions
Rvs94 25:ae908de29943 67 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 68
Rvs94 20:f5091e29cd26 69 //HIDScope
Rvs94 25:ae908de29943 70 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 71 {
Rvs94 25:ae908de29943 72 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 73 scope.set(1, position2);
Rvs94 25:ae908de29943 74 scope.set(2, reference1 - position1);
Rvs94 25:ae908de29943 75 scope.set(3, position1);
Rvs94 25:ae908de29943 76 scope.send();
Rvs94 1:48aba8d5610a 77
Rvs94 25:ae908de29943 78 }
Rvs94 12:69ab81cf5b7d 79
Rvs94 12:69ab81cf5b7d 80 // Biquad filter
Rvs94 25:ae908de29943 81 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 82 {
Rvs94 25:ae908de29943 83 double v = u - a1*v1 - a2*v2;
Rvs94 25:ae908de29943 84 double y = b0*v + b1*v1 + b2*v2;
Rvs94 25:ae908de29943 85 v2 = v1; v1 = v;
Rvs94 25:ae908de29943 86 return y;
Rvs94 25:ae908de29943 87 }
Rvs94 12:69ab81cf5b7d 88
Rvs94 12:69ab81cf5b7d 89
Rvs94 12:69ab81cf5b7d 90 // Reusable PID controller
Rvs94 25:ae908de29943 91 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 92 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 25:ae908de29943 93 {
Rvs94 12:69ab81cf5b7d 94 // Derivative
Rvs94 25:ae908de29943 95 double e_der = (e-e_prev)/Ts;
Rvs94 25:ae908de29943 96 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 25:ae908de29943 97 e_prev = e;
Rvs94 12:69ab81cf5b7d 98 // Integral
Rvs94 25:ae908de29943 99 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 100 // PID
Rvs94 25:ae908de29943 101 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 102 }
Margreeth95 26:cd1db85359aa 103
Margreeth95 26:cd1db85359aa 104 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 26:cd1db85359aa 105 //EMG functions
Margreeth95 26:cd1db85359aa 106 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 26:cd1db85359aa 107
Margreeth95 26:cd1db85359aa 108 // Hier komen functies die de EMG signalen uitlezen en filteren
Margreeth95 26:cd1db85359aa 109
Rvs94 25:ae908de29943 110 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 111 // Motor control functions
Rvs94 25:ae908de29943 112 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 113
Margreeth95 18:6f71bb91b8bd 114 // Motor2 control
Rvs94 25:ae908de29943 115 void motor2_Controller()
Rvs94 9:774fc3c6a39e 116 {
Rvs94 25:ae908de29943 117 // Setpoint motor 2
Rvs94 25:ae908de29943 118 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 119 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 120 // Speed control
Rvs94 25:ae908de29943 121 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 122 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 25:ae908de29943 123 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 124 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 125 // Direction control
Rvs94 25:ae908de29943 126 if(m2_P2 > 0)
Rvs94 25:ae908de29943 127 {
Rvs94 25:ae908de29943 128 motor2direction = 0;
Rvs94 25:ae908de29943 129 }
Rvs94 25:ae908de29943 130 else
Rvs94 25:ae908de29943 131 {
Rvs94 25:ae908de29943 132 motor2direction = 1;
Rvs94 25:ae908de29943 133 }
Rvs94 25:ae908de29943 134 }
Rvs94 25:ae908de29943 135
Rvs94 25:ae908de29943 136 // Motor1 control
Rvs94 25:ae908de29943 137 void motor1_Controller()
Rvs94 25:ae908de29943 138 {
Rvs94 25:ae908de29943 139 // Setpoint Motor 1
Rvs94 25:ae908de29943 140 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 141 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 142 // Speed control
Rvs94 25:ae908de29943 143 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 144 m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 145 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 146 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 147 // Direction control
Rvs94 25:ae908de29943 148 if(m1_P2 > 0)
Rvs94 25:ae908de29943 149 {
Rvs94 25:ae908de29943 150 motor1direction = 1;
Rvs94 25:ae908de29943 151 }
Rvs94 25:ae908de29943 152 else
Rvs94 25:ae908de29943 153 {
Rvs94 25:ae908de29943 154 motor1direction = 0;
Rvs94 25:ae908de29943 155 }
Rvs94 9:774fc3c6a39e 156 }
Rvs94 3:687729d7996e 157
Rvs94 25:ae908de29943 158 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 159 // Main function
Rvs94 25:ae908de29943 160 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 161 int main()
Rvs94 25:ae908de29943 162 {
Rvs94 25:ae908de29943 163 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 164 // Initalizing
Rvs94 25:ae908de29943 165 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 166 //LEDs OFF
Rvs94 25:ae908de29943 167 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 168
Rvs94 25:ae908de29943 169 //PC connection & check
Rvs94 25:ae908de29943 170 pc.baud(115200);
Rvs94 25:ae908de29943 171 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 172
Rvs94 25:ae908de29943 173 // Tickers
Rvs94 25:ae908de29943 174 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 25:ae908de29943 175 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 25:ae908de29943 176 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Margreeth95 26:cd1db85359aa 177 // 2 Tickers die de waarden van de EMG-signalen uitlezen
Rvs94 25:ae908de29943 178
Rvs94 25:ae908de29943 179 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 180 // Control Program
Rvs94 25:ae908de29943 181 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 182 while(true)
Rvs94 20:f5091e29cd26 183 {
Rvs94 20:f5091e29cd26 184 char c = pc.getc();
Rvs94 25:ae908de29943 185 // 1 Program UP
Margreeth95 26:cd1db85359aa 186 if(c == 'e') // If ((EMG1 => Threshold) && (EMG2 => Threshold))
Rvs94 20:f5091e29cd26 187 {
Rvs94 20:f5091e29cd26 188 count = count + 1;
Rvs94 20:f5091e29cd26 189 if(count > 2)
Rvs94 20:f5091e29cd26 190 {
Rvs94 20:f5091e29cd26 191 count = 2;
Rvs94 20:f5091e29cd26 192 }
Rvs94 20:f5091e29cd26 193
Rvs94 20:f5091e29cd26 194 }
Rvs94 25:ae908de29943 195 // 1 Program DOWN
Margreeth95 26:cd1db85359aa 196 if(c == 'd') // Hoe gaat dit aangestuurd worden?
Rvs94 20:f5091e29cd26 197 {
Rvs94 20:f5091e29cd26 198 count = count - 1;
Rvs94 20:f5091e29cd26 199 if(count < 0)
Rvs94 20:f5091e29cd26 200 {
Rvs94 20:f5091e29cd26 201 count = 0;
Rvs94 20:f5091e29cd26 202 }
Rvs94 25:ae908de29943 203 }
Rvs94 25:ae908de29943 204 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 205 if(count == 0)
Rvs94 20:f5091e29cd26 206 {
Rvs94 20:f5091e29cd26 207
Rvs94 20:f5091e29cd26 208 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 209 LedG = 0;
Margreeth95 26:cd1db85359aa 210 if(c == 'r') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
Margreeth95 19:9417d2011e8b 211 {
Rvs94 24:d0af4b2be295 212 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 213 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 214 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 215 {
Rvs94 24:d0af4b2be295 216 m2_ref = Grens2;
Rvs94 25:ae908de29943 217 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 218 }
Margreeth95 19:9417d2011e8b 219 }
Margreeth95 26:cd1db85359aa 220 if(c == 'f') // if ((EMG2 => Threshold) && (EMG1 =< Threshold))
Margreeth95 19:9417d2011e8b 221 {
Rvs94 24:d0af4b2be295 222 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 223 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 224 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 225 {
Rvs94 24:d0af4b2be295 226 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 227 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 228 }
Margreeth95 19:9417d2011e8b 229 }
Rvs94 20:f5091e29cd26 230 }
Rvs94 25:ae908de29943 231 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 232 if(count == 1)
Rvs94 20:f5091e29cd26 233 {
Rvs94 20:f5091e29cd26 234 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 235 LedR = 0;
Margreeth95 26:cd1db85359aa 236 if(c == 't') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
Rvs94 24:d0af4b2be295 237 {
Rvs94 24:d0af4b2be295 238 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 239 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 240 {
Rvs94 24:d0af4b2be295 241 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 242 }
Rvs94 24:d0af4b2be295 243 }
Margreeth95 26:cd1db85359aa 244 if(c == 'g') // if ((EMG1 => Threshold) && (EMG2 =< Threshold))
Rvs94 24:d0af4b2be295 245 {
Rvs94 24:d0af4b2be295 246 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 247 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 248 {
Rvs94 24:d0af4b2be295 249 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 250 }
Rvs94 24:d0af4b2be295 251 }
Rvs94 20:f5091e29cd26 252 }
Rvs94 25:ae908de29943 253 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 254 if(count == 2)
Rvs94 20:f5091e29cd26 255 {
Rvs94 20:f5091e29cd26 256
Rvs94 20:f5091e29cd26 257 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 258 LedB = 0;
Rvs94 25:ae908de29943 259 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 260 wait(1);
Rvs94 24:d0af4b2be295 261 m2_ref = 0;
Margreeth95 26:cd1db85359aa 262 m1_ref = 0;
Margreeth95 26:cd1db85359aa 263 count = 0;
Rvs94 20:f5091e29cd26 264 }
Margreeth95 0:284ed397e046 265 }
Rvs94 9:774fc3c6a39e 266
Margreeth95 0:284ed397e046 267 }