Richard Roos
/
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
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 DigitalOut ledR(LED_RED), LedB(LED3); // Leds on K64F 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 KS(A5); // Killswitch 00019 HIDScope scope(6); // Hidscope, amount of scopes 00020 Ticker EMGticker, tickerControl, tickerBreak, 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_AIM, CALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, 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 const int on = 0, off = 1; // on off 00035 int maxCounts = 13000; // max richt motor counts Aim motor 00036 int breakPerc = 0; 00037 const double servoL = 0.001, servoR = 0.0011; // Range servo,between servoL en servoR (= pulsewidth pwm servo) 00038 const double tControl = 0.005, tBreak = 0.1, tLcd = 2; // tickers 00039 const double Fs = 50; //Sample frequency Hz 00040 double t = 1/ Fs; // time EMGticker 00041 double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0; 00042 double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0; 00043 double yL = 0, yR = 0; // y values EMG left and right 00044 volatile bool L = false, R = false; // Booleans for checking if mode. has been 1? 00045 volatile bool btn = false; // Button is pressed? 00046 volatile bool controlFlag = false, btnFlag = false, breakFlag = false, emgFlag = false, lcdFlag = false; // Go flags 00047 //----------------------------Functions----------------------------------------------------------------------- 00048 void flipLed(DigitalOut& led){ //function to flip one LED 00049 led.write(on); 00050 wait(0.2); 00051 led.write(off); 00052 } 00053 void PRINT(const char* text){ 00054 lcd.cls(); // clear LCD scherm 00055 lcd.printf(text); // print text op lcd 00056 } 00057 void EMGkalibratieL(){ // Determine thresholds left 00058 PRINT("EMG LEFT relax muscle"); 00059 double ymin = KalibratieMin(emgL, true); // Minimum value left EMG, boolean indicates left 00060 wait(1); 00061 PRINT("EMG LEFT flex muscle"); // LCD 00062 double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left 00063 PRINT("EMG LEFT well done!"); // LCD 00064 00065 if((ymax-ymin) < 0.05){ // No EMG connected 00066 ymin = 10; 00067 ymax = 10; 00068 } 00069 thresholdlowL = 4 * ymin; // Lowest threshold 00070 thresholdmidL = 0.5 * ymax; // Midi threshold 00071 thresholdhighL = 0.8 * ymax; // Highest threshold 00072 00073 pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); 00074 } 00075 void EMGkalibratieR(){ // Determine thresholds right EMG, same as left 00076 PRINT("EMG RIGHT relax muscle"); 00077 double ymin = KalibratieMin(emgR, false); 00078 wait(1); 00079 PRINT("EMG RIGHT flex muscle"); 00080 double ymax = KalibratieMax(emgR, false); 00081 PRINT("EMG LEFT well done!"); 00082 00083 if((ymax-ymin) < 0.05){ 00084 ymin = 10; 00085 ymax = 10; 00086 } 00087 thresholdlowR = 4 * ymin; 00088 thresholdmidR = 0.5 * ymax; 00089 thresholdhighR = 0.8 * ymax; 00090 00091 pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal 00092 } 00093 int EMGfilter(AnalogIn& emg, bool side){ 00094 double u = emg.read(); // read emg signal (left or right EMG) 00095 int mode = 1; 00096 if(side){ 00097 double y = FilterdesignsLeft(u); // filter signal left EMG 00098 mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3) 00099 } 00100 else { 00101 double y = FilterdesignsRight(u); 00102 mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); // right EMG 00103 } 00104 return mode; 00105 } 00106 int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3) 00107 double volt = pot.read(); 00108 int mode = 1; 00109 if(volt > 0.8){ 00110 mode = 3; 00111 } 00112 else if(volt>0.35 && volt<0.65){ 00113 mode = 2; 00114 } 00115 return mode; 00116 } 00117 int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // Determine mode both from EMG and Potentialmeter, ONE of them must be ONE! 00118 int emgMode = EMGfilter(emg, side); 00119 int potMode = PotReader(pot); 00120 int mode = 1; 00121 if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1 00122 if(emgMode > potMode){ // maximum of emg and pot 00123 mode = emgMode; 00124 } 00125 else{ 00126 mode = potMode; 00127 } 00128 } 00129 return mode; 00130 } 00131 void setEmgFlag(){ // Goflag EMG 00132 emgFlag = true; 00133 } 00134 void setLcdFlag(){ // Goflag EMG 00135 lcdFlag = true; 00136 } 00137 void btnSetAction(){ // Set knop K64F 00138 btn = true; // GoFlag setknop 00139 } 00140 void setControlFlag(){ // Go flag setButton 00141 controlFlag = true; 00142 } 00143 void setBreakFlag(){ // Go flag Break 00144 breakFlag = true; 00145 } 00146 void checkAim(){ // check if Killswitch is on or max counts is reached 00147 double volt = KS.read(); 00148 if(volt> 0.5 || abs(enc2.getPulses()) > (maxCounts -50) || enc2.getPulses() < 0){ 00149 pwmM2.write(0); // Aim motor freeze 00150 pc.printf("BOEM! CRASH! KASTUK! \r\n"); // Terminal 00151 PRINT("BOEM! CRASH!"); // LCD 00152 } 00153 } 00154 void motorAim(int dir){ // Rotate Aim motor with given direction 00155 dirM2.write(dir); 00156 pwmM2.write(0.25); 00157 } 00158 bool controlAim(){ // Function to control aim motor with modes 00159 bool both = false; // boolean if both modes are 3 00160 int modeL = defMode(emgL, potL, true); 00161 int modeR = defMode(emgR, potR, false); 00162 00163 scope.set(0, modeL); 00164 scope.set(1, modeR); 00165 scope.send(); //send values to HIDScope 00166 00167 if(modeR < 2 && !R){ // control if mode has been 1 00168 R = true; 00169 } 00170 if(modeL < 2 && !L){ 00171 L = true; 00172 } 00173 00174 if((modeL>2) && (modeR >2 && R && L)) { // mode L and mode R both 3, and both has been 1 herefore 00175 both = true; // Return true 00176 pwmM2.write(0); // Aim motor freeze 00177 aimState = OFF; // next state 00178 } 00179 else if((modeR == 2) && (modeL == 2)) { // modes are both 2 00180 if(aimState!=OFF){ // only if aim motor is rotating 00181 pwmM2.write(0); // Aim motor freeze 00182 aimState = OFF; // motor state is off 00183 pc.printf("Motor freeze\r\n"); // LCD 00184 L = false; // Modes must be first 1 for another action 00185 R = false; // "" 00186 } 00187 } 00188 else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1 00189 if(L){ 00190 aimState = CCW; // Rotate CCW 00191 pc.printf("Rotate -, CCW\r\n"); 00192 motorAim(0); 00193 } 00194 } 00195 else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1 00196 if(R){ 00197 aimState = CW; // Rotate CW 00198 pc.printf("Rotate +, CW\r\n"); 00199 motorAim(1); 00200 } 00201 } 00202 return both; 00203 } 00204 int main(){ 00205 flipLed(ledR); // test if code begins 00206 btnSet.mode(PullUp); // Button mode 00207 btnSet.rise(&btnSetAction); // Connect button to function 00208 tickerControl.attach(&setControlFlag,tControl); // ticker control motor 00209 tickerBreak.attach(&setBreakFlag,tBreak); // ticker break 00210 EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H 00211 tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd 00212 00213 pc.printf("\n\n\n\n\n"); // Terminal 00214 pc.printf("---NEW GAME---\r\n"); 00215 PRINT("--- NEW GAME ---"); // LCD 00216 while(1){ // Run program 00217 switch(state){ 00218 case CALIBRATE_EMG: { 00219 EMGkalibratieL(); // calibrate left EMG, determine thresholds 00220 EMGkalibratieR(); // calibrate right EMG, determine thresholds 00221 state = CALIBRATE_AIM; // Next state 00222 break; 00223 } 00224 case CALIBRATE_AIM: { 00225 pwmM2.period(1/100000); // period motor 2 00226 printf("Position is kalibrating\r\n"); // terminal 00227 PRINT("Wait a moment, please"); // LCD 00228 dirM2.write(0); // direction aim motor 00229 pwmM2.write(0.1); // percentage motor power 00230 bool calibrated = false; // 00231 while(state==CALIBRATE_AIM){ 00232 pc.printf("Killswitch: %f\r\n", KS.read()); // terminal 00233 if(KS.read() > 0.5){ // Killswitch? //2* gedaan, zodat breuk uit de if-statement is 00234 pwmM2.write(0); // Aim motor freeze 00235 enc2.reset(); // Reset encoder 00236 PRINT("Aim calibrated"); // LCD 00237 calibrated = true; // 00238 state = CALIBRATE_PEND; // next state 00239 //dirM2.write(1); // direction aim motor 00240 //pwmM2.write(0.25); // percentage motor power, turn it on 00241 } 00242 /* 00243 if(controlFlag && calibrated){ // motor regelen op GoFlag 00244 controlFlag = false; 00245 if(enc1.getPulses() > maxCounts/2){ // rotate aim motor to midi position 00246 pwmM2.write(0); // Aim motor freeze 00247 state = CALIBRATE_PEND; // next state 00248 } 00249 } */ 00250 } 00251 break; 00252 } 00253 case CALIBRATE_PEND: { 00254 pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal 00255 pwmM1.period(1/100000); // period beam motor 00256 servo.period(0.02); // 20ms period servo 00257 //pwmM1.write(0.25); // Turn motor on, low power 00258 btn = false; // Button is unpressed 00259 R = false; // modeR must become 1 00260 L = false; // modeL must become 1 00261 PRINT("Calibrate beam to 10 o'clock"); 00262 wait(1); 00263 PRINT("Flex right half to swing beam"); 00264 while(state==CALIBRATE_PEND){ 00265 if(emgFlag){ 00266 pc.printf(""); // lege regel printen, anders doet setknop het niet 00267 emgFlag = false; 00268 00269 int modeL = defMode(emgL, potL, true); // determine modeL 00270 int modeR = defMode(emgR, potR, false); // determine modeR 00271 00272 if(modeR < 2) { // modeR == 1 00273 R = true; 00274 } 00275 if(modeL < 2) { // modeL == 1 00276 L = true; 00277 } 00278 if (btn || (modeL == 3 && L) || (modeR == 3 && R)){ // If setbutton is on or one mode is 3, and has been 1 00279 pwmM1.write(0); // Motor 1 freeze 00280 enc1.reset(); // encoder 1 reset 00281 PRINT("Beam calibrated"); 00282 btn = false; // button op false 00283 state = AIM; // next state 00284 } 00285 else if(modeL == 2){ 00286 pwmM1.write(0); // beam freeze 00287 PRINT("Flex both full to continue"); // LCD 00288 } 00289 else if(modeR == 2){ 00290 pwmM1.write(0.025); // beam rotate 00291 PRINT("Flex left half to stop beam"); // LCD 00292 } 00293 } 00294 } 00295 lcd.cls(); 00296 break; 00297 } 00298 case AIM: { 00299 pc.printf("Aim with EMG\r\n"); // terminal 00300 int i = 0; // counter for lcd 00301 while(state == AIM){ 00302 if(lcdFlag){ // LCD loopje to project 3 texts on lcd 00303 lcdFlag = false; 00304 if(i%3 == 0){ 00305 PRINT("Flex left or right half to aim"); 00306 } 00307 else if(i%3 == 1){ 00308 PRINT("Flex both half to freeze"); 00309 } 00310 else { 00311 PRINT("Flex both full to continue"); 00312 } 00313 i++; 00314 } 00315 00316 if(controlFlag){ // motor control on GoFlag 00317 controlFlag = false; 00318 checkAim(); // Check position 00319 } 00320 if(emgFlag){ // Go flag EMG sampel 00321 emgFlag = false; 00322 if(controlAim()){ 00323 pc.printf("Position chosen\r\n"); // terminal 00324 state = BREAK; // Next state (BREAK) 00325 } 00326 } 00327 } 00328 lcd.cls(); 00329 break; 00330 } 00331 case BREAK: { 00332 pc.printf("Set break percentage, current is: %.0f\r\n", breakPerc); 00333 L = false; 00334 R = false; 00335 int i = 0; // counter for lcd 00336 while(state == BREAK){ 00337 if(lcdFlag){ // LCD loop to project text on LCD 00338 lcdFlag = false; 00339 if(i%2 == 0){ 00340 PRINT("Flex L or R to adjust break"); 00341 } 00342 else { 00343 PRINT("Flex both to continue"); 00344 } 00345 i++; 00346 } 00347 if(emgFlag){ // Go flag EMG sampelen 00348 emgFlag = false; 00349 int modeL = defMode(emgL, potL, true); 00350 int modeR = defMode(emgR, potR, false); 00351 00352 if(modeR < 2) { // modeR is 1 00353 R = true; 00354 } 00355 if(modeL < 2) { // modeL is 1 00356 L = true; 00357 } 00358 if(L && R){ 00359 if((modeL > 1) && modeR > 1) { // both modes 00360 state = FIRE; 00361 R = false; 00362 L = false; 00363 } 00364 else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire 00365 state = FIRE; 00366 R = false; 00367 L = false; 00368 } 00369 else if((modeL == 2) && L) { // modeL = BREAK lower with one 00370 if(breakPerc>1){ 00371 breakPerc--; 00372 } 00373 L = false; 00374 } 00375 else if((modeR == 2) && R) { // modeR = Break +1 00376 if(breakPerc<10){ 00377 breakPerc++; 00378 } 00379 R = false; 00380 } 00381 if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1 00382 pc.printf("Current breaking scale: %i\r\n", breakPerc); 00383 lcd.cls(); 00384 lcd.printf("Break scale: %i", breakPerc); 00385 } 00386 } 00387 } 00388 } 00389 L = false; // modeL must be one for another action can take place 00390 R = false; // modeR "" 00391 lcd.cls(); 00392 break; 00393 } 00394 case FIRE: { 00395 pc.printf("Shooting\r\n"); 00396 PRINT("FIRING"); 00397 servo.pulsewidth(servoL); // BREAK release 00398 pwmM1.write(0.25); // beam motor on 00399 bool servoPos = true; 00400 while(state == FIRE){ // while in FIRE state 00401 if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){ // BREAK goFlag half rotation beam = breaking 00402 breakFlag = false; 00403 if(servoPos){ 00404 servoPos = false; 00405 servo.pulsewidth(servoL); // left 00406 } 00407 else{ 00408 servoPos = true; 00409 servo.pulsewidth(servoR); // right 00410 } 00411 } 00412 if(controlFlag){ 00413 controlFlag = false; 00414 if((abs(enc1.getPulses())+50)%4200 < 150){ // rotate beam one revolution 00415 pwmM1.write(0); // motor beam off 00416 pc.printf("Beam on startposition\r\n"); 00417 state = AIM; // Next stage 00418 } 00419 } 00420 } 00421 lcd.cls(); 00422 break; 00423 } 00424 } 00425 } 00426 }
Generated on Fri Aug 5 2022 02:26:40 by
1.7.2