Den iz / Mbed 2 deprecated FinalCode

Final version used at presentation. Working parts: idle, enccali, emgcali, brushingmode, demomode(potmeter).

Dependencies:   mbed QEI HIDScope biquadFilter MODSERIAL Encoder FastPWM

main.cpp@3:8eb1b9e0d676, 2019-11-04 (annotated)

Committer:
dguru
Date:
Mon Nov 04 10:32:10 2019 +0000
Revision:
3:8eb1b9e0d676
Parent:
2:6ca30ccec353 
Final Version, used for presentation.; ; Working state machine for [Idle, enc_cali, Emg_cali, demo, brushingmode]; all of these modes kind of function as well.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dguru 3:8eb1b9e0d676 1 #include <math.h>
RobertoO 0:67c50348f842 2 #include "mbed.h"
dguru 3:8eb1b9e0d676 3 #include "MODSERIAL.h"
linde101 2:6ca30ccec353 4 #include "QEI.h"
dguru 3:8eb1b9e0d676 5 #include "FastPWM.h"
dguru 3:8eb1b9e0d676 6 #include "HIDScope.h"
dguru 3:8eb1b9e0d676 7 #include <stdio.h> /* printf */
dguru 3:8eb1b9e0d676 8 #include <stdlib.h> /* abs */
dguru 3:8eb1b9e0d676 9 #include <complex>
dguru 3:8eb1b9e0d676 10 #include "BiQuad.h"
linde101 2:6ca30ccec353 11
dguru 3:8eb1b9e0d676 12
dguru 3:8eb1b9e0d676 13 const double PI = 3.1415926535897;
linde101 2:6ca30ccec353 14
dguru 3:8eb1b9e0d676 15 InterruptIn but1(D1);
dguru 3:8eb1b9e0d676 16 InterruptIn but2(D0);
dguru 3:8eb1b9e0d676 17 DigitalIn butsw2(SW3);
dguru 3:8eb1b9e0d676 18 DigitalIn butsw3(SW2);
dguru 3:8eb1b9e0d676 19 AnalogIn potMeter1(A5);
dguru 3:8eb1b9e0d676 20 AnalogIn potMeter2(A4);
linde101 2:6ca30ccec353 21
dguru 3:8eb1b9e0d676 22 DigitalOut motor1_direction(D4); //richting van motor1
dguru 3:8eb1b9e0d676 23 FastPWM motor1_pwm(D5); //Motor 1 PWM controle van de snelheid
dguru 3:8eb1b9e0d676 24 DigitalOut motor2_direction(D7); //richting van motor2
dguru 3:8eb1b9e0d676 25 FastPWM motor2_pwm(D6); //Motor 2 PWM controle van de snelheid
linde101 2:6ca30ccec353 26
dguru 3:8eb1b9e0d676 27 Ticker loop_ticker;
dguru 3:8eb1b9e0d676 28 // SERIAL COMMUNICATION WITH PC.................................................
linde101 2:6ca30ccec353 29 Serial pc(USBTX, USBRX);
linde101 2:6ca30ccec353 30
dguru 3:8eb1b9e0d676 31 enum States {s_idle, s_calibratie_encoder, s_demonstratie, s_EMGcalibratie, s_EMG};
dguru 3:8eb1b9e0d676 32 States state;
dguru 3:8eb1b9e0d676 33
dguru 3:8eb1b9e0d676 34 // ENCODER .....................................................................
dguru 3:8eb1b9e0d676 35 QEI enc1 (D13, D12, NC, 4200); //encoder 1 gebruiken
dguru 3:8eb1b9e0d676 36 QEI enc2 (D9, D8, NC, 4200); //encoder 2 gebruiken
dguru 3:8eb1b9e0d676 37
dguru 3:8eb1b9e0d676 38 const float dt = 0.001;
dguru 3:8eb1b9e0d676 39 double e_int = 0;
dguru 3:8eb1b9e0d676 40 double e_int2 = 0;
dguru 3:8eb1b9e0d676 41 double theta1;
dguru 3:8eb1b9e0d676 42 double theta2;
dguru 3:8eb1b9e0d676 43 int enc1_zero = 0;
dguru 3:8eb1b9e0d676 44 int enc2_zero = 0;
dguru 3:8eb1b9e0d676 45 int enc1_value;
dguru 3:8eb1b9e0d676 46 int enc2_value;
dguru 3:8eb1b9e0d676 47 const int maxwaarde = 400;
linde101 2:6ca30ccec353 48
dguru 3:8eb1b9e0d676 49 volatile double Huidigepositie1=0;
dguru 3:8eb1b9e0d676 50 volatile double Huidigepositie2=0;
dguru 3:8eb1b9e0d676 51 volatile double motorValue1;
dguru 3:8eb1b9e0d676 52 volatile double motorValue2;
dguru 3:8eb1b9e0d676 53 volatile double refP;
dguru 3:8eb1b9e0d676 54 volatile double refP2;
dguru 3:8eb1b9e0d676 55 volatile double error1;
dguru 3:8eb1b9e0d676 56 volatile double error2;
dguru 3:8eb1b9e0d676 57
dguru 3:8eb1b9e0d676 58 bool state_changed = false;
dguru 3:8eb1b9e0d676 59 volatile bool A=true;
dguru 3:8eb1b9e0d676 60 volatile bool B=true;
dguru 3:8eb1b9e0d676 61 volatile bool but1_pressed = false;
dguru 3:8eb1b9e0d676 62 volatile bool but2_pressed = false;
dguru 3:8eb1b9e0d676 63 volatile bool butsw2_pressed = false;
dguru 3:8eb1b9e0d676 64 volatile bool butsw3_pressed = false;
dguru 3:8eb1b9e0d676 65 volatile bool failure_occurred = false;
linde101 2:6ca30ccec353 66
dguru 3:8eb1b9e0d676 67 const bool clockwise1 = true;
dguru 3:8eb1b9e0d676 68 const bool clockwise2 = true;
dguru 3:8eb1b9e0d676 69 volatile bool direction1 = clockwise1;
dguru 3:8eb1b9e0d676 70 volatile bool direction2 = clockwise2;
dguru 3:8eb1b9e0d676 71
dguru 3:8eb1b9e0d676 72 // RKI VARIABELEN...............................................................
dguru 3:8eb1b9e0d676 73 // Lengtes zijn in meters
dguru 3:8eb1b9e0d676 74 // Vaste variabelen:
dguru 3:8eb1b9e0d676 75 const double L0 = 0.25; // lengte arm 1 --> moet nog goed worden opgemeten!
dguru 3:8eb1b9e0d676 76 const double L2 = 0.375; // Lengte arm 2 --> moet ook nog goed opgemeten worden!
dguru 3:8eb1b9e0d676 77 const double r_trans = 0.035; // gebruiken voor translation naar shaft rotation
dguru 3:8eb1b9e0d676 78 //const double gear1=1.0;
dguru 3:8eb1b9e0d676 79 const double gear2=1.25;
dguru 3:8eb1b9e0d676 80
dguru 3:8eb1b9e0d676 81
dguru 3:8eb1b9e0d676 82 // Variërende variabelen:
dguru 3:8eb1b9e0d676 83 double q1 = 0; // Motorhoek van joint 1 op beginpositie
dguru 3:8eb1b9e0d676 84 double q2 = PI/2; // Motorhoek van joint 2 op beginpositie
dguru 3:8eb1b9e0d676 85 double v_x; // Snelheid van end effector in x richting --> Door EMG signals
dguru 3:8eb1b9e0d676 86 double v_y; // Snelheid van end effector in y richting --> Door EMG signals
linde101 2:6ca30ccec353 87
dguru 3:8eb1b9e0d676 88 double Lq1; // Translatieafstand als gevolg van motor rotation joint 1
dguru 3:8eb1b9e0d676 89 double Cq2; // Joint angle van het systeem (gecorrigeerd voor gear ratio 1:1.1)
dguru 3:8eb1b9e0d676 90
dguru 3:8eb1b9e0d676 91 double q1_dot; // Benodigde hoeksnelheid van motor 1
dguru 3:8eb1b9e0d676 92 double q2_dot; // Benodigde hoeksnelheid van motor 2
dguru 3:8eb1b9e0d676 93
dguru 3:8eb1b9e0d676 94 double q1_ii; // Referentie signaal voor motorhoek q1
dguru 3:8eb1b9e0d676 95 double q2_ii; // Referentie signaal voor motorhoek q2
dguru 3:8eb1b9e0d676 96
dguru 3:8eb1b9e0d676 97 //PI VARIABELEN................................................................
dguru 3:8eb1b9e0d676 98 const double kp=0.01; // Soort van geschaald --> meer onderzoek nodig
dguru 3:8eb1b9e0d676 99 const double ki=0.0001;
dguru 3:8eb1b9e0d676 100 const double kp2=0.01;
dguru 3:8eb1b9e0d676 101 const double ki2=0.0001;
dguru 3:8eb1b9e0d676 102
dguru 3:8eb1b9e0d676 103
dguru 3:8eb1b9e0d676 104 double maxPWM1 = 0.2;
dguru 3:8eb1b9e0d676 105 double maxPWM2 = 0.2;
dguru 3:8eb1b9e0d676 106
dguru 3:8eb1b9e0d676 107 //FILTERS EMG...................................................................
linde101 2:6ca30ccec353 108
dguru 3:8eb1b9e0d676 109 //Filters EMG
dguru 3:8eb1b9e0d676 110 //Filters Linker Biceps
dguru 3:8eb1b9e0d676 111 //In de volgorde High pass, notch, low pass
dguru 3:8eb1b9e0d676 112 //BiQuad LBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
dguru 3:8eb1b9e0d676 113 //BiQuad LBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
dguru 3:8eb1b9e0d676 114 BiQuad LBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
dguru 3:8eb1b9e0d676 115 //BiQuad LBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
dguru 3:8eb1b9e0d676 116 BiQuad LBN1( 0.5, 0, 0.5, 0, 0);
dguru 3:8eb1b9e0d676 117 BiQuad LBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
dguru 3:8eb1b9e0d676 118 //BiQuad LBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
dguru 3:8eb1b9e0d676 119 BiQuadChain LeftBicepHP;
dguru 3:8eb1b9e0d676 120 BiQuadChain LeftBicepN;
dguru 3:8eb1b9e0d676 121 BiQuadChain LeftBicepLP;
dguru 3:8eb1b9e0d676 122
dguru 3:8eb1b9e0d676 123 //Filters Rechter Biceps
dguru 3:8eb1b9e0d676 124 //In de volgorde High pass, notch, low pass
dguru 3:8eb1b9e0d676 125 //BiQuad RBHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
dguru 3:8eb1b9e0d676 126 //BiQuad RBHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
dguru 3:8eb1b9e0d676 127 BiQuad RBHP1(0.995566972017647, -1.991133944035294, 0.995566972017647, 1.000000000000000, -1.991114292201654, 0.991153595868935);
dguru 3:8eb1b9e0d676 128 //BiQuad RBHP2([3.913020539916823e-05,7.826041079833645e-05,3.913020539916823e-05, -1.982228929792529,0.982385450614126]);
dguru 3:8eb1b9e0d676 129 BiQuad RBN1( 0.5, 0, 0.5, 0, 0);
dguru 3:8eb1b9e0d676 130 BiQuad RBLP1(0000.087655548754006, 0000.175311097508013, 0000.087655548754006, 1.000000000000000, -1.973344249781299, 0.973694871976315);
dguru 3:8eb1b9e0d676 131 //BiQuad RBLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
dguru 3:8eb1b9e0d676 132 BiQuadChain RightBicepHP;
dguru 3:8eb1b9e0d676 133 BiQuadChain RightBicepN;
dguru 3:8eb1b9e0d676 134 BiQuadChain RightBicepLP;
dguru 3:8eb1b9e0d676 135
dguru 3:8eb1b9e0d676 136
dguru 3:8eb1b9e0d676 137 //Filters Rechter Quadriceps
dguru 3:8eb1b9e0d676 138 //In de volgorde High pass, notch, low pass
dguru 3:8eb1b9e0d676 139 //BiQuad RTHP1(1, 2.000000023182220, 0.999999981993818, 1.893415601022358, 0.896255659548714);
dguru 3:8eb1b9e0d676 140 //BiQuad RTHP2(1, 1.999999976817780, 0.999999976817780, 1.778313488139574, 0.790599715524785);
dguru 3:8eb1b9e0d676 141 BiQuad RTHP1(0.995566972017647, -1.991133944035294 , 0.995566972017647 ,1.000000000000000, -1.991114292201654 , 0.991153595868935);
dguru 3:8eb1b9e0d676 142 //BiQuad RTHP2(1, 1.999999966458334, 0.999999966458334, -1.988418014198592, 0.988451549797368);
dguru 3:8eb1b9e0d676 143 BiQuad RTN1( 0.5, 0, 0.5, 0, 0);
dguru 3:8eb1b9e0d676 144 BiQuad RTLP1(0000.087655548754006 , 0.175311097508013 , 0.087655548754006, 1.000000000000000 , -1.973344249781299 , 0.973694871976315);
dguru 3:8eb1b9e0d676 145 //BiQuad RTLP2( 0.000155148423475721, 0.000310296846951441, 0.000155148423475721,-1.96446058020523, 0.965081173899135);
dguru 3:8eb1b9e0d676 146 BiQuadChain RightLegHP;
dguru 3:8eb1b9e0d676 147 BiQuadChain RightLegN;
dguru 3:8eb1b9e0d676 148 BiQuadChain RightLegLP;
linde101 2:6ca30ccec353 149
dguru 3:8eb1b9e0d676 150 double emgLBHP;
dguru 3:8eb1b9e0d676 151 double emgLBN;
dguru 3:8eb1b9e0d676 152 double emgLBA;
dguru 3:8eb1b9e0d676 153 //double emgLBLP;
dguru 3:8eb1b9e0d676 154 //double emgAbsLBNotch;
dguru 3:8eb1b9e0d676 155
dguru 3:8eb1b9e0d676 156 double emgRBHP;
dguru 3:8eb1b9e0d676 157 double emgRBN;
dguru 3:8eb1b9e0d676 158 double emgRBA;
dguru 3:8eb1b9e0d676 159 double emgRBLP;
dguru 3:8eb1b9e0d676 160 double emgAbsRBNotch;
dguru 3:8eb1b9e0d676 161
dguru 3:8eb1b9e0d676 162 double emgRTN;
dguru 3:8eb1b9e0d676 163 double emgRTHP;
dguru 3:8eb1b9e0d676 164 double emgRTA;
dguru 3:8eb1b9e0d676 165 //double emgRTLP;
dguru 3:8eb1b9e0d676 166 //double emgAbsRTNotch;
dguru 3:8eb1b9e0d676 167
dguru 3:8eb1b9e0d676 168 double emgLBfiltered;
dguru 3:8eb1b9e0d676 169 double emgRBfiltered;
dguru 3:8eb1b9e0d676 170 double emgRTfiltered;
dguru 3:8eb1b9e0d676 171 double emgLBraw;
dguru 3:8eb1b9e0d676 172 double emgRBraw;
dguru 3:8eb1b9e0d676 173 double emgRTraw;
dguru 3:8eb1b9e0d676 174
dguru 3:8eb1b9e0d676 175
dguru 3:8eb1b9e0d676 176 double threshold_emgLB;
dguru 3:8eb1b9e0d676 177 double threshold_emgRB;
dguru 3:8eb1b9e0d676 178 double threshold_emgRT;
dguru 3:8eb1b9e0d676 179 double threshold = 0.5;
dguru 3:8eb1b9e0d676 180 double emgLB_max = 0.000;
dguru 3:8eb1b9e0d676 181 double emgRB_max = 0.000;
dguru 3:8eb1b9e0d676 182 double emgRT_max = 0.000;
dguru 3:8eb1b9e0d676 183 double emgLB_maxrust = 0.000;
dguru 3:8eb1b9e0d676 184 double emgRB_maxrust = 0.000;
dguru 3:8eb1b9e0d676 185 double emgRT_maxrust = 0.000;
dguru 3:8eb1b9e0d676 186 volatile int tijd = 0;
dguru 3:8eb1b9e0d676 187 double timecalibration;
dguru 3:8eb1b9e0d676 188
dguru 3:8eb1b9e0d676 189 double emgsumLB;
dguru 3:8eb1b9e0d676 190 double emgsumRB;
dguru 3:8eb1b9e0d676 191 double emgsumRT;
dguru 3:8eb1b9e0d676 192 double restmeanLB = 0.000;
dguru 3:8eb1b9e0d676 193 double restmeanRB = 0.000;
dguru 3:8eb1b9e0d676 194 double restmeanRT = 0.000;
dguru 3:8eb1b9e0d676 195 //double emgmeansubLB;
dguru 3:8eb1b9e0d676 196 //double emgmeansubRB;
dguru 3:8eb1b9e0d676 197 //double emgmeansubRT;
dguru 3:8eb1b9e0d676 198 double LBvalue;
dguru 3:8eb1b9e0d676 199 double RBvalue;
dguru 3:8eb1b9e0d676 200 double RTvalue;
dguru 3:8eb1b9e0d676 201 double emgLBrust;
dguru 3:8eb1b9e0d676 202 double emgRBrust;
dguru 3:8eb1b9e0d676 203 double emgRTrust;
dguru 3:8eb1b9e0d676 204 double RestmeanLB;
dguru 3:8eb1b9e0d676 205 double RestmeanRB;
dguru 3:8eb1b9e0d676 206 double RestmeanRT;
dguru 3:8eb1b9e0d676 207
dguru 3:8eb1b9e0d676 208
dguru 3:8eb1b9e0d676 209 DigitalOut ledr(LED_RED);
dguru 3:8eb1b9e0d676 210 DigitalOut ledg(LED_GREEN);
dguru 3:8eb1b9e0d676 211 DigitalOut ledb(LED_BLUE);
dguru 3:8eb1b9e0d676 212
dguru 3:8eb1b9e0d676 213
dguru 3:8eb1b9e0d676 214 AnalogIn emgLB(A0); //read EMG left bicep
dguru 3:8eb1b9e0d676 215 AnalogIn emgRB(A1); //read EMG right bicep
dguru 3:8eb1b9e0d676 216 AnalogIn emgRT(A2); //read EMG right quadriceps
dguru 3:8eb1b9e0d676 217
dguru 3:8eb1b9e0d676 218 //HIDScope scope(6); //aantal kanalen HIDScope (voor test 5, voor echt 6)
dguru 3:8eb1b9e0d676 219
dguru 3:8eb1b9e0d676 220 void rest()
dguru 3:8eb1b9e0d676 221 // Rust. Hier wordt niets uitgevoerd. Wanneer button1
dguru 3:8eb1b9e0d676 222 {
dguru 3:8eb1b9e0d676 223 // wordt ingedrukt gaan we naar de volgende state waar
dguru 3:8eb1b9e0d676 224 if (but1_pressed) { // de encoders worden gekalibreerd.
dguru 3:8eb1b9e0d676 225 state_changed = true;
dguru 3:8eb1b9e0d676 226 state = s_calibratie_encoder;
dguru 3:8eb1b9e0d676 227 but1_pressed = false;
linde101 2:6ca30ccec353 228 }
linde101 2:6ca30ccec353 229 }
linde101 2:6ca30ccec353 230
dguru 3:8eb1b9e0d676 231 void calibratie_encoder() // calibratie encoder. Hier worden de encoders gekalibreerd en op
dguru 3:8eb1b9e0d676 232 {
dguru 3:8eb1b9e0d676 233 // 0 gezet. Wanneer op button 1 wordt gedrukt gaan we naar de
dguru 3:8eb1b9e0d676 234 if (state_changed) { // demo modus, wanneer op button 2 wordt gedruk gaan we naar
dguru 3:8eb1b9e0d676 235 pc.printf("Started encoder calibration\r\n"); // de EMG calibratie
dguru 3:8eb1b9e0d676 236 state_changed = false;
dguru 3:8eb1b9e0d676 237 }
dguru 3:8eb1b9e0d676 238 else if (but1_pressed) {
dguru 3:8eb1b9e0d676 239 pc.printf("Encoder has been calibrated \r\n");
dguru 3:8eb1b9e0d676 240 enc1_value = enc1.getPulses();
dguru 3:8eb1b9e0d676 241 enc2_value = enc2.getPulses();
dguru 3:8eb1b9e0d676 242 //enc1_value -= enc1_zero;
dguru 3:8eb1b9e0d676 243 //enc2_value -= enc2_zero;
dguru 3:8eb1b9e0d676 244 theta1 = (float)(enc1_value)/(float)(4200)*2*PI; // First arm rotated fully ccw
dguru 3:8eb1b9e0d676 245 theta2 = (float)(enc2_value)/(float)(4200)*2*PI; // second arm fully clockwise --> looks like a Z
dguru 3:8eb1b9e0d676 246 enc1_zero = enc1_value;
dguru 3:8eb1b9e0d676 247 enc2_zero = enc2_value;
dguru 3:8eb1b9e0d676 248
dguru 3:8eb1b9e0d676 249
dguru 3:8eb1b9e0d676 250 pc.printf("enc01: %i\r\n", enc1_zero);
dguru 3:8eb1b9e0d676 251 pc.printf("enc1value: %i\r\n", enc1_value);
dguru 3:8eb1b9e0d676 252 pc.printf("enc02: %i\r\n",enc2_zero);
dguru 3:8eb1b9e0d676 253 pc.printf("enc2value: %i\r\n", enc2_value);
dguru 3:8eb1b9e0d676 254 pc.printf("hoek 1: %f\r\n",theta1);
dguru 3:8eb1b9e0d676 255 pc.printf("hoek 2: %f\r\n",theta2);
dguru 3:8eb1b9e0d676 256 but1_pressed = false;
dguru 3:8eb1b9e0d676 257 state_changed = true;
dguru 3:8eb1b9e0d676 258 state = s_demonstratie;
dguru 3:8eb1b9e0d676 259 }
linde101 2:6ca30ccec353 260
dguru 3:8eb1b9e0d676 261 else if (but2_pressed) {
dguru 3:8eb1b9e0d676 262 pc.printf("Encoder has been calibrated \r\n");
dguru 3:8eb1b9e0d676 263 enc1_value = enc1.getPulses();
dguru 3:8eb1b9e0d676 264 enc2_value = enc2.getPulses();
dguru 3:8eb1b9e0d676 265 //enc1_value -= enc1_zero;
dguru 3:8eb1b9e0d676 266 //enc2_value -= enc2_zero;
dguru 3:8eb1b9e0d676 267 theta1 = (float)(enc1_value)/(float)(4200)*2*PI;
dguru 3:8eb1b9e0d676 268 theta2 = (float)(enc2_value)/(float)(4200)*2*PI;
dguru 3:8eb1b9e0d676 269 enc1_zero = enc1_value;
dguru 3:8eb1b9e0d676 270 enc2_zero = enc2_value;
dguru 3:8eb1b9e0d676 271
dguru 3:8eb1b9e0d676 272
dguru 3:8eb1b9e0d676 273 pc.printf("enc01: %i\r\n", enc1_zero);
dguru 3:8eb1b9e0d676 274 pc.printf("enc1value: %i\r\n", enc1_value);
dguru 3:8eb1b9e0d676 275 pc.printf("enc02: %i\r\n",enc2_zero);
dguru 3:8eb1b9e0d676 276 pc.printf("enc2value: %i\r\n", enc2_value);
dguru 3:8eb1b9e0d676 277 pc.printf("hoek 1: %f\r\n",theta1);
dguru 3:8eb1b9e0d676 278 pc.printf("hoek 2: %f\r\n",theta2);
dguru 3:8eb1b9e0d676 279
dguru 3:8eb1b9e0d676 280 but2_pressed = false;
dguru 3:8eb1b9e0d676 281 state_changed = true;
dguru 3:8eb1b9e0d676 282 state = s_EMGcalibratie;
dguru 3:8eb1b9e0d676 283
dguru 3:8eb1b9e0d676 284 }
linde101 2:6ca30ccec353 285 }
linde101 2:6ca30ccec353 286
dguru 3:8eb1b9e0d676 287 void filteren()
dguru 3:8eb1b9e0d676 288 {
dguru 3:8eb1b9e0d676 289 //linkerbicep
dguru 3:8eb1b9e0d676 290 emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
dguru 3:8eb1b9e0d676 291 emgLBHP=LeftBicepHP.step(emgLBraw);
dguru 3:8eb1b9e0d676 292 emgLBN=LeftBicepN.step(emgLBHP);
dguru 3:8eb1b9e0d676 293 //emgmeansubLB = emgLBN - RestmeanLB;
dguru 3:8eb1b9e0d676 294 emgLBA= fabs(emgLBN);
dguru 3:8eb1b9e0d676 295 emgLBfiltered=LeftBicepLP.step(emgLBA);
dguru 3:8eb1b9e0d676 296 //LBvalue = emgLBfiltered/emgLB_max;
dguru 3:8eb1b9e0d676 297
dguru 3:8eb1b9e0d676 298 /* //to show if filter is working
dguru 3:8eb1b9e0d676 299 scope.set(0, emgLBraw);
dguru 3:8eb1b9e0d676 300 scope.set(1, emgLBfiltered);
dguru 3:8eb1b9e0d676 301 scope.set(2, emgRBraw);
dguru 3:8eb1b9e0d676 302 scope.set(3, emgRBfiltered);
dguru 3:8eb1b9e0d676 303 scope.set(4, emgRTraw);
dguru 3:8eb1b9e0d676 304 scope.set(5, emgRTfiltered);
dguru 3:8eb1b9e0d676 305 scope.send();
dguru 3:8eb1b9e0d676 306 */
dguru 3:8eb1b9e0d676 307 //rechterbicep
dguru 3:8eb1b9e0d676 308 emgRBraw= emgRB.read();
dguru 3:8eb1b9e0d676 309 emgRBHP= RightBicepHP.step(emgRBraw);
dguru 3:8eb1b9e0d676 310 emgRBN=RightBicepN.step(emgRBHP);
dguru 3:8eb1b9e0d676 311 //emgmeansubRB = emgRBN - RestmeanRB;
dguru 3:8eb1b9e0d676 312 emgRBA= fabs(emgRBN);
dguru 3:8eb1b9e0d676 313 emgRBfiltered=RightBicepLP.step(emgRBA);
dguru 3:8eb1b9e0d676 314 //RBvalue = emgRBfiltered/emgRB_max;
dguru 3:8eb1b9e0d676 315
dguru 3:8eb1b9e0d676 316 //Right Quadriceps
dguru 3:8eb1b9e0d676 317 emgRTraw= emgRT.read();
dguru 3:8eb1b9e0d676 318 emgRTHP= RightLegHP.step(emgRTraw);
dguru 3:8eb1b9e0d676 319 emgRTN= RightLegN.step(emgRTHP);
dguru 3:8eb1b9e0d676 320 // emgmeansubRT = emgRTHP - RestmeanRT;
dguru 3:8eb1b9e0d676 321 emgRTA= fabs(emgRTN);
dguru 3:8eb1b9e0d676 322 emgRTfiltered=RightLegLP.step(emgRTA);
dguru 3:8eb1b9e0d676 323 //RTvalue = emgRTfiltered/emgRT_max;
dguru 3:8eb1b9e0d676 324
linde101 2:6ca30ccec353 325
linde101 2:6ca30ccec353 326 }
linde101 2:6ca30ccec353 327
dguru 3:8eb1b9e0d676 328 void EMGcalibration()
dguru 3:8eb1b9e0d676 329 {
dguru 3:8eb1b9e0d676 330
dguru 3:8eb1b9e0d676 331 emgLBraw= emgLB.read(); //dit wordt: emgLBoffset
dguru 3:8eb1b9e0d676 332 emgLBHP=LeftBicepHP.step(emgLBraw);
dguru 3:8eb1b9e0d676 333 emgLBN=LeftBicepN.step(emgLBHP);
dguru 3:8eb1b9e0d676 334 emgLBA= fabs(emgLBN);
dguru 3:8eb1b9e0d676 335 emgLBfiltered=LeftBicepLP.step(emgLBA);
dguru 3:8eb1b9e0d676 336
dguru 3:8eb1b9e0d676 337 emgRBraw= emgRB.read();
dguru 3:8eb1b9e0d676 338 emgRBHP= RightBicepHP.step(emgRBraw);
dguru 3:8eb1b9e0d676 339 emgRBN=RightBicepN.step(emgRBHP);
dguru 3:8eb1b9e0d676 340 emgRBA= fabs(emgRBN);
dguru 3:8eb1b9e0d676 341 emgRBfiltered=RightBicepLP.step(emgRBA);
dguru 3:8eb1b9e0d676 342
dguru 3:8eb1b9e0d676 343
dguru 3:8eb1b9e0d676 344 emgRTraw= emgRT.read();
dguru 3:8eb1b9e0d676 345 emgRTHP= RightLegHP.step(emgRTraw);
dguru 3:8eb1b9e0d676 346 emgRTN= RightLegN.step(emgRTHP);
dguru 3:8eb1b9e0d676 347 emgRTA= fabs(emgRTN);
dguru 3:8eb1b9e0d676 348 emgRTfiltered=RightLegLP.step(emgRTA);
dguru 3:8eb1b9e0d676 349
dguru 3:8eb1b9e0d676 350 if (tijd > 1000 && tijd < 6000) {
dguru 3:8eb1b9e0d676 351 emgLBraw= emgLB.read();
dguru 3:8eb1b9e0d676 352 emgLBHP=LeftBicepHP.step(emgLBraw);
dguru 3:8eb1b9e0d676 353 emgLBN=LeftBicepN.step(emgLBHP);
dguru 3:8eb1b9e0d676 354 //emgmeansubLB = emgLBN - restmeanLB;
dguru 3:8eb1b9e0d676 355 emgLBA= fabs(emgLBN);
dguru 3:8eb1b9e0d676 356 emgLBfiltered=LeftBicepLP.step(emgLBA);
dguru 3:8eb1b9e0d676 357 if (emgLBfiltered > emgLB_max) {
dguru 3:8eb1b9e0d676 358 emgLB_max = emgLBfiltered;
dguru 3:8eb1b9e0d676 359 }
dguru 3:8eb1b9e0d676 360 //pc.baud(115200);
dguru 3:8eb1b9e0d676 361 //pc.printf("emgLB_max = %f \r\n", emgLB_max);// geen tekst printen in ticker want dat gaat mis xjes
dguru 3:8eb1b9e0d676 362 ledr=0; //led gaat aan zodra je linkerbicep moet aanspannen
dguru 3:8eb1b9e0d676 363 }
dguru 3:8eb1b9e0d676 364
dguru 3:8eb1b9e0d676 365 else if (tijd > 7000 && tijd < 12000) {
dguru 3:8eb1b9e0d676 366 emgRBraw= emgRB.read();
dguru 3:8eb1b9e0d676 367 emgRBHP= RightBicepHP.step(emgRBraw);
dguru 3:8eb1b9e0d676 368 emgRBN=RightBicepN.step(emgRBHP);
dguru 3:8eb1b9e0d676 369 //emgmeansubRB = emgRBN - RestmeanRB;
dguru 3:8eb1b9e0d676 370 emgRBA= fabs(emgRBN);
dguru 3:8eb1b9e0d676 371 emgRBfiltered=RightBicepLP.step(emgRBA);
dguru 3:8eb1b9e0d676 372 if (emgRBfiltered > emgRB_max) {
dguru 3:8eb1b9e0d676 373 emgRB_max = emgRBfiltered;
dguru 3:8eb1b9e0d676 374 }
dguru 3:8eb1b9e0d676 375 //pc.printf("emgRB_max = %f \r\n", emgRB_max);
dguru 3:8eb1b9e0d676 376 ledr=0; //led gaat uit zodra je rechterbicep moet aanspannen
dguru 3:8eb1b9e0d676 377 }
dguru 3:8eb1b9e0d676 378
dguru 3:8eb1b9e0d676 379
dguru 3:8eb1b9e0d676 380 else if (tijd > 13000 && tijd < 18000) {
dguru 3:8eb1b9e0d676 381 emgRTraw= emgRT.read();
dguru 3:8eb1b9e0d676 382 emgRTHP= RightLegHP.step(emgRTraw);
dguru 3:8eb1b9e0d676 383 emgRTN= RightLegN.step(emgRTHP);
dguru 3:8eb1b9e0d676 384 //emgmeansubRT = emgRTHP - RestmeanRT;
dguru 3:8eb1b9e0d676 385 emgRTA= fabs(emgRTN);
dguru 3:8eb1b9e0d676 386 emgRTfiltered=RightLegLP.step(emgRTA);
dguru 3:8eb1b9e0d676 387 if (emgRTfiltered > emgRT_max) {
dguru 3:8eb1b9e0d676 388 emgRT_max = emgRTfiltered;
dguru 3:8eb1b9e0d676 389 }
dguru 3:8eb1b9e0d676 390 //pc.printf("emgRT_max = %f \r\n", emgRT_max);
dguru 3:8eb1b9e0d676 391 ledr=0; //led gaat aan zodra je rechterbeenspier moet aanspannen
dguru 3:8eb1b9e0d676 392 } else if (tijd > 18000) {
dguru 3:8eb1b9e0d676 393 ledr=1; //led gaat uit zodra kalibratie voltooid
dguru 3:8eb1b9e0d676 394 state=s_EMG;
dguru 3:8eb1b9e0d676 395 state_changed=true;
dguru 3:8eb1b9e0d676 396
dguru 3:8eb1b9e0d676 397 //pc.printf("Calibration finished!");
dguru 3:8eb1b9e0d676 398 } else {
dguru 3:8eb1b9e0d676 399 ledr=1;
dguru 3:8eb1b9e0d676 400 }
dguru 3:8eb1b9e0d676 401
dguru 3:8eb1b9e0d676 402 pc.printf("%i", tijd);
dguru 3:8eb1b9e0d676 403 tijd++;
dguru 3:8eb1b9e0d676 404 threshold_emgLB = threshold*emgLB_max; //bepaal threshold, nu op 0.15*maximale waarde.
dguru 3:8eb1b9e0d676 405 threshold_emgRB = threshold*emgRB_max;
dguru 3:8eb1b9e0d676 406 threshold_emgRT = threshold*emgRT_max;
dguru 3:8eb1b9e0d676 407
dguru 3:8eb1b9e0d676 408
dguru 3:8eb1b9e0d676 409 // if threshold_emgx is reached, brushingmodes activated
dguru 3:8eb1b9e0d676 410 }
linde101 2:6ca30ccec353 411
dguru 3:8eb1b9e0d676 412
linde101 2:6ca30ccec353 413
dguru 3:8eb1b9e0d676 414 void SetMotor1(float motorValue1)
dguru 3:8eb1b9e0d676 415 {
dguru 3:8eb1b9e0d676 416 // gegeven motorValue1 <=1, schrijft snelheid naar pwm.
dguru 3:8eb1b9e0d676 417 // MotorValues buiten range worden afgekapt dat ze binnen de range vallen.
dguru 3:8eb1b9e0d676 418 motor1_pwm.write(fabs(motorValue1) > 1 ? 1 : fabs(motorValue1));
dguru 3:8eb1b9e0d676 419 }
dguru 3:8eb1b9e0d676 420
dguru 3:8eb1b9e0d676 421 void SetMotor2(float motorValue2)
dguru 3:8eb1b9e0d676 422 {
dguru 3:8eb1b9e0d676 423 motor2_pwm.write(fabs(motorValue2) > 1 ? 1 : fabs(motorValue2));
linde101 2:6ca30ccec353 424 }
linde101 2:6ca30ccec353 425
dguru 3:8eb1b9e0d676 426 void brushingmode()
dguru 3:8eb1b9e0d676 427 {
dguru 3:8eb1b9e0d676 428
dguru 3:8eb1b9e0d676 429 //boven draait arm 1 aan, dus motor 1
dguru 3:8eb1b9e0d676 430 //onder draait arm 2 aan, dus motor 2
dguru 3:8eb1b9e0d676 431 if (emgLBfiltered > threshold_emgLB){ //tandenborstel naar links (cw/ccw) //direction1/2 = cw/ccw
dguru 3:8eb1b9e0d676 432
dguru 3:8eb1b9e0d676 433 motor2_direction.write(direction1); //motor 2 gaat cw
dguru 3:8eb1b9e0d676 434 //motor2_direction.write(direction1 = !direction1); //is counterclockwise
dguru 3:8eb1b9e0d676 435 motor2_pwm.write(maxPWM2);
dguru 3:8eb1b9e0d676 436 ledr=0;
dguru 3:8eb1b9e0d676 437
dguru 3:8eb1b9e0d676 438
dguru 3:8eb1b9e0d676 439 }else if (emgRBfiltered > threshold_emgRB){
dguru 3:8eb1b9e0d676 440 motor2_pwm.write(maxPWM2);
dguru 3:8eb1b9e0d676 441 motor2_direction.write(direction1 = !direction1); //is counterclockwise
dguru 3:8eb1b9e0d676 442 //tandenborstel naar rechts
dguru 3:8eb1b9e0d676 443 ledg=0;
dguru 3:8eb1b9e0d676 444
dguru 3:8eb1b9e0d676 445
dguru 3:8eb1b9e0d676 446 }else if (emgRTfiltered > threshold_emgRT){
dguru 3:8eb1b9e0d676 447 motor1_direction.write(direction1 = !direction1);
dguru 3:8eb1b9e0d676 448 motor1_pwm.write(maxPWM1);
dguru 3:8eb1b9e0d676 449 //tandenborstel naar achter
dguru 3:8eb1b9e0d676 450 ledb=0;
dguru 3:8eb1b9e0d676 451
dguru 3:8eb1b9e0d676 452 }else if (emgRBfiltered > threshold_emgRB && emgLBfiltered > threshold_emgLB){
dguru 3:8eb1b9e0d676 453 motor1_direction.write(direction1); //motor 1 gaat cw
dguru 3:8eb1b9e0d676 454 motor1_pwm.write(maxPWM1);
dguru 3:8eb1b9e0d676 455 //tandenborstel naar voren
dguru 3:8eb1b9e0d676 456 ledg=0;
dguru 3:8eb1b9e0d676 457 ledr=0;
dguru 3:8eb1b9e0d676 458 }
dguru 3:8eb1b9e0d676 459 }
dguru 3:8eb1b9e0d676 460
dguru 3:8eb1b9e0d676 461 void demonstratie()
dguru 3:8eb1b9e0d676 462 {
dguru 3:8eb1b9e0d676 463 if (state_changed) { //
dguru 3:8eb1b9e0d676 464 pc.printf("Demonstratie gestart\r\n") ; //
dguru 3:8eb1b9e0d676 465 state_changed = false;
dguru 3:8eb1b9e0d676 466 }
dguru 3:8eb1b9e0d676 467 }
dguru 3:8eb1b9e0d676 468
dguru 3:8eb1b9e0d676 469
dguru 3:8eb1b9e0d676 470 void RKI()
linde101 2:6ca30ccec353 471 {
dguru 3:8eb1b9e0d676 472 Lq1 = q1*r_trans;
dguru 3:8eb1b9e0d676 473 Cq2 = q2/gear2; //1.25 is gear ratio voor q2
dguru 3:8eb1b9e0d676 474
dguru 3:8eb1b9e0d676 475 q1_dot = (v_x + (v_y*(L2*sin(q2/gear2)))/(L0 + q1*r_trans + L2*cos(q2/gear2)))/r_trans;
dguru 3:8eb1b9e0d676 476 q2_dot = (gear2*v_y)/(L0 + q1*r_trans + L2*cos(q2/gear2));
dguru 3:8eb1b9e0d676 477
dguru 3:8eb1b9e0d676 478 q1_ii = q1 + q1_dot*dt;
dguru 3:8eb1b9e0d676 479 q2_ii = q2 + q2_dot*dt;
dguru 3:8eb1b9e0d676 480
dguru 3:8eb1b9e0d676 481 q1 = q1_ii;
dguru 3:8eb1b9e0d676 482 q2 = q2_ii;
dguru 3:8eb1b9e0d676 483
dguru 3:8eb1b9e0d676 484 }
dguru 3:8eb1b9e0d676 485
dguru 3:8eb1b9e0d676 486 double GetReferencePosition()
dguru 3:8eb1b9e0d676 487 {
dguru 3:8eb1b9e0d676 488 double Potmeterwaarde = potMeter2.read();
dguru 3:8eb1b9e0d676 489
dguru 3:8eb1b9e0d676 490 double refP = Potmeterwaarde*maxwaarde;
dguru 3:8eb1b9e0d676 491 return refP;
dguru 3:8eb1b9e0d676 492 }
linde101 2:6ca30ccec353 493
dguru 3:8eb1b9e0d676 494 double GetReferencePosition2()
dguru 3:8eb1b9e0d676 495 {
dguru 3:8eb1b9e0d676 496 double potmeterwaarde2 = potMeter1.read();
dguru 3:8eb1b9e0d676 497 double refP2 = potmeterwaarde2*maxwaarde;
dguru 3:8eb1b9e0d676 498 return refP2;
dguru 3:8eb1b9e0d676 499 }
dguru 3:8eb1b9e0d676 500
dguru 3:8eb1b9e0d676 501 double FeedBackControl(double error, double &e_int)
linde101 2:6ca30ccec353 502 {
dguru 3:8eb1b9e0d676 503 //double Proportional= kp*error1;
dguru 3:8eb1b9e0d676 504 double motorValue1= kp*error1;
dguru 3:8eb1b9e0d676 505 //e_int = e_int+dt*error1;
dguru 3:8eb1b9e0d676 506 //double Integrator = ki*e_int;
dguru 3:8eb1b9e0d676 507
dguru 3:8eb1b9e0d676 508 // motorValue1 = Proportional + Integrator ;
dguru 3:8eb1b9e0d676 509 return motorValue1;
dguru 3:8eb1b9e0d676 510 }
dguru 3:8eb1b9e0d676 511
dguru 3:8eb1b9e0d676 512 double FeedBackControl2(double error2, double &e_int2)
dguru 3:8eb1b9e0d676 513 {
dguru 3:8eb1b9e0d676 514 //double Proportional2= kp2*error2;
dguru 3:8eb1b9e0d676 515 double motorValue2= kp2*error2;
dguru 3:8eb1b9e0d676 516 //e_int2 = e_int2+dt*error2;
dguru 3:8eb1b9e0d676 517 //double Integrator2 = ki2*e_int2;
dguru 3:8eb1b9e0d676 518
dguru 3:8eb1b9e0d676 519 //double motorValue2 = Proportional2 + Integrator2 ;
dguru 3:8eb1b9e0d676 520 return motorValue2;
dguru 3:8eb1b9e0d676 521 }
linde101 2:6ca30ccec353 522
dguru 3:8eb1b9e0d676 523 void MeasureAndControl()
dguru 3:8eb1b9e0d676 524 {
dguru 3:8eb1b9e0d676 525 // RKI aanroepen
dguru 3:8eb1b9e0d676 526 //RKI();
dguru 3:8eb1b9e0d676 527 // hier the control of the 1st control system
dguru 3:8eb1b9e0d676 528 refP = GetReferencePosition(); //moet eigenlijk nog met RKI
dguru 3:8eb1b9e0d676 529 Huidigepositie1 = enc1.getPulses()*4200;
dguru 3:8eb1b9e0d676 530 //error1 = (refP - Huidigepositie1);// make an error
dguru 3:8eb1b9e0d676 531 error1 = refP ;// ffwd
dguru 3:8eb1b9e0d676 532 motorValue1 = FeedBackControl(error1, e_int);
dguru 3:8eb1b9e0d676 533 SetMotor1(motorValue1);
dguru 3:8eb1b9e0d676 534 // hier the control of the 2nd control system
dguru 3:8eb1b9e0d676 535 refP2 = GetReferencePosition2();
dguru 3:8eb1b9e0d676 536 Huidigepositie2 = enc2.getPulses();
dguru 3:8eb1b9e0d676 537 //error2 = (refP2 - Huidigepositie2);// make an error
dguru 3:8eb1b9e0d676 538 error2 = refP2;// ffwd
dguru 3:8eb1b9e0d676 539 motorValue2 = FeedBackControl2(error2, e_int2);
dguru 3:8eb1b9e0d676 540 SetMotor2(motorValue2);
dguru 3:8eb1b9e0d676 541 //pc.printf("refP = %d, huidigepos = %d, motorvalue = %d, refP2 = %d, huidigepos2 = %d, motorvalue2 = %d \r\n", refP, Huidigepositie1, motorValue1, refP2, Huidigepositie2, Huidigepositie2);
dguru 3:8eb1b9e0d676 542
dguru 3:8eb1b9e0d676 543 }
dguru 3:8eb1b9e0d676 544
dguru 3:8eb1b9e0d676 545 void direction()
dguru 3:8eb1b9e0d676 546 {
dguru 3:8eb1b9e0d676 547 if (butsw2==0) {
dguru 3:8eb1b9e0d676 548 if (A==true) { // zodat het knopje 1 x wordt afgelezen
dguru 3:8eb1b9e0d676 549 // richting veranderen
dguru 3:8eb1b9e0d676 550 motor1_direction.write(direction1 = !direction1);
dguru 3:8eb1b9e0d676 551 pc.printf("direction: %s\r\n\n", direction1 ? "clockwise" : "counter clockwise");
dguru 3:8eb1b9e0d676 552 A=false;
dguru 3:8eb1b9e0d676 553 }
dguru 3:8eb1b9e0d676 554 } else {
dguru 3:8eb1b9e0d676 555 A=true;
dguru 3:8eb1b9e0d676 556 }
dguru 3:8eb1b9e0d676 557
dguru 3:8eb1b9e0d676 558 if (butsw3==0) {
dguru 3:8eb1b9e0d676 559 if (B==true) {
dguru 3:8eb1b9e0d676 560 motor2_direction.write(direction2 = !direction2);
dguru 3:8eb1b9e0d676 561 pc.printf("direction: %s\r\n\n", direction2 ? "clockwise" : "counter clockwise");
dguru 3:8eb1b9e0d676 562 B=false;
dguru 3:8eb1b9e0d676 563 }
dguru 3:8eb1b9e0d676 564 } else {
dguru 3:8eb1b9e0d676 565 B=true;
dguru 3:8eb1b9e0d676 566 }
dguru 3:8eb1b9e0d676 567
dguru 3:8eb1b9e0d676 568 }
dguru 3:8eb1b9e0d676 569
dguru 3:8eb1b9e0d676 570 void but1_interrupt()
dguru 3:8eb1b9e0d676 571 {
dguru 3:8eb1b9e0d676 572 /*if(but2.read()) {//both buttons are pressed
dguru 3:8eb1b9e0d676 573 failure_occurred = true;
dguru 3:8eb1b9e0d676 574 }*/
dguru 3:8eb1b9e0d676 575 but1_pressed = true;
dguru 3:8eb1b9e0d676 576 pc.printf("Button 1 interrupt active \n\r");
dguru 3:8eb1b9e0d676 577 }
dguru 3:8eb1b9e0d676 578
dguru 3:8eb1b9e0d676 579 void but2_interrupt()
dguru 3:8eb1b9e0d676 580 {
dguru 3:8eb1b9e0d676 581 /*if(but1.read()) {//both buttons are pressed
dguru 3:8eb1b9e0d676 582 failure_occurred = true;
dguru 3:8eb1b9e0d676 583 }*/
dguru 3:8eb1b9e0d676 584 but2_pressed = true;
dguru 3:8eb1b9e0d676 585 pc.printf("Button 2 interrupt active \n\r");
dguru 3:8eb1b9e0d676 586 }
linde101 2:6ca30ccec353 587
dguru 3:8eb1b9e0d676 588 void state_machine()
dguru 3:8eb1b9e0d676 589 {
dguru 3:8eb1b9e0d676 590 //run current state
dguru 3:8eb1b9e0d676 591 switch (state) {
dguru 3:8eb1b9e0d676 592 case s_idle: // in deze state gebeurd niets. Als op knop 1 wordt gedrukt
dguru 3:8eb1b9e0d676 593 rest(); // gaan we over naar s_calibratie_encoder
dguru 3:8eb1b9e0d676 594 break;
dguru 3:8eb1b9e0d676 595 case s_calibratie_encoder: // in deze state worden de encoders gekalibreerd. knop 1 -> s_demonstratie
dguru 3:8eb1b9e0d676 596 calibratie_encoder(); // knop 2 -> s_EMGcalibratie
dguru 3:8eb1b9e0d676 597 break;
dguru 3:8eb1b9e0d676 598 case s_demonstratie: // in deze state kunnen de motors worden bestuurd met de potmeters
dguru 3:8eb1b9e0d676 599 demonstratie();
dguru 3:8eb1b9e0d676 600 MeasureAndControl(); // en switch 2 en 3( pot1,sw2->motor1 / pot2,sw3->motor2
dguru 3:8eb1b9e0d676 601 direction(); // als op knop 2 wordt gedrukt komen we in de s_idle state
dguru 3:8eb1b9e0d676 602 if (but2_pressed) {
dguru 3:8eb1b9e0d676 603 pc.printf("fail. \r\n");
dguru 3:8eb1b9e0d676 604 but2_pressed = false;
dguru 3:8eb1b9e0d676 605 state_changed = true;
dguru 3:8eb1b9e0d676 606 state = s_idle;
dguru 3:8eb1b9e0d676 607 }
dguru 3:8eb1b9e0d676 608 break;
dguru 3:8eb1b9e0d676 609
dguru 3:8eb1b9e0d676 610 case s_EMGcalibratie:
dguru 3:8eb1b9e0d676 611 EMGcalibration();
dguru 3:8eb1b9e0d676 612 break;
dguru 3:8eb1b9e0d676 613
dguru 3:8eb1b9e0d676 614 case s_EMG:
dguru 3:8eb1b9e0d676 615 ledg=1;
dguru 3:8eb1b9e0d676 616 ledr=1;
dguru 3:8eb1b9e0d676 617 ledb=1;
dguru 3:8eb1b9e0d676 618 state_changed=false;
dguru 3:8eb1b9e0d676 619 filteren();
dguru 3:8eb1b9e0d676 620 brushingmode();
dguru 3:8eb1b9e0d676 621 if (but2_pressed) {
dguru 3:8eb1b9e0d676 622 pc.printf("fail. \r\n");
dguru 3:8eb1b9e0d676 623 but2_pressed = false;
dguru 3:8eb1b9e0d676 624 state_changed = true;
dguru 3:8eb1b9e0d676 625 state = s_idle;
dguru 3:8eb1b9e0d676 626 }
dguru 3:8eb1b9e0d676 627 break;
dguru 3:8eb1b9e0d676 628
dguru 3:8eb1b9e0d676 629 }
dguru 3:8eb1b9e0d676 630 }
dguru 3:8eb1b9e0d676 631
RobertoO 0:67c50348f842 632 int main()
RobertoO 0:67c50348f842 633 {
RobertoO 0:67c50348f842 634 pc.baud(115200);
dguru 3:8eb1b9e0d676 635 pc.printf("Executing main()... \r\n");
dguru 3:8eb1b9e0d676 636 state = s_idle;
dguru 3:8eb1b9e0d676 637 ledg=1;
dguru 3:8eb1b9e0d676 638 ledr=1;
dguru 3:8eb1b9e0d676 639 ledb=1;
dguru 3:8eb1b9e0d676 640
dguru 3:8eb1b9e0d676 641 LeftBicepHP.add( &LBHP1 ); //BiQuadChain bqc; //bqc.add( &bq1 ).add( &bq2 );
dguru 3:8eb1b9e0d676 642 LeftBicepN.add( &LBN1 );
dguru 3:8eb1b9e0d676 643 LeftBicepLP.add( &LBLP1 );
dguru 3:8eb1b9e0d676 644
dguru 3:8eb1b9e0d676 645
dguru 3:8eb1b9e0d676 646 RightBicepHP.add( &RBHP1 );
dguru 3:8eb1b9e0d676 647 RightBicepN.add( &RBN1 );
dguru 3:8eb1b9e0d676 648 RightBicepLP.add( &RBLP1 );
dguru 3:8eb1b9e0d676 649
dguru 3:8eb1b9e0d676 650 RightLegHP.add( &RTHP1 );
dguru 3:8eb1b9e0d676 651 RightLegN.add( &RTN1 );
dguru 3:8eb1b9e0d676 652 RightLegLP.add( &RTLP1 );
dguru 3:8eb1b9e0d676 653
dguru 3:8eb1b9e0d676 654 but1.fall(&but1_interrupt);
dguru 3:8eb1b9e0d676 655 but2.fall(&but2_interrupt);
dguru 3:8eb1b9e0d676 656 loop_ticker.attach(&state_machine, dt);
dguru 3:8eb1b9e0d676 657 pc.printf("state_machine active!\n\r");
dguru 3:8eb1b9e0d676 658
dguru 3:8eb1b9e0d676 659 }
dguru 3:8eb1b9e0d676 660
dguru 3:8eb1b9e0d676 661
dguru 3:8eb1b9e0d676 662