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@29:c34c2c3d30a9, 2015-10-13 (annotated)

Committer:
Margreeth95
Date:
Tue Oct 13 14:00:41 2015 +0000
Revision:
29:c34c2c3d30a9
Parent:
28:a40884792e0a 
2 EMG signalen toegevoegd, karakters weggehaald. Nog niet getest

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
Margreeth95 28:a40884792e0a 18 HIDScope scope(5); // Scope, 4 channels
Margreeth95 29:c34c2c3d30a9 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
Margreeth95 27:3392f03bfada 31 //EMG
Margreeth95 27:3392f03bfada 32 AnalogIn EMG_left(A0); //Analog input
Margreeth95 27:3392f03bfada 33 AnalogIn EMG_right(A1);
Margreeth95 28:a40884792e0a 34
Rvs94 25:ae908de29943 35 // Tickers
Margreeth95 28:a40884792e0a 36 Ticker ScopeTime;
Margreeth95 28:a40884792e0a 37 Ticker myControllerTicker2;
Margreeth95 28:a40884792e0a 38 Ticker myControllerTicker1;
Margreeth95 29:c34c2c3d30a9 39 Ticker SampleEMGLeft;
Margreeth95 29:c34c2c3d30a9 40 Ticker SampleEMGRight;
Margreeth95 28:a40884792e0a 41 Ticker ScopeTimer;
Margreeth95 28:a40884792e0a 42 Ticker serial;
Margreeth95 29:c34c2c3d30a9 43 Ticker MovingAverageLeft;
Margreeth95 29:c34c2c3d30a9 44 Ticker MovingAverageRight;
Margreeth95 29:c34c2c3d30a9 45
Rvs94 25:ae908de29943 46 // Constants
Rvs94 25:ae908de29943 47 double reference2, reference1;
Rvs94 25:ae908de29943 48 double position2 = 0, position1 = 0;
Rvs94 25:ae908de29943 49 double m2_ref = 0, m1_ref = 0;
Rvs94 25:ae908de29943 50 int count = 0;
Rvs94 25:ae908de29943 51 double Grens2 = 90, Grens1 = 90;
Rvs94 25:ae908de29943 52 double Stapgrootte = 5;
Margreeth95 27:3392f03bfada 53
Margreeth95 27:3392f03bfada 54 double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
Margreeth95 27:3392f03bfada 55 double EMG_L_fh=0;
Margreeth95 27:3392f03bfada 56 double EMG_left_value;
Margreeth95 27:3392f03bfada 57 double EMG_left_f1;
Margreeth95 27:3392f03bfada 58 double EMG_left_f2;
Margreeth95 28:a40884792e0a 59 double EMG_left_f3;
Margreeth95 28:a40884792e0a 60 double EMG_left_f4;
Margreeth95 28:a40884792e0a 61 double EMG_left_f5;
Margreeth95 28:a40884792e0a 62 double EMG_left_f6;
Margreeth95 27:3392f03bfada 63 double EMG_left_abs;
Margreeth95 29:c34c2c3d30a9 64
Margreeth95 27:3392f03bfada 65 double EMG_right_value;
Margreeth95 27:3392f03bfada 66 double EMG_right_f1;
Margreeth95 27:3392f03bfada 67 double EMG_right_f2;
Margreeth95 29:c34c2c3d30a9 68 double EMG_right_f3;
Margreeth95 29:c34c2c3d30a9 69 double EMG_right_f4;
Margreeth95 29:c34c2c3d30a9 70 double EMG_right_f5;
Margreeth95 29:c34c2c3d30a9 71 double EMG_right_f6;
Margreeth95 27:3392f03bfada 72 double EMG_right_abs;
Margreeth95 28:a40884792e0a 73 double Threshold1 = 0.02;
Margreeth95 28:a40884792e0a 74 double Threshold2 = 0.06;
Margreeth95 28:a40884792e0a 75
Margreeth95 28:a40884792e0a 76 int N = 50;
Margreeth95 28:a40884792e0a 77 double MAF_left[50];
Margreeth95 28:a40884792e0a 78 double EMG_left_MAF;
Margreeth95 29:c34c2c3d30a9 79 double MAF_right[50];
Margreeth95 29:c34c2c3d30a9 80 double EMG_right_MAF;
Rvs94 2:099da0fc31b6 81
Rvs94 25:ae908de29943 82 //Sample time (motor-step)
Rvs94 25:ae908de29943 83 const double m2_Ts = 0.01, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 84
Rvs94 25:ae908de29943 85 //Controller gain Motor 2 & 1
Margreeth95 26:cd1db85359aa 86 const double m2_Kp = 5,m2_Ki = 0.01, m2_Kd = 20;
Margreeth95 26:cd1db85359aa 87 const double m1_Kp = 5,m1_Ki = 0.01, m1_Kd = 20;
Rvs94 25:ae908de29943 88 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 25:ae908de29943 89 double m1_err_int = 0, m1_prev_err = 0;
Rvs94 20:f5091e29cd26 90
Rvs94 25:ae908de29943 91 //Derivative filter coeffs Motor 2 & 1
Margreeth95 26:cd1db85359aa 92 const double BiGain2 = 0.012, BiGain1 = 0.016955;
Rvs94 25:ae908de29943 93 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 94 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;
Margreeth95 27:3392f03bfada 95
Margreeth95 28:a40884792e0a 96 // coëfficiënten
Margreeth95 28:a40884792e0a 97 const double BiGainEMG_H1 = 0.795375, BiGainEMG_H2 = 0.895763;
Margreeth95 29:c34c2c3d30a9 98 const double EMGH1_a1 = -1.56308931068, EMGH1_a2 = 0.61765749583, EMGH1_b0 = 1.0*BiGainEMG_H1, EMGH1_b1 = -1.99909075151*BiGainEMG_H1, EMGH1_b2 = 1.0*BiGainEMG_H1; //coefficients for high-pass filter
Margreeth95 29:c34c2c3d30a9 99 const double EMGH2_a1 = -1.75651417587, EMGH2_a2 = 0.82183182692, EMGH2_b0 = 1.0*BiGainEMG_H2, EMGH2_b1 = -1.99470632157*BiGainEMG_H2, EMGH2_b2 = 1.0*BiGainEMG_H2; //coefficients for high-pass filter
Margreeth95 28:a40884792e0a 100
Margreeth95 28:a40884792e0a 101 const double BiGainEMG_L1=0.959332, BiGainEMG_L2 = 0.223396;
Margreeth95 29:c34c2c3d30a9 102 const double EMGL1_a1 = -1.55576653052, EMGL1_a2 = 0.61374320375, EMGL1_b0 = 1.0*BiGainEMG_L1, EMGL1_b1 = -0.90928276835*BiGainEMG_L1, EMGL1_b2 = 1.0*BiGainEMG_L1; // coefficients for low-pass filter
Margreeth95 29:c34c2c3d30a9 103 const double EMGL2_a1 = -1.79696141922, EMGL2_a2 = 0.85096669383, EMGL2_b0 = 1.0*BiGainEMG_L2, EMGL2_b1 = -1.75825311060*BiGainEMG_L2, EMGL2_b2 = 1.0*BiGainEMG_L2; // coefficients for low-pass filter
Rvs94 20:f5091e29cd26 104
Margreeth95 28:a40884792e0a 105 const double BiGainEMG_N1 = 1.0, BiGainEMG_N2 = 0.965081;
Margreeth95 29:c34c2c3d30a9 106 const double EMGN1_a1 = -1.56858163035, EMGN1_a2 = 0.96424138362, EMGN1_b0 = 1.0*BiGainEMG_N1, EMGN1_b1 = -1.61854514265*BiGainEMG_N1, EMGN1_b2 = 1.0*BiGainEMG_N1; //coefficients for notch filter
Margreeth95 29:c34c2c3d30a9 107 const double EMGN2_a1 = -1.61100357722, EMGN2_a2 = 0.96592170538, EMGN2_b0 = 1.0*BiGainEMG_N2, EMGN2_b1 = -1.61854514265*BiGainEMG_N2, EMGN2_b2 = 1.0*BiGainEMG_N2; //coefficients for notch filter
Margreeth95 28:a40884792e0a 108
Rvs94 20:f5091e29cd26 109 // Filter variables
Rvs94 25:ae908de29943 110 double m2_f_v1 = 0, m2_f_v2 = 0;
Rvs94 25:ae908de29943 111 double m1_f_v1 = 0, m1_f_v2 = 0;
Margreeth95 27:3392f03bfada 112
Margreeth95 27:3392f03bfada 113 // Creating the filters
Margreeth95 28:a40884792e0a 114 biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
Margreeth95 28:a40884792e0a 115 biquadFilter EMG_highpass2 (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
Margreeth95 28:a40884792e0a 116 biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
Margreeth95 28:a40884792e0a 117 biquadFilter EMG_lowpass2 (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);
Margreeth95 28:a40884792e0a 118 biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
Margreeth95 28:a40884792e0a 119 biquadFilter EMG_notch2 (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
Margreeth95 29:c34c2c3d30a9 120
Margreeth95 29:c34c2c3d30a9 121 biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
Margreeth95 29:c34c2c3d30a9 122 biquadFilter EMG_highpass2R (EMGH2_a1, EMGH2_a2, EMGH2_b0, EMGH2_b1, EMGH2_b2);
Margreeth95 29:c34c2c3d30a9 123 biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
Margreeth95 29:c34c2c3d30a9 124 biquadFilter EMG_lowpass2R (EMGL2_a1, EMGL2_a2, EMGL2_b0, EMGL2_b1, EMGL2_b2);
Margreeth95 29:c34c2c3d30a9 125 biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
Margreeth95 29:c34c2c3d30a9 126 biquadFilter EMG_notch2R (EMGN2_a1, EMGN2_a2, EMGN2_b0, EMGN2_b1, EMGN2_b2);
Margreeth95 28:a40884792e0a 127
Rvs94 25:ae908de29943 128 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 129 // General Functions
Rvs94 25:ae908de29943 130 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 131
Rvs94 20:f5091e29cd26 132 //HIDScope
Rvs94 25:ae908de29943 133 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 134 {
Rvs94 25:ae908de29943 135 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 136 scope.set(1, position2);
Rvs94 25:ae908de29943 137 scope.set(2, reference1 - position1);
Rvs94 25:ae908de29943 138 scope.set(3, position1);
Margreeth95 28:a40884792e0a 139 scope.set(4, EMG_left_MAF);
Margreeth95 29:c34c2c3d30a9 140 scope.set(5, EMG_right_MAF);
Rvs94 25:ae908de29943 141 scope.send();
Rvs94 1:48aba8d5610a 142
Rvs94 25:ae908de29943 143 }
Rvs94 12:69ab81cf5b7d 144
Rvs94 12:69ab81cf5b7d 145 // Biquad filter
Rvs94 25:ae908de29943 146 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 147 {
Rvs94 25:ae908de29943 148 double v = u - a1*v1 - a2*v2;
Rvs94 25:ae908de29943 149 double y = b0*v + b1*v1 + b2*v2;
Rvs94 25:ae908de29943 150 v2 = v1; v1 = v;
Rvs94 25:ae908de29943 151 return y;
Rvs94 25:ae908de29943 152 }
Rvs94 12:69ab81cf5b7d 153
Rvs94 12:69ab81cf5b7d 154
Rvs94 12:69ab81cf5b7d 155 // Reusable PID controller
Rvs94 25:ae908de29943 156 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 157 const double f_a1,const double f_a2, const double f_b0, const double f_b1, const double f_b2)
Rvs94 25:ae908de29943 158 {
Rvs94 12:69ab81cf5b7d 159 // Derivative
Rvs94 25:ae908de29943 160 double e_der = (e-e_prev)/Ts;
Rvs94 25:ae908de29943 161 e_der = biquad(e_der,f_v1,f_v2,f_a1,f_a2,f_b0,f_b1,f_b2);
Rvs94 25:ae908de29943 162 e_prev = e;
Rvs94 12:69ab81cf5b7d 163 // Integral
Rvs94 25:ae908de29943 164 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 165 // PID
Rvs94 25:ae908de29943 166 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 167 }
Margreeth95 26:cd1db85359aa 168
Margreeth95 26:cd1db85359aa 169 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 26:cd1db85359aa 170 //EMG functions
Margreeth95 26:cd1db85359aa 171 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 26:cd1db85359aa 172
Margreeth95 27:3392f03bfada 173 // EMG filtering function
Margreeth95 29:c34c2c3d30a9 174 void EMGfilterLeft()
Margreeth95 27:3392f03bfada 175 {
Margreeth95 27:3392f03bfada 176 EMG_left_value = EMG_left.read();
Margreeth95 28:a40884792e0a 177 EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
Margreeth95 28:a40884792e0a 178 EMG_left_f2 = EMG_highpass2.step(EMG_left_f1);
Margreeth95 27:3392f03bfada 179 EMG_left_abs = fabs(EMG_left_f2);
Margreeth95 28:a40884792e0a 180 EMG_left_f3 = EMG_lowpass1.step(EMG_left_abs);
Margreeth95 28:a40884792e0a 181 EMG_left_f4 = EMG_lowpass2.step(EMG_left_f3);
Margreeth95 28:a40884792e0a 182 EMG_left_f5 = EMG_notch1.step(EMG_left_f4);
Margreeth95 28:a40884792e0a 183 EMG_left_f6 = EMG_notch1.step(EMG_left_f5);
Margreeth95 29:c34c2c3d30a9 184 }
Margreeth95 29:c34c2c3d30a9 185
Margreeth95 29:c34c2c3d30a9 186 void EMGfilterRight()
Margreeth95 29:c34c2c3d30a9 187 {
Margreeth95 29:c34c2c3d30a9 188 EMG_right_value = EMG_right.read();
Margreeth95 29:c34c2c3d30a9 189 EMG_right_f1 = EMG_highpass1R.step(EMG_right_value);
Margreeth95 29:c34c2c3d30a9 190 EMG_right_f2 = EMG_highpass2R.step(EMG_right_f1);
Margreeth95 29:c34c2c3d30a9 191 EMG_right_abs = fabs(EMG_right_f2);
Margreeth95 29:c34c2c3d30a9 192 EMG_right_f3 = EMG_lowpass1R.step(EMG_right_abs);
Margreeth95 29:c34c2c3d30a9 193 EMG_right_f4 = EMG_lowpass2R.step(EMG_right_f3);
Margreeth95 29:c34c2c3d30a9 194 EMG_right_f5 = EMG_notch1R.step(EMG_right_f4);
Margreeth95 29:c34c2c3d30a9 195 EMG_right_f6 = EMG_notch1R.step(EMG_right_f5);
Margreeth95 27:3392f03bfada 196 }
Margreeth95 26:cd1db85359aa 197
Margreeth95 29:c34c2c3d30a9 198 // Movingaverage Filter
Margreeth95 29:c34c2c3d30a9 199 void MovingAverageFilterLeft()
Margreeth95 29:c34c2c3d30a9 200 {
Margreeth95 29:c34c2c3d30a9 201 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;
Margreeth95 29:c34c2c3d30a9 202 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];
Margreeth95 29:c34c2c3d30a9 203 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];
Margreeth95 29:c34c2c3d30a9 204 MAF_left[0] = EMG_left_f6;
Margreeth95 29:c34c2c3d30a9 205 }
Margreeth95 29:c34c2c3d30a9 206
Margreeth95 29:c34c2c3d30a9 207 void MovingAverageFilterRight()
Margreeth95 29:c34c2c3d30a9 208 {
Margreeth95 29:c34c2c3d30a9 209 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;
Margreeth95 29:c34c2c3d30a9 210 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];
Margreeth95 29:c34c2c3d30a9 211 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];
Margreeth95 29:c34c2c3d30a9 212 MAF_right[0] = EMG_right_f6;
Margreeth95 29:c34c2c3d30a9 213 }
Margreeth95 29:c34c2c3d30a9 214
Rvs94 25:ae908de29943 215 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 216 // Motor control functions
Rvs94 25:ae908de29943 217 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 218
Margreeth95 18:6f71bb91b8bd 219 // Motor2 control
Rvs94 25:ae908de29943 220 void motor2_Controller()
Rvs94 9:774fc3c6a39e 221 {
Rvs94 25:ae908de29943 222 // Setpoint motor 2
Rvs94 25:ae908de29943 223 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 224 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 225 // Speed control
Rvs94 25:ae908de29943 226 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 227 m2_f_b0, m2_f_b1, m2_f_b2);
Rvs94 25:ae908de29943 228 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 229 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 230 // Direction control
Rvs94 25:ae908de29943 231 if(m2_P2 > 0)
Rvs94 25:ae908de29943 232 {
Rvs94 25:ae908de29943 233 motor2direction = 0;
Rvs94 25:ae908de29943 234 }
Rvs94 25:ae908de29943 235 else
Rvs94 25:ae908de29943 236 {
Rvs94 25:ae908de29943 237 motor2direction = 1;
Rvs94 25:ae908de29943 238 }
Rvs94 25:ae908de29943 239 }
Rvs94 25:ae908de29943 240
Rvs94 25:ae908de29943 241 // Motor1 control
Rvs94 25:ae908de29943 242 void motor1_Controller()
Rvs94 25:ae908de29943 243 {
Rvs94 25:ae908de29943 244 // Setpoint Motor 1
Rvs94 25:ae908de29943 245 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 246 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 247 // Speed control
Rvs94 25:ae908de29943 248 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 249 m1_f_b0, m1_f_b1, m1_f_b2);
Rvs94 25:ae908de29943 250 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 251 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 252 // Direction control
Rvs94 25:ae908de29943 253 if(m1_P2 > 0)
Rvs94 25:ae908de29943 254 {
Rvs94 25:ae908de29943 255 motor1direction = 1;
Rvs94 25:ae908de29943 256 }
Rvs94 25:ae908de29943 257 else
Rvs94 25:ae908de29943 258 {
Rvs94 25:ae908de29943 259 motor1direction = 0;
Rvs94 25:ae908de29943 260 }
Rvs94 9:774fc3c6a39e 261 }
Rvs94 3:687729d7996e 262
Rvs94 25:ae908de29943 263 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 264 // Main function
Rvs94 25:ae908de29943 265 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 266 int main()
Rvs94 25:ae908de29943 267 {
Rvs94 25:ae908de29943 268 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 269 // Initalizing
Rvs94 25:ae908de29943 270 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 271 //LEDs OFF
Rvs94 25:ae908de29943 272 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 273
Rvs94 25:ae908de29943 274 //PC connection & check
Rvs94 25:ae908de29943 275 pc.baud(115200);
Rvs94 25:ae908de29943 276 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 277
Rvs94 25:ae908de29943 278 // Tickers
Rvs94 25:ae908de29943 279 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 25:ae908de29943 280 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 25:ae908de29943 281 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Margreeth95 29:c34c2c3d30a9 282 SampleEMGLeft.attach(&EMGfilterLeft, 0.01f);
Margreeth95 29:c34c2c3d30a9 283 SampleEMGRight.attach(&EMGfilterRight, 0.01f);
Margreeth95 29:c34c2c3d30a9 284 MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.01f);
Margreeth95 29:c34c2c3d30a9 285 MovingAverageRight.attach(&MovingAverageFilterRight, 0.01f);
Rvs94 25:ae908de29943 286 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 287 // Control Program
Rvs94 25:ae908de29943 288 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 289 while(true)
Rvs94 20:f5091e29cd26 290 {
Margreeth95 29:c34c2c3d30a9 291 //char c = pc.getc();
Rvs94 25:ae908de29943 292 // 1 Program UP
Margreeth95 29:c34c2c3d30a9 293 if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF >= Threshold1)) //if(c == 'e') //
Rvs94 20:f5091e29cd26 294 {
Rvs94 20:f5091e29cd26 295 count = count + 1;
Rvs94 20:f5091e29cd26 296 if(count > 2)
Rvs94 20:f5091e29cd26 297 {
Rvs94 20:f5091e29cd26 298 count = 2;
Rvs94 20:f5091e29cd26 299 }
Rvs94 20:f5091e29cd26 300
Rvs94 20:f5091e29cd26 301 }
Rvs94 25:ae908de29943 302 // 1 Program DOWN
Margreeth95 29:c34c2c3d30a9 303 // if(c == 'd') // Hoe gaat dit aangestuurd worden?
Margreeth95 29:c34c2c3d30a9 304 // {
Margreeth95 29:c34c2c3d30a9 305 // count = count - 1;
Margreeth95 29:c34c2c3d30a9 306 // if(count < 0)
Margreeth95 29:c34c2c3d30a9 307 // {
Margreeth95 29:c34c2c3d30a9 308 // count = 0;
Margreeth95 29:c34c2c3d30a9 309 // }
Margreeth95 29:c34c2c3d30a9 310 // }
Rvs94 25:ae908de29943 311 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 25:ae908de29943 312 if(count == 0)
Rvs94 20:f5091e29cd26 313 {
Rvs94 20:f5091e29cd26 314
Rvs94 20:f5091e29cd26 315 LedR = LedB = 1;
Rvs94 20:f5091e29cd26 316 LedG = 0;
Margreeth95 29:c34c2c3d30a9 317 if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF <= Threshold1)) //if(c == 'r') //
Margreeth95 19:9417d2011e8b 318 {
Rvs94 24:d0af4b2be295 319 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 320 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 321 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 322 {
Rvs94 24:d0af4b2be295 323 m2_ref = Grens2;
Rvs94 25:ae908de29943 324 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 325 }
Margreeth95 19:9417d2011e8b 326 }
Margreeth95 29:c34c2c3d30a9 327 if(EMG_right_MAF < Threshold1) && (EMG_left_MAF > Threshold1)) //if (c == 'f') //
Margreeth95 19:9417d2011e8b 328 {
Rvs94 24:d0af4b2be295 329 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 330 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 331 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 332 {
Rvs94 24:d0af4b2be295 333 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 334 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 335 }
Margreeth95 19:9417d2011e8b 336 }
Rvs94 20:f5091e29cd26 337 }
Rvs94 25:ae908de29943 338 // PROGRAM 1: Motor 1 control, Red LED
Rvs94 25:ae908de29943 339 if(count == 1)
Rvs94 20:f5091e29cd26 340 {
Rvs94 20:f5091e29cd26 341 LedG = LedB = 1;
Rvs94 20:f5091e29cd26 342 LedR = 0;
Margreeth95 29:c34c2c3d30a9 343 if ((EMG_right_MAF >= Threshold1) && (EMG_left_MAF <= Threshold1)) // if(c == 't') //
Rvs94 24:d0af4b2be295 344 {
Rvs94 24:d0af4b2be295 345 m1_ref = m1_ref + Stapgrootte;
Rvs94 24:d0af4b2be295 346 if (m1_ref > Grens1)
Rvs94 24:d0af4b2be295 347 {
Rvs94 24:d0af4b2be295 348 m1_ref = Grens1;
Rvs94 24:d0af4b2be295 349 }
Rvs94 24:d0af4b2be295 350 }
Margreeth95 29:c34c2c3d30a9 351 if ((EMG_left_MAF > Threshold1) && (EMG_right_MAF < Threshold1)) //if(c == 'g') //
Rvs94 24:d0af4b2be295 352 {
Rvs94 24:d0af4b2be295 353 m1_ref = m1_ref - Stapgrootte;
Rvs94 24:d0af4b2be295 354 if (m1_ref < -1*Grens1)
Rvs94 24:d0af4b2be295 355 {
Rvs94 24:d0af4b2be295 356 m1_ref = -1*Grens1;
Rvs94 24:d0af4b2be295 357 }
Rvs94 24:d0af4b2be295 358 }
Rvs94 20:f5091e29cd26 359 }
Rvs94 25:ae908de29943 360 // PROGRAM 2: Firing mechanism & Reset, Blue LED
Rvs94 25:ae908de29943 361 if(count == 2)
Rvs94 20:f5091e29cd26 362 {
Rvs94 20:f5091e29cd26 363
Rvs94 20:f5091e29cd26 364 LedR = LedG = 1;
Rvs94 24:d0af4b2be295 365 LedB = 0;
Rvs94 25:ae908de29943 366 //VUUUUR!! (To Do)
Rvs94 24:d0af4b2be295 367 wait(1);
Rvs94 24:d0af4b2be295 368 m2_ref = 0;
Margreeth95 26:cd1db85359aa 369 m1_ref = 0;
Margreeth95 26:cd1db85359aa 370 count = 0;
Rvs94 20:f5091e29cd26 371 }
Margreeth95 0:284ed397e046 372 }
Rvs94 9:774fc3c6a39e 373
Margreeth95 0:284ed397e046 374 }