Remco Dasselaar
/
TotalControlEmg2
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: HIDScope MODSERIAL QEI TextLCD mbed
Fork of
TotalControlEmg2
by 
Remco Dasselaar
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 //--------------------Include files and libraries------- 00002 #include "mbed.h" 00003 #include "QEI.h" 00004 #include "MODSERIAL.h" 00005 #include "TextLCD.h" 00006 #include "HIDScope.h" 00007 #include "Filterdesigns.h" 00008 #include "Kalibratie.h" 00009 #include "Mode.h" 00010 //--------------------Classes------------------------ 00011 InterruptIn btnSet(PTC6); // Kalibration button 00012 InterruptIn emgSet(PTA4); // EMG on/off button 00013 MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty 00014 TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD screen on inner row of K64F, rs, e, d4-d7 00015 PwmOut servo(D3); // PwmOut servo 00016 AnalogIn emgL(A0), emgR(A1); // Analog input of EMG, left and right 00017 AnalogIn potL(A2), potR(A3); // Potential meter left and right 00018 AnalogIn ksLeft(A5), ksRight(A4); // Killswitch left and right 00019 HIDScope scope(2); // Hidscope, amount of scopes 00020 Ticker EMGticker, tickerControl, tickerLcd; // Ticker for EMG, regulator and break 00021 // QEI Encoder(port 1, port 2, ,counts/rev 00022 QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64); 00023 // Motor1 met PWM power control and direction 00024 PwmOut pwmM1(D6); 00025 DigitalOut dirM1(D7); 00026 // Motor2 met PWM power control and direction 00027 PwmOut pwmM2(D5); 00028 DigitalOut dirM2(D4); 00029 enum spelfase {CALIBRATE_EMG, CALIBRATE_R, CALIBRATE_L, CALIBRATE_MID, CALIBRATE_BEAM, AIM, BREAK, FIRE}; // Program states, ACKNOWLEDGEMENT switch: BMT groep 4 2014 00030 uint8_t state = CALIBRATE_EMG; // first state program 00031 enum aimFase {OFF, CW, CCW}; // Aim motor possible states 00032 uint8_t aimState = OFF; // first state aim motor 00033 //-------------------------------Variables--------------------------------------------------------------------- 00034 int maxCounts = 0; // max richt motor counts Aim motor 00035 int breakPerc = 0; // Break part {0, 1 , 2 ,3}/4 of half revolution 00036 volatile int modeL = 1, modeR = 1; // modes left and right 00037 const double servoBreak = 0.00165, servoFree = 0.0020; // Range servo,between servoL en servoR (= pulsewidth pwm servo) 00038 const double tControl = 0.01, tLcd = 2, tEmg = 0.005; // ticker time (sec) 00039 const double wM2 = 0.31; 00040 double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0; 00041 double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0; 00042 volatile bool L = false, R = false; // Booleans for checking if mode. has been 1? 00043 volatile bool btn = false; // Button is pressed? 00044 volatile bool controlFlag = false, btnFlag = false, emgFlag = false, lcdFlag = false, runEmg = true; // Go flags 00045 //----------------------------Functions----------------------------------------------------------------------- 00046 void PRINT(const char* text){ 00047 lcd.cls(); // clear LCD scherm 00048 lcd.printf(text); // print text op lcd 00049 } 00050 void EMGkalibratieL(){ // Determine thresholds left 00051 PRINT("EMG LEFT relax muscle"); 00052 double ymin = KalibratieMin(emgL, true); // Minimum value left EMG, boolean indicates left 00053 wait(1); 00054 PRINT("EMG LEFT flex muscle"); // LCD 00055 double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left 00056 PRINT("EMG LEFT well done!"); // LCD 00057 00058 if((ymax-ymin) < 0.05 || !runEmg){ // No EMG connected or button pushed 00059 ymin = 10; 00060 ymax = 10; 00061 } 00062 thresholdlowL = 4 * ymin; // Lowest threshold 00063 thresholdmidL = 0.5 * ymax; // Midi threshold 00064 thresholdhighL = 0.8 * ymax; // Highest threshold 00065 00066 pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); 00067 wait(1); 00068 } 00069 void EMGkalibratieR(){ // Determine thresholds right EMG, same as left 00070 PRINT("EMG RIGHT relax muscle"); 00071 double ymin = KalibratieMin(emgR, false); 00072 wait(1); 00073 PRINT("EMG RIGHT flex muscle"); 00074 double ymax = KalibratieMax(emgR, false); 00075 PRINT("EMG LEFT well done!"); 00076 00077 if((ymax-ymin) < 0.05|| !runEmg){ 00078 ymin = 10; 00079 ymax = 10; 00080 } 00081 thresholdlowR = 4 * ymin; 00082 thresholdmidR = 0.5 * ymax; 00083 thresholdhighR = 0.8 * ymax; 00084 00085 pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal 00086 wait(1); 00087 } 00088 int EMGfilter(AnalogIn& emg, bool side){ 00089 double u = emg.read(); // read emg signal (left or right EMG) 00090 int mode = 1; 00091 if(side){ 00092 double y = FilterdesignsLeft(u); // filter signal left EMG 00093 mode = Mode(modeL, y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3) 00094 } 00095 else { 00096 double y = FilterdesignsRight(u); 00097 mode = Mode(modeR, y, thresholdlowR, thresholdmidR, thresholdhighR); // right EMG 00098 } 00099 return mode; 00100 } 00101 int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3) 00102 double volt = pot.read(); 00103 int mode = 1; 00104 if(volt > 0.8){ 00105 mode = 3; 00106 } 00107 else if(volt>0.35 && volt<0.65){ 00108 mode = 2; 00109 } 00110 return mode; 00111 } 00112 int defMode(AnalogIn& emg, AnalogIn& pot, bool side){// Determine mode both from EMG and Potentialmeter, ONE of them must be ONE! 00113 int emgMode = EMGfilter(emg, side); 00114 int potMode = PotReader(pot); 00115 int mode = 1; 00116 if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1 00117 if(emgMode > potMode){ // maximum of emg and pot 00118 mode = emgMode; 00119 } 00120 else{ 00121 mode = potMode; 00122 } 00123 } 00124 return mode; 00125 } 00126 void setEmgFlag(){ // Goflag EMG 00127 emgFlag = true; 00128 } 00129 void setLcdFlag(){ // Goflag EMG 00130 lcdFlag = true; 00131 } 00132 void btnSetAction(){ // Set knop K64F PTC4 00133 btn = true; // GoFlag setknop 00134 } 00135 void emgOnOff(){ // Set knop K64F, push PTA4 00136 runEmg = !runEmg; // switch enable emg 00137 pc.printf("EMG is enabled: %i\r\n", runEmg); 00138 } 00139 void setControlFlag(){ // Go flag setButton 00140 controlFlag = true; 00141 } 00142 void checkSide(AnalogIn& ks, int dir){ 00143 if((ks.read()>0.95) && (dirM2.read() == dir)){ 00144 pwmM2.write(0); // Aim motor freeze 00145 PRINT("BOEM! CRASH!"); // LCD 00146 pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal 00147 wait(1); 00148 } 00149 } 00150 void checkAim(){ // check if killswitch clashes with direction motor 00151 if(pwmM2.read()>0){ // direction motor clashes with killswitch and motor is on 00152 checkSide(ksLeft, 1); 00153 checkSide(ksRight, 0); 00154 } 00155 } 00156 void motorAim(int dir){ // Rotate Aim motor with given direction 00157 dirM2.write(dir); 00158 pwmM2.write(wM2); 00159 } 00160 bool controlAim(){ // Function to control aim motor with modes 00161 bool both = false; // boolean if both modes are 3 00162 modeL = defMode(emgL, potL, true); // determine mode left 00163 modeR = defMode(emgR, potR, false); // determine mode right 00164 00165 scope.set(0, modeL); 00166 scope.set(1, modeR); 00167 scope.send(); //send values to HIDScope 00168 00169 if(modeR < 2 && !R){ // control if mode has been 1 00170 R = true; 00171 } 00172 if(modeL < 2 && !L){ 00173 L = true; 00174 } 00175 00176 if((modeL>1 && L) && (modeR >1 && R)) { // mode L and mode R both 3, and both has been 1 00177 both = true; // Return true 00178 pwmM2.write(0); // Aim motor freeze 00179 aimState = OFF; // next state 00180 } 00181 else if((modeR == 2)) { // modeR ==2 00182 if(aimState != CCW){ 00183 aimState = CCW; // Rotate CCW 00184 pc.printf("Rotate -, CW\r\n"); 00185 PRINT("Rotate CW"); 00186 motorAim(0); 00187 } 00188 } 00189 else if((modeL == 2)) { // modeL == 2 00190 if(aimState != CW){ 00191 aimState = CW; // Rotate CW 00192 pc.printf("Rotate +, CCW\r\n"); 00193 PRINT("Rotate CCW"); 00194 motorAim(1); 00195 } 00196 } 00197 // modeR AND CCW OR modeL and CW 00198 else if((modeR == 1 && aimState == CCW) || (modeL == 1 && aimState == CW)) { // R=1 en CW 00199 pwmM2.write(0); // Aim motor freeze 00200 aimState = OFF; 00201 pc.printf("Freeze\r\n"); 00202 PRINT("Freeze"); 00203 } 00204 return both; 00205 } 00206 int main(){ 00207 btnSet.mode(PullUp); // Button mode 00208 btnSet.rise(&btnSetAction); // Connect button to function 00209 emgSet.mode(PullUp); // Button mode 00210 emgSet.rise(&emgOnOff); // Connect button to function 00211 tickerControl.attach(&setControlFlag,tControl); // ticker control motor 00212 EMGticker.attach(&setEmgFlag, tEmg); // ticker EMG 00213 tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd 00214 00215 pc.printf("\n\n\n\n\n"); // Terminal 00216 pc.printf("---NEW GAME---\r\n"); 00217 PRINT("--- NEW GAME ---"); // LCD 00218 00219 if(potL.read() > 0.3 || potR.read() > 0.3){ // check if one of potmeters is on 00220 pc.printf("Potentialmeter is on!!!\r\n"); 00221 } 00222 00223 while(1){ // Run program 00224 switch(state){ 00225 case CALIBRATE_EMG: { 00226 EMGkalibratieL(); // calibrate left EMG, determine thresholds 00227 EMGkalibratieR(); // calibrate right EMG, determine thresholds 00228 state = CALIBRATE_R; // Next state 00229 break; 00230 } 00231 // following 3 cases kalibrate aim right killswitch, left killswitch, and go to mid position 00232 case CALIBRATE_R: { 00233 pwmM2.period(1/100000); // period motor 2 00234 pc.printf("Position is kalibrating\r\n"); // terminal 00235 PRINT("Wait a moment, please"); // LCD 00236 dirM2.write(0); // direction aim motor 00237 pwmM2.write(wM2); // percentage motor power 00238 00239 while(state==CALIBRATE_R){ 00240 if(controlFlag){ 00241 controlFlag = false; 00242 if((ksRight.read()>0.95)){ // Killswitch? 00243 pwmM2.write(0); // Aim motor freeze 00244 enc2.reset(); // Reset encoder 00245 state = CALIBRATE_L; 00246 } 00247 } 00248 } 00249 break; 00250 } 00251 case CALIBRATE_L: { 00252 dirM2.write(1); // direction aim motor, other direction 00253 pwmM2.write(wM2); // percentage motor power, turn it on 00254 while(state==CALIBRATE_L){ 00255 if(controlFlag){ 00256 controlFlag = false; 00257 if((ksLeft.read()>0.95)){ // calibrate left killswitch 00258 pwmM2.write(0); // Aim motor freeze 00259 maxCounts = abs(enc2.getPulses()); 00260 state = CALIBRATE_MID; 00261 } 00262 } 00263 } 00264 break; 00265 } 00266 case CALIBRATE_MID: { 00267 dirM2.write(0); // direction aim motor 00268 pwmM2.write(wM2); // percentage motor power, turn it on 00269 while(state==CALIBRATE_MID){ 00270 if(controlFlag){ 00271 controlFlag = false; 00272 if(abs(enc2.getPulses()) < (maxCounts/2)){ // rotate aim motor to midi position 00273 pwmM2.write(0); // Aim motor freeze 00274 state = CALIBRATE_BEAM; // next state 00275 } 00276 } 00277 } 00278 break; 00279 } 00280 case CALIBRATE_BEAM: { 00281 pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal 00282 dirM1.write(0); // direction beam motor 00283 pwmM1.period(1/100000); // period beam motor 00284 servo.period(0.02); // 20ms period servo 00285 btn = false; // Button is unpressed 00286 PRINT("Calibrate beam to 10 o'clock"); 00287 while(state==CALIBRATE_BEAM){ 00288 if(controlFlag){ 00289 pc.printf(""); // lege regel printen, anders doet setknop het niet 00290 controlFlag = false; 00291 if(btn){ // If setbutton is on or one mode is 3, and has been 1 00292 enc1.reset(); // encoder 1 reset 00293 PRINT("Beam calibrated"); 00294 pc.printf("Beam calibrated\r\n"); 00295 btn = false; // button op false 00296 state = AIM; // next state 00297 } 00298 } 00299 } 00300 lcd.cls(); 00301 break; 00302 } 00303 // Game stages: 00304 case AIM: { 00305 pc.printf("Aim with EMG\r\n"); // terminal 00306 int i = 0; // counter for lcd 00307 R = false; 00308 L = false; 00309 while(state == AIM){ 00310 if(controlFlag){ // motor control on GoFlag 00311 controlFlag = false; 00312 checkAim(); // Check position 00313 } 00314 if(emgFlag){ // Go flag EMG sampel 00315 emgFlag = false; 00316 if(controlAim()){ 00317 pc.printf("Position chosen\r\n"); // terminal 00318 state = BREAK; // Next state (BREAK) 00319 } 00320 } 00321 if(lcdFlag){ // LCD loop to project 3 texts on lcd 00322 lcdFlag = false; 00323 if(i%3 == 0){ 00324 PRINT("Flex L or R half to aim"); 00325 } 00326 else if(i%3 == 1){ 00327 PRINT("Flex both half to freeze"); 00328 } 00329 else { 00330 PRINT("Flex both full to continue"); 00331 } 00332 i++; 00333 } 00334 } 00335 lcd.cls(); 00336 break; 00337 } 00338 case BREAK: { 00339 pc.printf("Set break percentage, current is: %i\r\n", breakPerc); 00340 L = false; 00341 R = false; 00342 PRINT("L := -1, R := +1 L+R = continue"); 00343 00344 wait(1); 00345 while(state == BREAK){ 00346 if(emgFlag){ // Go flag EMG sampelen 00347 00348 emgFlag = false; 00349 modeL = defMode(emgL, potL, true); 00350 modeR = defMode(emgR, potR, false); 00351 00352 if((modeL > 1) && (modeR > 1) && L && R) { // both modes > 1 00353 state = FIRE; 00354 } 00355 else if((modeL > 2 && L)|| (modeR > 2 && R)) { // left of right mode 3 = fire 00356 state = FIRE; 00357 } 00358 else { 00359 if(modeR < 2) { // modeR is 1 00360 R = true; 00361 } 00362 if(modeL < 2) { // modeL is 1 00363 L = true; 00364 } 00365 if(L && R){ // L and R has been 1 00366 if((modeL == 2) && L) { // modeL = BREAK lower with one 00367 if(breakPerc > 0){ 00368 breakPerc--; 00369 } 00370 L = false; 00371 } 00372 else if((modeR == 2) && R) { // modeR = Break +1 00373 if(breakPerc < 3){ 00374 breakPerc ++; 00375 } 00376 R = false; 00377 } 00378 if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1 00379 pc.printf("Current breaking scale: %i \r\n", breakPerc); 00380 } 00381 } 00382 } 00383 if(lcdFlag){ // every 2 seconds update lcd 00384 lcdFlag = false; 00385 lcd.cls(); 00386 lcd.printf("Break scale 0 - 3: %i", breakPerc); 00387 } 00388 } 00389 } 00390 lcd.cls(); 00391 lcd.printf("Break fixed on: %i", breakPerc); // lcd 00392 pc.printf("Break fixed\r\n"); // terminal 00393 L = false; // modeL must be one for another action can take place 00394 R = false; // modeR "" 00395 break; 00396 } 00397 case FIRE: { 00398 pc.printf("Shooting\r\n"); // terminal 00399 PRINT("FIRING"); // lcd 00400 pwmM1.write(0.3); // beam motor on 00401 bool breaking = false; 00402 int countBreak = 0; 00403 if(breakPerc > 0){ 00404 servo.pulsewidth(servoBreak); // Servo to break position 00405 breaking = true; // boolean breaking? 00406 countBreak = 2100*breakPerc/4; // Amount of counts where must be breaked 00407 } 00408 bool rev = false; 00409 while(state == FIRE){ // while in FIRE state 00410 if(controlFlag){ // BREAK goFlag half rotation beam = breaking 00411 controlFlag = false; // GoFlag 00412 int counts = abs(enc1.getPulses())+250; // counts encoder beam, +250 is to correct 10 to 12 o'clock // encoder is 0 on 100 counts before 12 o'clock 00413 if((counts%4200 > 2100) && !rev){ 00414 rev = true; 00415 } 00416 00417 if(breaking){ 00418 if((counts+countBreak)%4200 < 100){ // calculate with remainder, half revolution 00419 servo.pulsewidth(servoFree); // Servo to free position 00420 breaking = false; 00421 } 00422 } 00423 if(!breaking){ 00424 if(((counts-100)%4200 < 100) && rev){ // rotated 1 rev? 00425 pwmM1.write(0); // motor beam off 00426 pc.printf("Beam on startposition\r\n"); // terminal 00427 state = AIM; // Next stage 00428 } 00429 } 00430 } 00431 } 00432 lcd.cls(); 00433 break; 00434 } 00435 } 00436 } 00437 }
Generated on Sun Jul 17 2022 19:57:41 by
1.7.2