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@28:a40884792e0a, 2015-10-12 (annotated)

Committer:
Margreeth95
Date:
Mon Oct 12 16:53:25 2015 +0000
Revision:
28:a40884792e0a
Parent:
27:3392f03bfada
Child:
29:c34c2c3d30a9
Motor programma, EMG ingevoegd maar niet in gebruik

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