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@29:0f1c25496bdc, 2015-10-19 (annotated)

Committer:
Rvs94
Date:
Mon Oct 19 09:55:33 2015 +0000
Revision:
29:0f1c25496bdc
Parent:
28:2eb768b9cb3b 
Werkende aansturing motoren met EMG

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 28:2eb768b9cb3b 15 MODSERIAL pc(USBTX, USBRX); // To/From PC
Rvs94 28:2eb768b9cb3b 16 QEI Encoder2(D3, D2, NC, 32); // Encoder Motor 2
Rvs94 28:2eb768b9cb3b 17 QEI Encoder1(D13,D12,NC, 32); // Encoder Motor 1
Rvs94 28:2eb768b9cb3b 18 HIDScope scope(4); // Scope, 4 channels
Rvs94 28:2eb768b9cb3b 19 AnalogIn EMG_left(A0); // EMG Analog input LEFT
Rvs94 28:2eb768b9cb3b 20 AnalogIn EMG_right(A1); // EMG Analog input RIGHT
Rvs94 28:2eb768b9cb3b 21
Rvs94 20:f5091e29cd26 22
Rvs94 25:ae908de29943 23 // LEDs
Rvs94 25:ae908de29943 24 DigitalOut LedR(LED_RED);
Rvs94 25:ae908de29943 25 DigitalOut LedG(LED_GREEN);
Rvs94 25:ae908de29943 26 DigitalOut LedB(LED_BLUE);
Rvs94 20:f5091e29cd26 27
Rvs94 25:ae908de29943 28 // Motor
Rvs94 28:2eb768b9cb3b 29 DigitalOut motor1direction(D7); // Motor 1, Direction & Speed
Rvs94 28:2eb768b9cb3b 30 PwmOut motor1speed(D6);
Rvs94 28:2eb768b9cb3b 31 DigitalOut motor2direction(D4); // Motor 2, Direction & Speed
Rvs94 28:2eb768b9cb3b 32 PwmOut motor2speed(D5);
Rvs94 7:67b50d4fb03c 33
Rvs94 25:ae908de29943 34 // Tickers
Rvs94 25:ae908de29943 35 Ticker ScopeTime;
Rvs94 25:ae908de29943 36 Ticker myControllerTicker2;
Rvs94 25:ae908de29943 37 Ticker myControllerTicker1;
Rvs94 28:2eb768b9cb3b 38 // EMG Tickers
Rvs94 28:2eb768b9cb3b 39 Ticker SampleEMGLeft;
Rvs94 28:2eb768b9cb3b 40 Ticker SampleEMGRight;
Rvs94 28:2eb768b9cb3b 41 Ticker ScopeTimer;
Rvs94 28:2eb768b9cb3b 42 Ticker serial;
Rvs94 28:2eb768b9cb3b 43 Ticker MovingAverageLeft;
Rvs94 28:2eb768b9cb3b 44 Ticker MovingAverageRight;
Rvs94 25:ae908de29943 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;
Rvs94 2:099da0fc31b6 53
Rvs94 25:ae908de29943 54 //Sample time (motor-step)
Rvs94 25:ae908de29943 55 const double m2_Ts = 0.01, m1_Ts = 0.01;
Rvs94 20:f5091e29cd26 56
Rvs94 25:ae908de29943 57 //Controller gain Motor 2 & 1
Rvs94 28:2eb768b9cb3b 58 const double m2_Kp = 0.5, m2_Ki = 0.005, m2_Kd = 0.5;
Rvs94 28:2eb768b9cb3b 59 const double m1_Kp = 0.5,m1_Ki = 0.005, m1_Kd = 0.5;
Rvs94 25:ae908de29943 60 double m2_err_int = 0, m2_prev_err = 0;
Rvs94 25:ae908de29943 61 double m1_err_int = 0, m1_prev_err = 0;
Rvs94 20:f5091e29cd26 62
Rvs94 28:2eb768b9cb3b 63 // Declaring variables EMG
Rvs94 28:2eb768b9cb3b 64 double EMG_L_f_v1 = 0, EMG_L_f_v2 = 0;
Rvs94 28:2eb768b9cb3b 65 double EMG_L_fh=0;
Rvs94 28:2eb768b9cb3b 66 double EMG_left_value;
Rvs94 28:2eb768b9cb3b 67 double EMG_left_f1;
Rvs94 28:2eb768b9cb3b 68 double EMG_left_f2;
Rvs94 28:2eb768b9cb3b 69 double EMG_left_f3;
Rvs94 28:2eb768b9cb3b 70 double EMG_left_abs;
Rvs94 20:f5091e29cd26 71
Rvs94 28:2eb768b9cb3b 72 double EMG_right_value;
Rvs94 28:2eb768b9cb3b 73 double EMG_right_f1;
Rvs94 28:2eb768b9cb3b 74 double EMG_right_f2;
Rvs94 28:2eb768b9cb3b 75 double EMG_right_f3;
Rvs94 28:2eb768b9cb3b 76 double EMG_right_abs;
Rvs94 28:2eb768b9cb3b 77 double Threshold1;
Rvs94 28:2eb768b9cb3b 78 double Threshold2;
Rvs94 28:2eb768b9cb3b 79 double Threshold3;
Rvs94 28:2eb768b9cb3b 80 double Threshold4;
Rvs94 28:2eb768b9cb3b 81
Rvs94 28:2eb768b9cb3b 82 int N = 50;
Rvs94 28:2eb768b9cb3b 83 double MAF_left[50];
Rvs94 28:2eb768b9cb3b 84 double EMG_left_MAF;
Rvs94 28:2eb768b9cb3b 85 double MAF_right[50];
Rvs94 28:2eb768b9cb3b 86 double EMG_right_MAF;
Rvs94 28:2eb768b9cb3b 87
Rvs94 28:2eb768b9cb3b 88 // Filtercoëfficiënten EMG
Rvs94 28:2eb768b9cb3b 89 const double BiGainEMG_H1 = 0.796821;
Rvs94 28:2eb768b9cb3b 90 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
Rvs94 28:2eb768b9cb3b 91
Rvs94 28:2eb768b9cb3b 92 const double BiGainEMG_L1= 0.001041;
Rvs94 28:2eb768b9cb3b 93 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
Rvs94 28:2eb768b9cb3b 94
Rvs94 28:2eb768b9cb3b 95 const double BiGainEMG_N1 = 1.0;
Rvs94 28:2eb768b9cb3b 96 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
Rvs94 20:f5091e29cd26 97
Rvs94 25:ae908de29943 98 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 99 // General Functions
Rvs94 25:ae908de29943 100 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 20:f5091e29cd26 101
Rvs94 20:f5091e29cd26 102 //HIDScope
Rvs94 25:ae908de29943 103 void ScopeSend()//Functie die de gegevens voor de scope uitleest en doorstuurt
Rvs94 25:ae908de29943 104 {
Rvs94 25:ae908de29943 105 scope.set(0, reference2 - position2);
Rvs94 25:ae908de29943 106 scope.set(1, position2);
Rvs94 28:2eb768b9cb3b 107 scope.set(2, EMG_left_MAF);
Rvs94 29:0f1c25496bdc 108 scope.set(3, EMG_right_MAF);
Rvs94 25:ae908de29943 109 scope.send();
Rvs94 1:48aba8d5610a 110
Rvs94 25:ae908de29943 111 }
Rvs94 12:69ab81cf5b7d 112
Rvs94 12:69ab81cf5b7d 113 // Reusable PID controller
Rvs94 28:2eb768b9cb3b 114 double PID( double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev)
Rvs94 25:ae908de29943 115 {
Rvs94 12:69ab81cf5b7d 116 // Derivative
Rvs94 25:ae908de29943 117 double e_der = (e-e_prev)/Ts;
Rvs94 25:ae908de29943 118 e_prev = e;
Rvs94 12:69ab81cf5b7d 119 // Integral
Rvs94 25:ae908de29943 120 e_int = e_int + Ts*e;
Rvs94 12:69ab81cf5b7d 121 // PID
Rvs94 25:ae908de29943 122 return Kp * e + Ki*e_int + Kd*e_der;
Rvs94 25:ae908de29943 123 }
Rvs94 25:ae908de29943 124 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 28:2eb768b9cb3b 125 // EMG filtering function
Rvs94 28:2eb768b9cb3b 126 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 28:2eb768b9cb3b 127
Rvs94 28:2eb768b9cb3b 128 // Filter creation
Rvs94 28:2eb768b9cb3b 129 biquadFilter EMG_highpass1 (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
Rvs94 28:2eb768b9cb3b 130 biquadFilter EMG_lowpass1 (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
Rvs94 28:2eb768b9cb3b 131 biquadFilter EMG_notch1 (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
Rvs94 28:2eb768b9cb3b 132
Rvs94 28:2eb768b9cb3b 133 biquadFilter EMG_highpass1R (EMGH1_a1, EMGH1_a2, EMGH1_b0, EMGH1_b1, EMGH1_b2); // creates the high pass filter
Rvs94 28:2eb768b9cb3b 134 biquadFilter EMG_lowpass1R (EMGL1_a1, EMGL1_a2, EMGL1_b0, EMGL1_b1, EMGL1_b2); // creates the low pass filter
Rvs94 28:2eb768b9cb3b 135 biquadFilter EMG_notch1R (EMGN1_a1, EMGN1_a2, EMGN1_b0, EMGN1_b1, EMGN1_b2); // creates the notch filter
Rvs94 28:2eb768b9cb3b 136
Rvs94 28:2eb768b9cb3b 137 void EMGfilterLeft()
Rvs94 28:2eb768b9cb3b 138 {
Rvs94 28:2eb768b9cb3b 139 EMG_left_value = EMG_left.read();
Rvs94 28:2eb768b9cb3b 140 EMG_left_f1 = EMG_highpass1.step(EMG_left_value);
Rvs94 28:2eb768b9cb3b 141 EMG_left_abs = fabs(EMG_left_f1);
Rvs94 28:2eb768b9cb3b 142 EMG_left_f2 = EMG_lowpass1.step(EMG_left_abs);
Rvs94 28:2eb768b9cb3b 143 EMG_left_f3 = EMG_notch1.step(EMG_left_f2);
Rvs94 28:2eb768b9cb3b 144
Rvs94 28:2eb768b9cb3b 145 }
Rvs94 28:2eb768b9cb3b 146
Rvs94 28:2eb768b9cb3b 147 void EMGfilterRight()
Rvs94 28:2eb768b9cb3b 148 {
Rvs94 28:2eb768b9cb3b 149 EMG_right_value = EMG_right.read();
Rvs94 28:2eb768b9cb3b 150 EMG_right_f1 = EMG_highpass1R.step(EMG_right_value);
Rvs94 28:2eb768b9cb3b 151 EMG_right_abs = fabs(EMG_right_f1);
Rvs94 28:2eb768b9cb3b 152 EMG_right_f2 = EMG_lowpass1R.step(EMG_right_abs);
Rvs94 28:2eb768b9cb3b 153 EMG_right_f3 = EMG_notch1R.step(EMG_right_f2);
Rvs94 28:2eb768b9cb3b 154 }
Rvs94 28:2eb768b9cb3b 155
Rvs94 28:2eb768b9cb3b 156 // Movingaverage Filter
Rvs94 28:2eb768b9cb3b 157 void MovingAverageFilterLeft()
Rvs94 28:2eb768b9cb3b 158 {
Rvs94 28:2eb768b9cb3b 159 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;
Rvs94 28:2eb768b9cb3b 160 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];
Rvs94 28:2eb768b9cb3b 161 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];
Rvs94 28:2eb768b9cb3b 162 MAF_left[0] = EMG_left_f3;
Rvs94 28:2eb768b9cb3b 163 }
Rvs94 28:2eb768b9cb3b 164
Rvs94 28:2eb768b9cb3b 165 void MovingAverageFilterRight()
Rvs94 28:2eb768b9cb3b 166 {
Rvs94 28:2eb768b9cb3b 167 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;
Rvs94 28:2eb768b9cb3b 168 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];
Rvs94 28:2eb768b9cb3b 169 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];
Rvs94 28:2eb768b9cb3b 170 MAF_right[0] = EMG_right_f3;
Rvs94 28:2eb768b9cb3b 171 }
Rvs94 28:2eb768b9cb3b 172
Rvs94 28:2eb768b9cb3b 173 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 174 // Motor control functions
Rvs94 25:ae908de29943 175 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 176
Margreeth95 18:6f71bb91b8bd 177 // Motor2 control
Rvs94 25:ae908de29943 178 void motor2_Controller()
Rvs94 9:774fc3c6a39e 179 {
Rvs94 25:ae908de29943 180 // Setpoint motor 2
Rvs94 25:ae908de29943 181 reference2 = m2_ref; // Reference in degrees
Rvs94 25:ae908de29943 182 position2 = Encoder2.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 183 // Speed control
Rvs94 28:2eb768b9cb3b 184 double m2_P1 = PID( reference2 - position2, m2_Kp, m2_Ki, m2_Kd, m2_Ts, m2_err_int, m2_prev_err);
Rvs94 28:2eb768b9cb3b 185 double m2_P2 = m2_P1;
Rvs94 25:ae908de29943 186 motor2speed = abs(m2_P2);
Rvs94 25:ae908de29943 187 // Direction control
Rvs94 25:ae908de29943 188 if(m2_P2 > 0)
Rvs94 25:ae908de29943 189 {
Rvs94 25:ae908de29943 190 motor2direction = 0;
Rvs94 25:ae908de29943 191 }
Rvs94 25:ae908de29943 192 else
Rvs94 25:ae908de29943 193 {
Rvs94 25:ae908de29943 194 motor2direction = 1;
Rvs94 25:ae908de29943 195 }
Rvs94 25:ae908de29943 196 }
Rvs94 25:ae908de29943 197
Rvs94 25:ae908de29943 198 // Motor1 control
Rvs94 25:ae908de29943 199 void motor1_Controller()
Rvs94 25:ae908de29943 200 {
Rvs94 25:ae908de29943 201 // Setpoint Motor 1
Rvs94 25:ae908de29943 202 reference1 = m1_ref; // Reference in degrees
Rvs94 25:ae908de29943 203 position1 = Encoder1.getPulses()*360/(32*131); // Position in degrees
Rvs94 25:ae908de29943 204 // Speed control
Rvs94 28:2eb768b9cb3b 205 double m1_P1 = PID( reference1 - position1, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int, m1_prev_err);
Rvs94 28:2eb768b9cb3b 206 double m1_P2 = m1_P1;
Rvs94 25:ae908de29943 207 motor1speed = abs(m1_P2);
Rvs94 25:ae908de29943 208 // Direction control
Rvs94 25:ae908de29943 209 if(m1_P2 > 0)
Rvs94 25:ae908de29943 210 {
Rvs94 25:ae908de29943 211 motor1direction = 1;
Rvs94 25:ae908de29943 212 }
Rvs94 25:ae908de29943 213 else
Rvs94 25:ae908de29943 214 {
Rvs94 25:ae908de29943 215 motor1direction = 0;
Rvs94 25:ae908de29943 216 }
Rvs94 9:774fc3c6a39e 217 }
Rvs94 3:687729d7996e 218
Rvs94 25:ae908de29943 219 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 220 // Main function
Rvs94 25:ae908de29943 221 //--------------------------------------------------------------------------------------------------------------------------//
Margreeth95 0:284ed397e046 222 int main()
Rvs94 25:ae908de29943 223 {
Rvs94 25:ae908de29943 224 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 225 // Initalizing
Rvs94 25:ae908de29943 226 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 227 //LEDs OFF
Rvs94 25:ae908de29943 228 LedR = LedB = LedG = 1;
Rvs94 9:774fc3c6a39e 229
Rvs94 25:ae908de29943 230 //PC connection & check
Rvs94 25:ae908de29943 231 pc.baud(115200);
Rvs94 25:ae908de29943 232 pc.printf("Tot aan loop werkt\n");
Rvs94 25:ae908de29943 233
Rvs94 25:ae908de29943 234 // Tickers
Rvs94 28:2eb768b9cb3b 235 ScopeTime.attach(&ScopeSend, 0.01f); // 100 Hz, Scope
Rvs94 28:2eb768b9cb3b 236 myControllerTicker2.attach(&motor2_Controller, 0.01f ); // 100 Hz, Motor 2
Rvs94 28:2eb768b9cb3b 237 myControllerTicker1.attach(&motor1_Controller, 0.01f ); // 100 Hz, Motor 1
Rvs94 28:2eb768b9cb3b 238 SampleEMGLeft.attach(&EMGfilterLeft, 0.002); // 500 Hz, EMG LEFT
Rvs94 28:2eb768b9cb3b 239 SampleEMGRight.attach(&EMGfilterRight, 0.002); // 500 Hz, EMG RIGHT
Rvs94 28:2eb768b9cb3b 240 MovingAverageLeft.attach(&MovingAverageFilterLeft, 0.005); // 200 Hz, MAF LEFT
Rvs94 28:2eb768b9cb3b 241 MovingAverageRight.attach(&MovingAverageFilterRight, 0.005);// 200 Hz, MAF RIGHT
Rvs94 28:2eb768b9cb3b 242
Rvs94 28:2eb768b9cb3b 243 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 28:2eb768b9cb3b 244 // Determing Threshold
Rvs94 28:2eb768b9cb3b 245 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 28:2eb768b9cb3b 246 wait(20);
Rvs94 28:2eb768b9cb3b 247
Rvs94 28:2eb768b9cb3b 248 LedG.write(1);
Rvs94 28:2eb768b9cb3b 249 wait(0.2);
Rvs94 28:2eb768b9cb3b 250 LedG.write(0);
Rvs94 28:2eb768b9cb3b 251 wait(0.2);
Rvs94 28:2eb768b9cb3b 252 LedG.write(1);
Rvs94 28:2eb768b9cb3b 253 wait(0.2);
Rvs94 28:2eb768b9cb3b 254 LedG.write(0);
Rvs94 28:2eb768b9cb3b 255 wait(0.2);
Rvs94 28:2eb768b9cb3b 256 LedG.write(1);
Rvs94 28:2eb768b9cb3b 257 wait(0.2);
Rvs94 28:2eb768b9cb3b 258 LedG.write(0);
Rvs94 28:2eb768b9cb3b 259 wait(2);
Rvs94 28:2eb768b9cb3b 260 Threshold1 = 0.5*EMG_left_MAF;
Rvs94 28:2eb768b9cb3b 261 Threshold2 = 0.2*EMG_left_MAF;
Rvs94 28:2eb768b9cb3b 262 LedG.write(1);
Rvs94 29:0f1c25496bdc 263 LedR.write(0);
Rvs94 29:0f1c25496bdc 264 wait(2);
Rvs94 29:0f1c25496bdc 265 LedR.write(1);
Rvs94 29:0f1c25496bdc 266
Rvs94 29:0f1c25496bdc 267 wait(2);
Rvs94 29:0f1c25496bdc 268 LedB.write(1);
Rvs94 29:0f1c25496bdc 269 wait(0.2);
Rvs94 29:0f1c25496bdc 270 LedB.write(0);
Rvs94 29:0f1c25496bdc 271 wait(0.2);
Rvs94 29:0f1c25496bdc 272 LedB.write(1);
Rvs94 29:0f1c25496bdc 273 wait(0.2);
Rvs94 29:0f1c25496bdc 274 LedB.write(0);
Rvs94 29:0f1c25496bdc 275 wait(0.2);
Rvs94 29:0f1c25496bdc 276 LedB.write(1);
Rvs94 29:0f1c25496bdc 277 wait(0.2);
Rvs94 29:0f1c25496bdc 278 LedB.write(0);
Rvs94 29:0f1c25496bdc 279 wait(2);
Rvs94 29:0f1c25496bdc 280 Threshold3 = 0.5*EMG_right_MAF;
Rvs94 29:0f1c25496bdc 281 Threshold4 = 0.2*EMG_right_MAF;
Rvs94 29:0f1c25496bdc 282 LedB.write(1);
Rvs94 29:0f1c25496bdc 283 pc.printf("T1 = %f, T2 = %f, T3 = %f, T4 = %f\n", Threshold1, Threshold2, Threshold3, Threshold4);
Rvs94 29:0f1c25496bdc 284 LedR.write(0);
Rvs94 29:0f1c25496bdc 285 wait(2);
Rvs94 29:0f1c25496bdc 286 LedR.write(1);
Rvs94 28:2eb768b9cb3b 287
Rvs94 25:ae908de29943 288
Rvs94 25:ae908de29943 289 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 25:ae908de29943 290 // Control Program
Rvs94 25:ae908de29943 291 //--------------------------------------------------------------------------------------------------------------------------//
Rvs94 9:774fc3c6a39e 292 while(true)
Rvs94 20:f5091e29cd26 293 {
Rvs94 25:ae908de29943 294 // PROGRAM 0: Motor 2 control and indirect control of motor 1, Green LED
Rvs94 29:0f1c25496bdc 295 if(true)
Rvs94 20:f5091e29cd26 296 {
Rvs94 20:f5091e29cd26 297
Rvs94 20:f5091e29cd26 298 LedR = LedB = 1;
Rvs94 29:0f1c25496bdc 299 LedG = 1;
Rvs94 28:2eb768b9cb3b 300 if(EMG_left_MAF > Threshold1)
Margreeth95 19:9417d2011e8b 301 {
Rvs94 24:d0af4b2be295 302 m2_ref = m2_ref + Stapgrootte;
Rvs94 25:ae908de29943 303 m1_ref = m1_ref - Stapgrootte;
Rvs94 29:0f1c25496bdc 304 wait(0.1);
Rvs94 29:0f1c25496bdc 305 LedG = 0;
Rvs94 24:d0af4b2be295 306 if (m2_ref > Grens2)
Margreeth95 19:9417d2011e8b 307 {
Rvs94 24:d0af4b2be295 308 m2_ref = Grens2;
Rvs94 25:ae908de29943 309 m1_ref = -1*Grens1;
Margreeth95 19:9417d2011e8b 310 }
Margreeth95 19:9417d2011e8b 311 }
Rvs94 29:0f1c25496bdc 312 if(EMG_right_MAF > Threshold3)
Margreeth95 19:9417d2011e8b 313 {
Rvs94 24:d0af4b2be295 314 m2_ref = m2_ref - Stapgrootte;
Rvs94 25:ae908de29943 315 m1_ref = m1_ref + Stapgrootte;
Rvs94 29:0f1c25496bdc 316 wait(0.1);
Rvs94 29:0f1c25496bdc 317 LedB = 0;
Rvs94 24:d0af4b2be295 318 if (m2_ref < -1*Grens2)
Margreeth95 19:9417d2011e8b 319 {
Rvs94 24:d0af4b2be295 320 m2_ref = -1*Grens2;
Rvs94 25:ae908de29943 321 m1_ref = Grens1;
Margreeth95 19:9417d2011e8b 322 }
Margreeth95 19:9417d2011e8b 323 }
Rvs94 20:f5091e29cd26 324 }
Margreeth95 0:284ed397e046 325 }
Margreeth95 0:284ed397e046 326 }