Margreeth de Breij / Mbed 2 deprecated Motor_EMG_Definitief

Het complete motorscript met alle gewenste functies

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of Motor_EMG_FinalV1_aangepastePIDwaarden by BMT M9 groep 7

main.cpp@10:5c5f2ea11045, 2015-10-30 (annotated)

Committer:
Margreeth95
Date:
Fri Oct 30 09:17:33 2015 +0000
Revision:
10:5c5f2ea11045
Parent:
9:350a82a7745a 
Definitieve code, laatste functie ingevoegd voor knipperen lampje

Who changed what in which revision?

UserRevisionLine numberNew contents of line
laura94 0:5816557b2064 1 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 2 // Motorscript voor 2 motoren voor de "SJOEL ROBOT", Groep 7
laura94 0:5816557b2064 3 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 4 // Libraries
laura94 0:5816557b2064 5 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 6 #include "mbed.h"
laura94 0:5816557b2064 7 #include "MODSERIAL.h"
laura94 0:5816557b2064 8 #include "HIDScope.h"
laura94 0:5816557b2064 9 #include "QEI.h"
laura94 0:5816557b2064 10 #include "biquadFilter.h"
laura94 0:5816557b2064 11
laura94 0:5816557b2064 12 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 13 // Constanten/Inputs/Outputs
laura94 0:5816557b2064 14 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 15 MODSERIAL pc(USBTX, USBRX); // To/From PC
laura94 0:5816557b2064 16 QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2
laura94 0:5816557b2064 17 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1
Rvs94 1:3f49c8818619 18 HIDScope scope(6); // Scope, 4 channels
laura94 0:5816557b2064 19
laura94 0:5816557b2064 20 // LEDs
laura94 0:5816557b2064 21 DigitalOut LedR(LED_RED);
laura94 0:5816557b2064 22 DigitalOut LedG(LED_GREEN);
laura94 0:5816557b2064 23 DigitalOut LedB(LED_BLUE);
laura94 0:5816557b2064 24
laura94 0:5816557b2064 25 // Motor
laura94 0:5816557b2064 26 DigitalOut motor1direction(D7); // Motor 1, Direction & Speed
laura94 0:5816557b2064 27 PwmOut motor1speed(D6);
laura94 0:5816557b2064 28 DigitalOut motor2direction(D4); // Motor 2, Direction & Speed
laura94 0:5816557b2064 29 PwmOut motor2speed(D5);
laura94 0:5816557b2064 30
laura94 0:5816557b2064 31 //EMG
laura94 0:5816557b2064 32 AnalogIn EMG_left(A0); //Analog input
laura94 0:5816557b2064 33 AnalogIn EMG_right(A1);
laura94 0:5816557b2064 34
laura94 0:5816557b2064 35 // Tickers
laura94 0:5816557b2064 36 Ticker ScopeTime;
laura94 0:5816557b2064 37 Ticker myControllerTicker2;
laura94 0:5816557b2064 38 Ticker myControllerTicker1;
laura94 0:5816557b2064 39 Ticker SampleEMGLeft;
laura94 0:5816557b2064 40 Ticker SampleEMGRight;
laura94 0:5816557b2064 41 Ticker ScopeTimer;
laura94 0:5816557b2064 42 Ticker serial;
laura94 0:5816557b2064 43 Ticker MovingAverageLeft;
laura94 0:5816557b2064 44 Ticker MovingAverageRight;
laura94 0:5816557b2064 45
laura94 0:5816557b2064 46 // Constants
laura94 0:5816557b2064 47 double reference2, reference1;
laura94 0:5816557b2064 48 double position2 = 0, position1 = 0;
laura94 0:5816557b2064 49 double m2_ref = 0, m1_ref = 0;
laura94 0:5816557b2064 50 int count = 0;
Margreeth95 9:350a82a7745a 51 double Grens2 = 60, Grens1 = 60;
Rvs94 4:92a3b38a5302 52 double Stapgrootte = 1;
laura94 0:5816557b2064 53
laura94 0:5816557b2064 54 // Declaring variables
laura94 0:5816557b2064 55 double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
laura94 0:5816557b2064 56 double EMG_L_fh=0;
laura94 0:5816557b2064 57 double EMG_left_value;
laura94 0:5816557b2064 58 double EMG_left_f1;
laura94 0:5816557b2064 59 double EMG_left_f2;
laura94 0:5816557b2064 60 double EMG_left_f3;
laura94 0:5816557b2064 61 double EMG_left_abs;
laura94 0:5816557b2064 62
laura94 0:5816557b2064 63 double EMG_right_value;
laura94 0:5816557b2064 64 double EMG_right_f1;
laura94 0:5816557b2064 65 double EMG_right_f2;
laura94 0:5816557b2064 66 double EMG_right_f3;
laura94 0:5816557b2064 67 double EMG_right_abs;
laura94 0:5816557b2064 68 double Threshold1;
laura94 0:5816557b2064 69 double Threshold3;
laura94 0:5816557b2064 70
laura94 0:5816557b2064 71 int N = 50;
laura94 0:5816557b2064 72 double MAF_left[50];
laura94 0:5816557b2064 73 double EMG_left_MAF;
laura94 0:5816557b2064 74 double MAF_right[50];
laura94 0:5816557b2064 75 double EMG_right_MAF;
laura94 0:5816557b2064 76
laura94 0:5816557b2064 77
laura94 0:5816557b2064 78 //Sample time (motor-step)
Rvs94 2:7873c44b0568 79 const double m2_Ts = 0.001, m1_Ts = 0.001;
laura94 0:5816557b2064 80
laura94 0:5816557b2064 81 //Controller gain Motor 2 & 1
laura94 6:d1d747050fcc 82 const double m2_Kp = 0.1523/5,m2_Ki = 0.08585/5, m2_Kd = 0.004534/5;
laura94 6:d1d747050fcc 83 const double m1_Kp = 0.1523/5,m1_Ki = 0.08585/5, m1_Kd = 0.004534/5;
laura94 0:5816557b2064 84 double m2_err_int = 0, m2_prev_err = 0;
laura94 0:5816557b2064 85 double m1_err_int = 0, m1_prev_err = 0;
laura94 0:5816557b2064 86
laura94 0:5816557b2064 87 // coëfficiënten
Rvs94 1:3f49c8818619 88 const double BiGainEMG_H1 = 0.796821;
laura94 0:5816557b2064 89 const double EMGH1_a1 = -1.47500228332, EMGH1_a2 = 0.55273994299, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99922446977*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
laura94 0:5816557b2064 90
laura94 0:5816557b2064 91 const double BiGainEMG_L1= 0.001041;
laura94 0:5816557b2064 92 const double EMGL1_a1 = -1.87506717001, EMGL1_a2 = 0.87923101463, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = 2.00000000000*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
laura94 0:5816557b2064 93
laura94 0:5816557b2064 94 const double BiGainEMG_N1 = 1.0;
laura94 0:5816557b2064 95 const double EMGN1_a1 = -1.58174308681, EMGN1_a2 = 0.96540248979, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61816176147*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
laura94 0:5816557b2064 96
laura94 0:5816557b2064 97 // Creating the filters
laura94 0:5816557b2064 98 biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
laura94 0:5816557b2064 99 biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
laura94 0:5816557b2064 100 biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
laura94 0:5816557b2064 101
laura94 0:5816557b2064 102 biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
laura94 0:5816557b2064 103 biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
laura94 0:5816557b2064 104 biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
laura94 0:5816557b2064 105
laura94 0:5816557b2064 106
laura94 0:5816557b2064 107 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 108 // General Functions
laura94 0:5816557b2064 109 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 110
Margreeth95 10:5c5f2ea11045 111 //Blinking led
Margreeth95 10:5c5f2ea11045 112 void BlinkLed(DigitalOut LED, int n, float Time)
Margreeth95 10:5c5f2ea11045 113 {
Margreeth95 10:5c5f2ea11045 114 for(int i=1; i<=n;i++)
Margreeth95 10:5c5f2ea11045 115 {
Margreeth95 10:5c5f2ea11045 116 LED.write(0);
Margreeth95 10:5c5f2ea11045 117 wait(Time);
Margreeth95 10:5c5f2ea11045 118 LED.write(1);
Margreeth95 10:5c5f2ea11045 119 wait(Time);
Margreeth95 10:5c5f2ea11045 120 }
Margreeth95 10:5c5f2ea11045 121 LED.write(1);
Margreeth95 10:5c5f2ea11045 122 }
Margreeth95 10:5c5f2ea11045 123
laura94 0:5816557b2064 124 //HIDScope
laura94 0:5816557b2064 125 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
laura94 0:5816557b2064 126 {
Rvs94 1:3f49c8818619 127 scope.set(0, position2);
Rvs94 1:3f49c8818619 128 scope.set(1, position1);
Rvs94 1:3f49c8818619 129 scope.set(2, EMG_left_MAF);
Rvs94 1:3f49c8818619 130 scope.set(3, EMG_right_MAF);
Rvs94 1:3f49c8818619 131 scope.set(4, count);
laura94 0:5816557b2064 132 scope.send();
laura94 0:5816557b2064 133
laura94 0:5816557b2064 134 }
laura94 0:5816557b2064 135
laura94 0:5816557b2064 136 // Reusable PID controller
laura94 0:5816557b2064 137 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
laura94 0:5816557b2064 138 {
laura94 0:5816557b2064 139 // Derivative
laura94 0:5816557b2064 140 double e_der = (e-e_prev)/Ts;
laura94 0:5816557b2064 141 e_prev = e;
laura94 0:5816557b2064 142 // Integral
laura94 0:5816557b2064 143 e_int = e_int + Ts*e;
laura94 0:5816557b2064 144 // PID
laura94 0:5816557b2064 145 return Kp * e + Ki*e_int + Kd*e_der;
laura94 0:5816557b2064 146 }
laura94 0:5816557b2064 147
laura94 0:5816557b2064 148 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 149 //EMG functions
laura94 0:5816557b2064 150 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 151
laura94 0:5816557b2064 152 // EMG filtering function
laura94 0:5816557b2064 153 void EMGfilterLeft()
laura94 0:5816557b2064 154 {
laura94 0:5816557b2064 155 EMG_left_value = EMG_left.read();
laura94 0:5816557b2064 156 EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
laura94 0:5816557b2064 157 EMG_left_abs = fabs(EMG_left_f1);
laura94 0:5816557b2064 158 EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs);
laura94 0:5816557b2064 159 EMG_left_f3 = EMG_notch1.step(EMG_left_f2);
laura94 0:5816557b2064 160
laura94 0:5816557b2064 161 }
laura94 0:5816557b2064 162
laura94 0:5816557b2064 163 void EMGfilterRight()
laura94 0:5816557b2064 164 {
laura94 0:5816557b2064 165 EMG_right_value = EMG_right.read();
laura94 0:5816557b2064 166 EMG_right_f1 = EMG_highpass1R.step(EMG_right_value);
laura94 0:5816557b2064 167 EMG_right_abs = fabs(EMG_right_f1);
laura94 0:5816557b2064 168 EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs);
laura94 0:5816557b2064 169 EMG_right_f3 = EMG_notch1R.step(EMG_right_f2);
laura94 0:5816557b2064 170 }
laura94 0:5816557b2064 171
laura94 0:5816557b2064 172 // Movingaverage Filter
laura94 0:5816557b2064 173 void MovingAverageFilterLeft()
laura94 0:5816557b2064 174 {
laura94 0:5816557b2064 175 EMG_left_MAF = (MAF_left[0]+MAF_left[1]+MAF_left[2]+MAF_left[3]+MAF_left[4]+MAF_left[5]+MAF_left[6]+MAF_left[7]+MAF_left[8]+MAF_left[9]+MAF_left[10]+MAF_left[11]+MAF_left[12]+MAF_left[13]+MAF_left[14]+MAF_left[15]+MAF_left[16]+MAF_left[17]+MAF_left[18]+MAF_left[19]+MAF_left[20]+MAF_left[21]+MAF_left[22]+MAF_left[23]+MAF_left[24]+MAF_left[25]+MAF_left[26]+MAF_left[27]+MAF_left[28]+MAF_left[29]+MAF_left[30]+MAF_left[31]+MAF_left[32]+MAF_left[33]+MAF_left[34]+MAF_left[35]+MAF_left[36]+MAF_left[37]+MAF_left[38]+MAF_left[39]+MAF_left[40]+MAF_left[41]+MAF_left[42]+MAF_left[43]+MAF_left[44]+MAF_left[45]+MAF_left[46]+MAF_left[47]+MAF_left[48]+MAF_left[49])/N;
laura94 0:5816557b2064 176 MAF_left[49] = MAF_left[48], MAF_left[48] = MAF_left[47], MAF_left[47] = MAF_left[46], MAF_left[46] = MAF_left[45], MAF_left[45] = MAF_left[44], MAF_left[44] = MAF_left[43], MAF_left[43] = MAF_left[42], MAF_left[42] = MAF_left[41], MAF_left[41] = MAF_left[40], MAF_left[40] = MAF_left[39], MAF_left[39] = MAF_left[38], MAF_left[38] = MAF_left[37], MAF_left[37] = MAF_left[36], MAF_left[36] = MAF_left[35], MAF_left[35] = MAF_left[34], MAF_left[34] = MAF_left[33], MAF_left[33] = MAF_left[32], MAF_left[32] = MAF_left[31], MAF_left[31] = MAF_left[30], MAF_left[30] = MAF_left[29], MAF_left[29] = MAF_left[28], MAF_left[28] = MAF_left[27], MAF_left[27] = MAF_left[26], MAF_left[26] = MAF_left[25];
laura94 0:5816557b2064 177 MAF_left[25] = MAF_left[24], MAF_left[24] = MAF_left[23], MAF_left[23] = MAF_left[22], MAF_left[22] = MAF_left[21], MAF_left[21] = MAF_left[20], MAF_left[20] = MAF_left[19], MAF_left[19] = MAF_left[18], MAF_left[18] = MAF_left[17], MAF_left[17] = MAF_left[16], MAF_left[16] = MAF_left[15], MAF_left[15] = MAF_left[14], MAF_left[14] = MAF_left[13], MAF_left[13] = MAF_left[12], MAF_left[12] = MAF_left[11], MAF_left[11] = MAF_left[10], MAF_left[10] = MAF_left[9], MAF_left[9] = MAF_left[8], MAF_left[8] = MAF_left[7], MAF_left[7] = MAF_left[6], MAF_left[6] = MAF_left[5], MAF_left[5] = MAF_left[4], MAF_left[4] = MAF_left[3], MAF_left[3] = MAF_left[2], MAF_left[2] = MAF_left[1], MAF_left[1] = MAF_left[0];
laura94 0:5816557b2064 178 MAF_left[0] = EMG_left_f3;
laura94 0:5816557b2064 179 }
laura94 0:5816557b2064 180
laura94 0:5816557b2064 181 void MovingAverageFilterRight()
laura94 0:5816557b2064 182 {
laura94 0:5816557b2064 183 EMG_right_MAF = (MAF_right[0]+MAF_right[1]+MAF_right[2]+MAF_right[3]+MAF_right[4]+MAF_right[5]+MAF_right[6]+MAF_right[7]+MAF_right[8]+MAF_right[9]+MAF_right[10]+MAF_right[11]+MAF_right[12]+MAF_right[13]+MAF_right[14]+MAF_right[15]+MAF_right[16]+MAF_right[17]+MAF_right[18]+MAF_right[19]+MAF_right[20]+MAF_right[21]+MAF_right[22]+MAF_right[23]+MAF_right[24]+MAF_right[25]+MAF_right[26]+MAF_right[27]+MAF_right[28]+MAF_right[29]+MAF_right[30]+MAF_right[31]+MAF_right[32]+MAF_right[33]+MAF_right[34]+MAF_right[35]+MAF_right[36]+MAF_right[37]+MAF_right[38]+MAF_right[39]+MAF_right[40]+MAF_right[41]+MAF_right[42]+MAF_right[43]+MAF_right[44]+MAF_right[45]+MAF_right[46]+MAF_right[47]+MAF_right[48]+MAF_right[49])/N;
laura94 0:5816557b2064 184 MAF_right[49] = MAF_right[48], MAF_right[48] = MAF_right[47], MAF_right[47] = MAF_right[46], MAF_right[46] = MAF_right[45], MAF_right[45] = MAF_right[44], MAF_right[44] = MAF_right[43], MAF_right[43] = MAF_right[42], MAF_right[42] = MAF_right[41], MAF_right[41] = MAF_right[40], MAF_right[40] = MAF_right[39], MAF_right[39] = MAF_right[38], MAF_right[38] = MAF_right[37], MAF_right[37] = MAF_right[36], MAF_right[36] = MAF_right[35], MAF_right[35] = MAF_right[34], MAF_right[34] = MAF_right[33], MAF_right[33] = MAF_right[32], MAF_right[32] = MAF_right[31], MAF_right[31] = MAF_right[30], MAF_right[30] = MAF_right[29], MAF_right[29] = MAF_right[28], MAF_right[28] = MAF_right[27], MAF_right[27] = MAF_right[26], MAF_right[26] = MAF_right[25];
laura94 0:5816557b2064 185 MAF_right[25] = MAF_right[24], MAF_right[24] = MAF_right[23], MAF_right[23] = MAF_right[22], MAF_right[22] = MAF_right[21], MAF_right[21] = MAF_right[20], MAF_right[20] = MAF_right[19], MAF_right[19] = MAF_right[18], MAF_right[18] = MAF_right[17], MAF_right[17] = MAF_right[16], MAF_right[16] = MAF_right[15], MAF_right[15] = MAF_right[14], MAF_right[14] = MAF_right[13], MAF_right[13] = MAF_right[12], MAF_right[12] = MAF_right[11], MAF_right[11] = MAF_right[10], MAF_right[10] = MAF_right[9], MAF_right[9] = MAF_right[8], MAF_right[8] = MAF_right[7], MAF_right[7] = MAF_right[6], MAF_right[6] = MAF_right[5], MAF_right[5] = MAF_right[4], MAF_right[4] = MAF_right[3], MAF_right[3] = MAF_right[2], MAF_right[2] = MAF_right[1], MAF_right[1] = MAF_right[0];
laura94 0:5816557b2064 186 MAF_right[0] = EMG_right_f3;
laura94 0:5816557b2064 187 }
laura94 0:5816557b2064 188
laura94 0:5816557b2064 189 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 190 // Motor control functions
laura94 0:5816557b2064 191 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 192
laura94 0:5816557b2064 193 // Motor2 control
laura94 0:5816557b2064 194 void motor2_Controller()
laura94 0:5816557b2064 195 {
laura94 0:5816557b2064 196 // Setpoint motor 2
laura94 0:5816557b2064 197 reference2 = m2_ref; // Reference in degrees
Rvs94 1:3f49c8818619 198 position2 = 360.0*Encoder2.getPulses()/(32.0*131.0); // Position in degrees
laura94 0:5816557b2064 199 // Speed control
laura94 0:5816557b2064 200 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
laura94 0:5816557b2064 201 double m2_P2 = m2_P1;
laura94 0:5816557b2064 202 motor2speed = abs(m2_P2);
laura94 0:5816557b2064 203 // Direction control
laura94 0:5816557b2064 204 if(m2_P2 > 0)
laura94 0:5816557b2064 205 {
laura94 0:5816557b2064 206 motor2direction = 0;
laura94 0:5816557b2064 207 }
laura94 0:5816557b2064 208 else
laura94 0:5816557b2064 209 {
laura94 0:5816557b2064 210 motor2direction = 1;
laura94 0:5816557b2064 211 }
laura94 0:5816557b2064 212 }
laura94 0:5816557b2064 213
laura94 0:5816557b2064 214 // Motor1 control
laura94 0:5816557b2064 215 void motor1_Controller()
laura94 0:5816557b2064 216 {
laura94 0:5816557b2064 217 // Setpoint Motor 1
laura94 0:5816557b2064 218 reference1 = m1_ref; // Reference in degrees
Rvs94 1:3f49c8818619 219 position1 = 360.0*Encoder1.getPulses()/(32.0*131.0); // Position in degrees
laura94 0:5816557b2064 220 // Speed control
laura94 0:5816557b2064 221 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err);
laura94 0:5816557b2064 222 double m1_P2 = m1_P1;
laura94 0:5816557b2064 223 motor1speed = abs(m1_P2);
laura94 0:5816557b2064 224 // Direction control
laura94 0:5816557b2064 225 if(m1_P2 > 0)
laura94 0:5816557b2064 226 {
laura94 0:5816557b2064 227 motor1direction = 1;
laura94 0:5816557b2064 228 }
laura94 0:5816557b2064 229 else
laura94 0:5816557b2064 230 {
laura94 0:5816557b2064 231 motor1direction = 0;
laura94 0:5816557b2064 232 }
laura94 0:5816557b2064 233 }
laura94 0:5816557b2064 234
laura94 0:5816557b2064 235 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 236 // Main function
laura94 0:5816557b2064 237 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 238 int main()
laura94 0:5816557b2064 239 {
laura94 0:5816557b2064 240 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 241 // Initalizing
laura94 0:5816557b2064 242 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 243 //LEDs OFF
laura94 0:5816557b2064 244 LedR = LedB = LedG = 1;
laura94 0:5816557b2064 245
laura94 0:5816557b2064 246 //PC connection & check
laura94 0:5816557b2064 247 pc.baud(115200);
laura94 0:5816557b2064 248 pc.printf("Tot aan loop werkt\n");
laura94 0:5816557b2064 249
laura94 0:5816557b2064 250 // Tickers
laura94 0:5816557b2064 251 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 2:7873c44b0568 252 myControllerTicker2.attach(&motor2_Controller, m2_Ts ); // 100 Hz, Motor 2
Rvs94 2:7873c44b0568 253 myControllerTicker1.attach(&motor1_Controller, m1_Ts ); // 100 Hz, Motor 1
laura94 0:5816557b2064 254 SampleEMGLeft.attach(&EMGfilterLeft, 0.01f);
laura94 0:5816557b2064 255 SampleEMGRight.attach(&EMGfilterRight, 0.01f);
laura94 0:5816557b2064 256 MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.01f);
laura94 0:5816557b2064 257 MovingAverageRight.attach(&MovingAverageFilterRight, 0.01f);
Rvs94 1:3f49c8818619 258 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 1:3f49c8818619 259 // Determing Threshold
Rvs94 1:3f49c8818619 260 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 261 wait(20);
laura94 0:5816557b2064 262
Margreeth95 10:5c5f2ea11045 263 BlinkLed(LedG, 2, 0.2); //bepaling threshold linkerarm
Rvs94 1:3f49c8818619 264 LedG.write(0);
Rvs94 1:3f49c8818619 265 wait(2);
Rvs94 7:111e13da9bc0 266 Threshold1 = 0.5*EMG_left_MAF;
Rvs94 1:3f49c8818619 267 LedG.write(1);
Margreeth95 10:5c5f2ea11045 268
Margreeth95 10:5c5f2ea11045 269 BlinkLed(LedR, 1, 2);
Rvs94 1:3f49c8818619 270
Margreeth95 10:5c5f2ea11045 271 BlinkLed(LedB, 2, 0.2); //bepaling threshold rechterarm
Rvs94 1:3f49c8818619 272 LedB.write(0);
Rvs94 1:3f49c8818619 273 wait(2);
Rvs94 7:111e13da9bc0 274 Threshold3 = 0.5*EMG_right_MAF;
Rvs94 1:3f49c8818619 275 LedB.write(1);
Margreeth95 10:5c5f2ea11045 276
Margreeth95 10:5c5f2ea11045 277 pc.printf("Threshold links = %f, Threshold rechts = %f/n", Threshold1, Threshold3);
Margreeth95 10:5c5f2ea11045 278 BlinkLed(LedR, 1, 2);
Margreeth95 10:5c5f2ea11045 279 LedR.write(1);
laura94 0:5816557b2064 280 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 281 // Control Program
laura94 0:5816557b2064 282 //--------------------------------------------------------------------------------------------------------------------------//
laura94 0:5816557b2064 283 while(true)
laura94 0:5816557b2064 284 {
laura94 0:5816557b2064 285 //char c = pc.getc();
laura94 0:5816557b2064 286 // 1 Program UP
Rvs94 1:3f49c8818619 287 if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
laura94 0:5816557b2064 288 {
laura94 0:5816557b2064 289 count = count + 1;
laura94 0:5816557b2064 290 if(count > 2)
laura94 0:5816557b2064 291 {
laura94 0:5816557b2064 292 count = 2;
laura94 0:5816557b2064 293 }
Rvs94 1:3f49c8818619 294 wait(2);
laura94 0:5816557b2064 295 }
laura94 0:5816557b2064 296 // 1 Program DOWN
laura94 0:5816557b2064 297 // if(c == 'd') // Hoe gaat dit aangestuurd worden?
laura94 0:5816557b2064 298 // {
laura94 0:5816557b2064 299 // count = count - 1;
laura94 0:5816557b2064 300 // if(count < 0)
laura94 0:5816557b2064 301 // {
laura94 0:5816557b2064 302 // count = 0;
laura94 0:5816557b2064 303 // }
laura94 0:5816557b2064 304 // }
laura94 0:5816557b2064 305 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
laura94 0:5816557b2064 306 if(count == 0)
laura94 0:5816557b2064 307 {
laura94 0:5816557b2064 308 LedR = LedB = 1;
laura94 0:5816557b2064 309 LedG = 0;
Margreeth95 10:5c5f2ea11045 310
Rvs94 3:86ca263a83dc 311 if ((EMG_right_MAF >= Threshold3) && (EMG_left_MAF <= Threshold1)) //if(c == 'r') //
laura94 0:5816557b2064 312 {
laura94 0:5816557b2064 313 m2_ref = m2_ref + Stapgrootte;
Rvs94 4:92a3b38a5302 314 m1_ref = m1_ref + Stapgrootte;
laura94 0:5816557b2064 315 if (m2_ref > Grens2)
laura94 0:5816557b2064 316 {
laura94 0:5816557b2064 317 m2_ref = Grens2;
Rvs94 4:92a3b38a5302 318 m1_ref = Grens1;
laura94 0:5816557b2064 319 }
Rvs94 4:92a3b38a5302 320 wait(0.05);
laura94 0:5816557b2064 321 }
Rvs94 3:86ca263a83dc 322 if((EMG_right_MAF < Threshold3) && (EMG_left_MAF > Threshold1)) //if (c == 'f') //
laura94 0:5816557b2064 323 {
laura94 0:5816557b2064 324 m2_ref = m2_ref - Stapgrootte;
Rvs94 4:92a3b38a5302 325 m1_ref = m1_ref - Stapgrootte;
laura94 0:5816557b2064 326 if (m2_ref < -1*Grens2)
laura94 0:5816557b2064 327 {
laura94 0:5816557b2064 328 m2_ref = -1*Grens2;
Rvs94 4:92a3b38a5302 329 m1_ref = -1*Grens1;
laura94 0:5816557b2064 330 }
Rvs94 4:92a3b38a5302 331 wait(0.05);
laura94 0:5816557b2064 332 }
Rvs94 1:3f49c8818619 333 }
laura94 0:5816557b2064 334 // PROGRAM 1: Motor 1 control, Red LED
laura94 0:5816557b2064 335 if(count == 1)
laura94 0:5816557b2064 336 {
laura94 0:5816557b2064 337 LedG = LedB = 1;
laura94 0:5816557b2064 338 LedR = 0;
Rvs94 7:111e13da9bc0 339 //Linker grenzen
Rvs94 8:86a13efe98c8 340 if ((EMG_right_MAF <= Threshold3) && (EMG_left_MAF >= Threshold1) && (m2_ref < 0)) // if(c == 't') // Draait naar links
laura94 0:5816557b2064 341 {
laura94 0:5816557b2064 342 m1_ref = m1_ref + Stapgrootte;
Rvs94 1:3f49c8818619 343
Margreeth95 9:350a82a7745a 344 if (m1_ref > m2_ref + 35 )
laura94 0:5816557b2064 345 {
Margreeth95 9:350a82a7745a 346 m1_ref = m2_ref + 35 ;
laura94 0:5816557b2064 347 }
Rvs94 4:92a3b38a5302 348 wait(0.05);
laura94 0:5816557b2064 349 }
Rvs94 8:86a13efe98c8 350 if ((EMG_left_MAF < Threshold1) && (EMG_right_MAF > Threshold3) && (m2_ref < 0)) //if(c == 'g') // Draait naar rechts
Rvs94 7:111e13da9bc0 351 {
Rvs94 7:111e13da9bc0 352 m1_ref = m1_ref - Stapgrootte;
Rvs94 7:111e13da9bc0 353
Margreeth95 9:350a82a7745a 354 if (m1_ref < -1*Grens1 )
Rvs94 7:111e13da9bc0 355 {
Margreeth95 9:350a82a7745a 356 m1_ref = -1*Grens1;
Rvs94 7:111e13da9bc0 357 }
Rvs94 7:111e13da9bc0 358 wait(0.05);
Rvs94 7:111e13da9bc0 359 }
Rvs94 7:111e13da9bc0 360 //Rechter grenzen
Rvs94 8:86a13efe98c8 361 if ((EMG_right_MAF <= Threshold3) && (EMG_left_MAF >= Threshold1) && (m2_ref >= 0)) // if(c == 't') // Draait naar links
Rvs94 8:86a13efe98c8 362 {
Rvs94 7:111e13da9bc0 363 m1_ref = m1_ref + Stapgrootte;
Rvs94 7:111e13da9bc0 364
Rvs94 8:86a13efe98c8 365 if (m1_ref > Grens1 )
Rvs94 7:111e13da9bc0 366 {
Rvs94 8:86a13efe98c8 367 m1_ref = Grens1;
Rvs94 7:111e13da9bc0 368 }
Rvs94 7:111e13da9bc0 369 wait(0.05);
Rvs94 7:111e13da9bc0 370 }
Rvs94 8:86a13efe98c8 371 if ((EMG_left_MAF < Threshold1) && (EMG_right_MAF > Threshold3) && (m2_ref >= 0)) //if(c == 'g') // Draait naar rechts
Rvs94 8:86a13efe98c8 372 {
laura94 0:5816557b2064 373 m1_ref = m1_ref - Stapgrootte;
Rvs94 1:3f49c8818619 374
Margreeth95 9:350a82a7745a 375 if (m1_ref < m2_ref - 35 )
laura94 0:5816557b2064 376 {
Margreeth95 9:350a82a7745a 377 m1_ref = m2_ref - 35 ;
laura94 0:5816557b2064 378 }
Rvs94 4:92a3b38a5302 379 wait(0.05);
laura94 0:5816557b2064 380 }
laura94 0:5816557b2064 381 }
laura94 0:5816557b2064 382 // PROGRAM 2: Firing mechanism & Reset, Blue LED
laura94 0:5816557b2064 383 if(count == 2)
laura94 0:5816557b2064 384 {
Margreeth95 10:5c5f2ea11045 385 LedG = LedR = 1;
Margreeth95 10:5c5f2ea11045 386 pc.printf("Please press the button to shoot the disc\n");
Margreeth95 10:5c5f2ea11045 387 BlinkLed(LedB, 25, 0.1);
Margreeth95 10:5c5f2ea11045 388
laura94 0:5816557b2064 389 //VUUUUR!! (To Do)
Margreeth95 10:5c5f2ea11045 390 while (m2_ref != 0) // positiearm terug naar startpositie
Margreeth95 10:5c5f2ea11045 391 {
Margreeth95 10:5c5f2ea11045 392 if(m2_ref < 0)
Margreeth95 10:5c5f2ea11045 393 {
Margreeth95 10:5c5f2ea11045 394 m2_ref = m2_ref+Stapgrootte;
Margreeth95 10:5c5f2ea11045 395 wait(0.05);
Margreeth95 10:5c5f2ea11045 396 }
Margreeth95 10:5c5f2ea11045 397 if(m2_ref > 0)
Margreeth95 10:5c5f2ea11045 398 {
Margreeth95 10:5c5f2ea11045 399 m2_ref = m2_ref-Stapgrootte;
Margreeth95 10:5c5f2ea11045 400 wait(0.05);
Margreeth95 10:5c5f2ea11045 401 }
Margreeth95 10:5c5f2ea11045 402 }
Margreeth95 10:5c5f2ea11045 403
Margreeth95 10:5c5f2ea11045 404 while (m1_ref != 0) //werparm terug naar startpositie
Margreeth95 10:5c5f2ea11045 405 {
Margreeth95 10:5c5f2ea11045 406 if(m1_ref<0)
Margreeth95 10:5c5f2ea11045 407 {
Margreeth95 10:5c5f2ea11045 408 m1_ref = m1_ref+Stapgrootte;
Margreeth95 10:5c5f2ea11045 409 wait(0.05);
Margreeth95 10:5c5f2ea11045 410 }
Margreeth95 10:5c5f2ea11045 411
Margreeth95 10:5c5f2ea11045 412 if(m1_ref>0)
Margreeth95 10:5c5f2ea11045 413 {
Margreeth95 10:5c5f2ea11045 414 m1_ref = m1_ref-Stapgrootte;
Margreeth95 10:5c5f2ea11045 415 wait(0.05);
Margreeth95 10:5c5f2ea11045 416 }
Margreeth95 10:5c5f2ea11045 417 }
Margreeth95 10:5c5f2ea11045 418
Margreeth95 10:5c5f2ea11045 419 if((m2_ref == 0) && (m1_ref == 0)) // terug naar program 0
Margreeth95 10:5c5f2ea11045 420 {
Margreeth95 10:5c5f2ea11045 421 count = 0;
Margreeth95 10:5c5f2ea11045 422 }
laura94 0:5816557b2064 423 }
Rvs94 1:3f49c8818619 424
laura94 0:5816557b2064 425 }
laura94 0:5816557b2064 426 }