Remco Dasselaar / Mbed 2 deprecated 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

main.cpp@48:07e3cf7dc819, 2015-10-27 (annotated)

Committer:
RemcoDas
Date:
Tue Oct 27 11:58:16 2015 +0000
Revision:
48:07e3cf7dc819
Parent:
47:959ef2792024
Child:
49:b571c822c3f9
Final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RemcoDas 33:b4757132437e 1 //--------------------Include files and libraries-------
Bartvaart 0:557b1ff83a8a 2 #include "mbed.h"
RemcoDas 24:ddd69385b55f 3 #include "QEI.h"
RemcoDas 24:ddd69385b55f 4 #include "MODSERIAL.h"
RemcoDas 24:ddd69385b55f 5 #include "TextLCD.h"
Bartvaart 7:040591b3f019 6 #include "HIDScope.h"
Bartvaart 5:b400209df739 7 #include "Filterdesigns.h"
Bartvaart 17:cfe44346645c 8 #include "Kalibratie.h"
Bartvaart 18:eec0880fcded 9 #include "Mode.h"
RemcoDas 24:ddd69385b55f 10 //--------------------Classes------------------------
RemcoDas 33:b4757132437e 11 InterruptIn btnSet(PTC6); // Kalibration button
RemcoDas 46:ed8ada80ba17 12 InterruptIn emgSet(PTA4); // EMG on/off button
RemcoDas 44:97f5622db2c4 13 //DigitalOut ledR(LED_RED), LedB(LED3); // Leds on K64F
RemcoDas 24:ddd69385b55f 14 MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty
RemcoDas 33:b4757132437e 15 TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD screen on inner row of K64F, rs, e, d4-d7
RemcoDas 24:ddd69385b55f 16 PwmOut servo(D3); // PwmOut servo
RemcoDas 33:b4757132437e 17 AnalogIn emgL(A0), emgR(A1); // Analog input of EMG, left and right
RemcoDas 33:b4757132437e 18 AnalogIn potL(A2), potR(A3); // Potential meter left and right
RemcoDas 44:97f5622db2c4 19 AnalogIn ksLeft(A5), ksRight(A4); // Killswitch left and right
RemcoDas 39:f0f9b3432f43 20 HIDScope scope(2); // Hidscope, amount of scopes
RemcoDas 43:929cab5358ee 21 Ticker EMGticker, tickerControl, tickerLcd; // Ticker for EMG, regulator and break
RemcoDas 33:b4757132437e 22 // QEI Encoder(port 1, port 2, ,counts/rev
RemcoDas 25:c97d079e07f3 23 QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
RemcoDas 33:b4757132437e 24 // Motor1 met PWM power control and direction
RemcoDas 24:ddd69385b55f 25 PwmOut pwmM1(D6);
RemcoDas 24:ddd69385b55f 26 DigitalOut dirM1(D7);
RemcoDas 33:b4757132437e 27 // Motor2 met PWM power control and direction
RemcoDas 24:ddd69385b55f 28 PwmOut pwmM2(D5);
RemcoDas 24:ddd69385b55f 29 DigitalOut dirM2(D4);
RemcoDas 44:97f5622db2c4 30 enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_BEAM, AIM, BREAK, FIRE}; // Program states, ACKNOWLEDGEMENT switch: BMT groep 4 2014
RemcoDas 41:91c8c39d7a33 31 uint8_t state = CALIBRATE_EMG; // first state program
RemcoDas 33:b4757132437e 32 enum aimFase {OFF, CW, CCW}; // Aim motor possible states
RemcoDas 33:b4757132437e 33 uint8_t aimState = OFF; // first state aim motor
RemcoDas 25:c97d079e07f3 34 //-------------------------------Variables---------------------------------------------------------------------
RemcoDas 43:929cab5358ee 35 int maxCounts = 13000; // max richt motor counts Aim motor
RemcoDas 33:b4757132437e 36 int breakPerc = 0;
RemcoDas 47:959ef2792024 37 volatile int modeL = 1, modeR = 1;
RemcoDas 46:ed8ada80ba17 38 const double servoBreak = 0.00165, servoFree = 0.002; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
RemcoDas 43:929cab5358ee 39 const double tControl = 0.05, tLcd = 2; // ticker time (sec)
RemcoDas 43:929cab5358ee 40 const double Fs = 200; //Sample frequency Hz
RemcoDas 43:929cab5358ee 41 double t = 1/ Fs; // time EMGticker
RemcoDas 27:f62e450bb411 42 double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
RemcoDas 27:f62e450bb411 43 double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
RemcoDas 33:b4757132437e 44 double yL = 0, yR = 0; // y values EMG left and right
RemcoDas 33:b4757132437e 45 volatile bool L = false, R = false; // Booleans for checking if mode. has been 1?
RemcoDas 33:b4757132437e 46 volatile bool btn = false; // Button is pressed?
RemcoDas 46:ed8ada80ba17 47 volatile bool controlFlag = false, btnFlag = false, emgFlag = false, lcdFlag = false, runEmg = true; // Go flags
RemcoDas 46:ed8ada80ba17 48 bool boolBrake = false;
RemcoDas 24:ddd69385b55f 49 //----------------------------Functions-----------------------------------------------------------------------
RemcoDas 33:b4757132437e 50 void PRINT(const char* text){
RemcoDas 25:c97d079e07f3 51 lcd.cls(); // clear LCD scherm
RemcoDas 33:b4757132437e 52 lcd.printf(text); // print text op lcd
RemcoDas 25:c97d079e07f3 53 }
RichardRoos 34:60391fb72629 54 void EMGkalibratieL(){ // Determine thresholds left
RichardRoos 34:60391fb72629 55 PRINT("EMG LEFT relax muscle");
RichardRoos 34:60391fb72629 56 double ymin = KalibratieMin(emgL, true); // Minimum value left EMG, boolean indicates left
RemcoDas 27:f62e450bb411 57 wait(1);
RichardRoos 34:60391fb72629 58 PRINT("EMG LEFT flex muscle"); // LCD
RichardRoos 34:60391fb72629 59 double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left
RichardRoos 34:60391fb72629 60 PRINT("EMG LEFT well done!"); // LCD
RemcoDas 27:f62e450bb411 61
RemcoDas 46:ed8ada80ba17 62 if((ymax-ymin) < 0.05 || !runEmg){ // No EMG connected
RemcoDas 27:f62e450bb411 63 ymin = 10;
RemcoDas 27:f62e450bb411 64 ymax = 10;
RemcoDas 27:f62e450bb411 65 }
RemcoDas 33:b4757132437e 66 thresholdlowL = 4 * ymin; // Lowest threshold
RemcoDas 33:b4757132437e 67 thresholdmidL = 0.5 * ymax; // Midi threshold
RemcoDas 33:b4757132437e 68 thresholdhighL = 0.8 * ymax; // Highest threshold
RemcoDas 27:f62e450bb411 69
RemcoDas 33:b4757132437e 70 pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
RemcoDas 27:f62e450bb411 71 }
RemcoDas 33:b4757132437e 72 void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
RichardRoos 34:60391fb72629 73 PRINT("EMG RIGHT relax muscle");
RemcoDas 30:8ae855348d22 74 double ymin = KalibratieMin(emgR, false);
Bartvaart 19:6c0245063b96 75 wait(1);
RemcoDas 41:91c8c39d7a33 76 PRINT("EMG RIGHT flex muscle");
RemcoDas 30:8ae855348d22 77 double ymax = KalibratieMax(emgR, false);
RemcoDas 41:91c8c39d7a33 78 PRINT("EMG LEFT well done!");
RemcoDas 27:f62e450bb411 79
RemcoDas 46:ed8ada80ba17 80 if((ymax-ymin) < 0.05|| !runEmg){
RemcoDas 27:f62e450bb411 81 ymin = 10;
RemcoDas 27:f62e450bb411 82 ymax = 10;
RemcoDas 29:9610df479f89 83 }
RemcoDas 33:b4757132437e 84 thresholdlowR = 4 * ymin;
RemcoDas 33:b4757132437e 85 thresholdmidR = 0.5 * ymax;
RemcoDas 27:f62e450bb411 86 thresholdhighR = 0.8 * ymax;
Bartvaart 22:c1811e60bfce 87
RemcoDas 33:b4757132437e 88 pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal
RemcoDas 24:ddd69385b55f 89 }
RemcoDas 27:f62e450bb411 90 int EMGfilter(AnalogIn& emg, bool side){
RemcoDas 33:b4757132437e 91 double u = emg.read(); // read emg signal (left or right EMG)
RemcoDas 27:f62e450bb411 92 int mode = 1;
RemcoDas 30:8ae855348d22 93 if(side){
RemcoDas 33:b4757132437e 94 double y = FilterdesignsLeft(u); // filter signal left EMG
RemcoDas 47:959ef2792024 95 mode = Mode(modeL, y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3)
RemcoDas 27:f62e450bb411 96 }
RemcoDas 30:8ae855348d22 97 else {
RemcoDas 33:b4757132437e 98 double y = FilterdesignsRight(u);
RemcoDas 47:959ef2792024 99 mode = Mode(modeR, y, thresholdlowR, thresholdmidR, thresholdhighR); // right EMG
RemcoDas 27:f62e450bb411 100 }
RemcoDas 25:c97d079e07f3 101 return mode;
RemcoDas 26:d9855716ced7 102 }
RemcoDas 33:b4757132437e 103 int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
RemcoDas 26:d9855716ced7 104 double volt = pot.read();
RemcoDas 41:91c8c39d7a33 105 int mode = 1;
RemcoDas 26:d9855716ced7 106 if(volt > 0.8){
RemcoDas 26:d9855716ced7 107 mode = 3;
RemcoDas 26:d9855716ced7 108 }
RemcoDas 26:d9855716ced7 109 else if(volt>0.35 && volt<0.65){
RemcoDas 26:d9855716ced7 110 mode = 2;
RemcoDas 26:d9855716ced7 111 }
RemcoDas 26:d9855716ced7 112 return mode;
RemcoDas 26:d9855716ced7 113 }
RemcoDas 33:b4757132437e 114 int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // Determine mode both from EMG and Potentialmeter, ONE of them must be ONE!
RemcoDas 27:f62e450bb411 115 int emgMode = EMGfilter(emg, side);
RemcoDas 26:d9855716ced7 116 int potMode = PotReader(pot);
RemcoDas 26:d9855716ced7 117 int mode = 1;
RemcoDas 33:b4757132437e 118 if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
RemcoDas 33:b4757132437e 119 if(emgMode > potMode){ // maximum of emg and pot
RemcoDas 26:d9855716ced7 120 mode = emgMode;
RemcoDas 26:d9855716ced7 121 }
RemcoDas 26:d9855716ced7 122 else{
RemcoDas 26:d9855716ced7 123 mode = potMode;
RemcoDas 26:d9855716ced7 124 }
RemcoDas 48:07e3cf7dc819 125 }
RemcoDas 26:d9855716ced7 126 return mode;
RemcoDas 26:d9855716ced7 127 }
RemcoDas 33:b4757132437e 128 void setEmgFlag(){ // Goflag EMG
RemcoDas 25:c97d079e07f3 129 emgFlag = true;
RichardRoos 34:60391fb72629 130 }
RichardRoos 34:60391fb72629 131 void setLcdFlag(){ // Goflag EMG
RichardRoos 34:60391fb72629 132 lcdFlag = true;
RichardRoos 34:60391fb72629 133 }
RemcoDas 24:ddd69385b55f 134 void btnSetAction(){ // Set knop K64F
RemcoDas 24:ddd69385b55f 135 btn = true; // GoFlag setknop
RemcoDas 46:ed8ada80ba17 136 }
RemcoDas 46:ed8ada80ba17 137 void emgOnOff(){ // Set knop K64F, push
RemcoDas 46:ed8ada80ba17 138 runEmg = !runEmg; // switch enable emg
RemcoDas 46:ed8ada80ba17 139 pc.printf("EMG is enabled: &i\r\n", runEmg);
RemcoDas 26:d9855716ced7 140 }
RemcoDas 33:b4757132437e 141 void setControlFlag(){ // Go flag setButton
RemcoDas 33:b4757132437e 142 controlFlag = true;
RemcoDas 24:ddd69385b55f 143 }
RemcoDas 44:97f5622db2c4 144 void checkSide(AnalogIn& ks, int dir){
RemcoDas 45:520d86583ff6 145 if((dirM2.read() == dir) && (pwmM2.read()>0)){ // direction motor clashes with killswitch and motor is on
RemcoDas 44:97f5622db2c4 146 if(ks.read()>0.8){
RemcoDas 39:f0f9b3432f43 147 pwmM2.write(0); // Aim motor freeze
RemcoDas 39:f0f9b3432f43 148 pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
RemcoDas 39:f0f9b3432f43 149 PRINT("BOEM! CRASH!"); // LCD
RemcoDas 39:f0f9b3432f43 150 }
RemcoDas 44:97f5622db2c4 151 }
RemcoDas 46:ed8ada80ba17 152 }
RemcoDas 45:520d86583ff6 153 void checkAim(){ // check if killswitch clashes with direction motor
RemcoDas 44:97f5622db2c4 154 checkSide(ksLeft, 1);
RemcoDas 44:97f5622db2c4 155 checkSide(ksRight, 0);
RemcoDas 24:ddd69385b55f 156 }
RemcoDas 33:b4757132437e 157 void motorAim(int dir){ // Rotate Aim motor with given direction
RemcoDas 33:b4757132437e 158 dirM2.write(dir);
RemcoDas 44:97f5622db2c4 159 pwmM2.write(0.2);
RemcoDas 24:ddd69385b55f 160 }
RemcoDas 33:b4757132437e 161 bool controlAim(){ // Function to control aim motor with modes
RemcoDas 33:b4757132437e 162 bool both = false; // boolean if both modes are 3
RemcoDas 47:959ef2792024 163 modeL = defMode(emgL, potL, true); // determine mode left
RemcoDas 47:959ef2792024 164 modeR = defMode(emgR, potR, false); // determine mode right
RemcoDas 27:f62e450bb411 165
RemcoDas 27:f62e450bb411 166 scope.set(0, modeL);
RemcoDas 27:f62e450bb411 167 scope.set(1, modeR);
RemcoDas 33:b4757132437e 168 scope.send(); //send values to HIDScope
RemcoDas 27:f62e450bb411 169
RemcoDas 33:b4757132437e 170 if(modeR < 2 && !R){ // control if mode has been 1
RemcoDas 27:f62e450bb411 171 R = true;
RemcoDas 27:f62e450bb411 172 }
RemcoDas 28:e6d2fe0e593e 173 if(modeL < 2 && !L){
RemcoDas 28:e6d2fe0e593e 174 L = true;
RemcoDas 24:ddd69385b55f 175 }
RemcoDas 28:e6d2fe0e593e 176
RemcoDas 46:ed8ada80ba17 177 if((modeL>2 && L) && (modeR >2 && R)) { // mode L and mode R both 3, and both has been 1 herefore
RemcoDas 26:d9855716ced7 178 both = true; // Return true
RemcoDas 33:b4757132437e 179 pwmM2.write(0); // Aim motor freeze
RemcoDas 33:b4757132437e 180 aimState = OFF; // next state
RemcoDas 28:e6d2fe0e593e 181 }
RemcoDas 46:ed8ada80ba17 182 /*else if((modeR == 2 && R) && (modeL == 2 && L)) { // modes are both 2
RemcoDas 45:520d86583ff6 183 both = true; // Return true
RemcoDas 45:520d86583ff6 184 pwmM2.write(0); // Aim motor freeze
RemcoDas 45:520d86583ff6 185 aimState = OFF; // next state
RemcoDas 46:ed8ada80ba17 186 }*/
RemcoDas 45:520d86583ff6 187 else if((modeR == 2)) { // modeR ==2
RemcoDas 45:520d86583ff6 188 if(aimState != CCW){
RemcoDas 43:929cab5358ee 189 aimState = CCW; // Rotate CCW
RichardRoos 34:60391fb72629 190 pc.printf("Rotate -, CCW\r\n");
RemcoDas 44:97f5622db2c4 191 PRINT("Rotate CCW");
RemcoDas 45:520d86583ff6 192 motorAim(0);
RemcoDas 28:e6d2fe0e593e 193 }
RemcoDas 24:ddd69385b55f 194 }
RemcoDas 45:520d86583ff6 195 else if((modeL == 2)) { // modeL == 2
RemcoDas 45:520d86583ff6 196 if(aimState != CW){
RemcoDas 43:929cab5358ee 197 aimState = CW; // Rotate CW
RichardRoos 34:60391fb72629 198 pc.printf("Rotate +, CW\r\n");
RemcoDas 44:97f5622db2c4 199 PRINT("Rotate CW");
RemcoDas 28:e6d2fe0e593e 200 motorAim(1);
RemcoDas 28:e6d2fe0e593e 201 }
RemcoDas 45:520d86583ff6 202 }
RemcoDas 45:520d86583ff6 203 // modeR AND CCW OR modeL and CW
RemcoDas 45:520d86583ff6 204 else if((modeR == 1 && aimState == CCW) || (modeL == 1 && aimState == CW)) { // R=1 en CW
RemcoDas 45:520d86583ff6 205 pwmM2.write(0); // Aim motor freeze
RemcoDas 45:520d86583ff6 206 aimState = OFF;
RemcoDas 45:520d86583ff6 207 pc.printf("Freeze\r\n");
RemcoDas 45:520d86583ff6 208 PRINT("Freeze");
RemcoDas 45:520d86583ff6 209 }
RemcoDas 24:ddd69385b55f 210 return both;
RemcoDas 24:ddd69385b55f 211 }
RemcoDas 43:929cab5358ee 212 int main(){
RemcoDas 24:ddd69385b55f 213 btnSet.mode(PullUp); // Button mode
RemcoDas 46:ed8ada80ba17 214 btnSet.rise(&btnSetAction); // Connect button to function
RemcoDas 46:ed8ada80ba17 215 emgSet.mode(PullUp); // Button mode
RemcoDas 46:ed8ada80ba17 216 emgSet.rise(&emgOnOff); // Connect button to function
RemcoDas 43:929cab5358ee 217 tickerControl.attach(&setControlFlag,tControl); // ticker control motor
RemcoDas 26:d9855716ced7 218 EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
RichardRoos 34:60391fb72629 219 tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd
RemcoDas 25:c97d079e07f3 220
RemcoDas 33:b4757132437e 221 pc.printf("\n\n\n\n\n"); // Terminal
RichardRoos 34:60391fb72629 222 pc.printf("---NEW GAME---\r\n");
RemcoDas 33:b4757132437e 223 PRINT("--- NEW GAME ---"); // LCD
RemcoDas 33:b4757132437e 224 while(1){ // Run program
RemcoDas 24:ddd69385b55f 225 switch(state){
RichardRoos 34:60391fb72629 226 case CALIBRATE_EMG: {
RichardRoos 34:60391fb72629 227 EMGkalibratieL(); // calibrate left EMG, determine thresholds
RichardRoos 34:60391fb72629 228 EMGkalibratieR(); // calibrate right EMG, determine thresholds
RichardRoos 34:60391fb72629 229 state = CALIBRATE_AIM; // Next state
RemcoDas 24:ddd69385b55f 230 break;
RemcoDas 24:ddd69385b55f 231 }
RichardRoos 34:60391fb72629 232 case CALIBRATE_AIM: {
RemcoDas 33:b4757132437e 233 pwmM2.period(1/100000); // period motor 2
RemcoDas 33:b4757132437e 234 printf("Position is kalibrating\r\n"); // terminal
RichardRoos 37:090ba5b1e655 235 PRINT("Wait a moment, please"); // LCD
RemcoDas 33:b4757132437e 236 dirM2.write(0); // direction aim motor
RemcoDas 46:ed8ada80ba17 237 pwmM2.write(0.2); // percentage motor power
RichardRoos 34:60391fb72629 238 bool calibrated = false; //
RemcoDas 38:08c8fd55a3bb 239 while(state==CALIBRATE_AIM){
RemcoDas 43:929cab5358ee 240 if(controlFlag){
RemcoDas 38:08c8fd55a3bb 241 controlFlag = false;
RemcoDas 39:f0f9b3432f43 242 if(!calibrated){ // Not calibrated
RemcoDas 44:97f5622db2c4 243 if((ksRight.read()>0.8)){ // Killswitch?
RemcoDas 39:f0f9b3432f43 244 pwmM2.write(0); // Aim motor freeze
RemcoDas 39:f0f9b3432f43 245 enc2.reset(); // Reset encoder
RemcoDas 39:f0f9b3432f43 246 PRINT("Aim calibrated"); // LCD
RemcoDas 39:f0f9b3432f43 247 pc.printf("Aim calibrated\r\n");// Terminal
RemcoDas 44:97f5622db2c4 248 calibrated = true;
RemcoDas 44:97f5622db2c4 249 pc.printf("Go to midi position\r\n");
RemcoDas 39:f0f9b3432f43 250 dirM2.write(1); // direction aim motor
RemcoDas 44:97f5622db2c4 251 pwmM2.write(0.2); // percentage motor power, turn it on
RemcoDas 39:f0f9b3432f43 252 }
RemcoDas 39:f0f9b3432f43 253 }
RemcoDas 44:97f5622db2c4 254 if(calibrated) { // Calibrated
RemcoDas 43:929cab5358ee 255 checkAim(); // Check killswitches
RemcoDas 43:929cab5358ee 256 if(abs(enc2.getPulses()) > 600){ // rotate aim motor to midi position
RemcoDas 39:f0f9b3432f43 257 pwmM2.write(0); // Aim motor freeze
RemcoDas 44:97f5622db2c4 258 state = CALIBRATE_BEAM; // next state
RemcoDas 39:f0f9b3432f43 259 }
RemcoDas 39:f0f9b3432f43 260 }
RemcoDas 39:f0f9b3432f43 261 }
RemcoDas 24:ddd69385b55f 262 }
RemcoDas 24:ddd69385b55f 263 break;
RemcoDas 24:ddd69385b55f 264 }
RemcoDas 44:97f5622db2c4 265 case CALIBRATE_BEAM: {
RemcoDas 39:f0f9b3432f43 266 pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
RemcoDas 39:f0f9b3432f43 267 dirM1.write(0); // direction beam motor
RichardRoos 34:60391fb72629 268 pwmM1.period(1/100000); // period beam motor
RemcoDas 33:b4757132437e 269 servo.period(0.02); // 20ms period servo
RichardRoos 34:60391fb72629 270 btn = false; // Button is unpressed
RemcoDas 46:ed8ada80ba17 271 PRINT("Calibrate beam to 10 o'clock");
RemcoDas 44:97f5622db2c4 272 while(state==CALIBRATE_BEAM){
RemcoDas 45:520d86583ff6 273 if(controlFlag){
RemcoDas 24:ddd69385b55f 274 pc.printf(""); // lege regel printen, anders doet setknop het niet
RemcoDas 45:520d86583ff6 275 controlFlag = false;
RemcoDas 45:520d86583ff6 276 if(btn){ // If setbutton is on or one mode is 3, and has been 1
RemcoDas 43:929cab5358ee 277 enc1.reset(); // encoder 1 reset
RichardRoos 34:60391fb72629 278 PRINT("Beam calibrated");
RemcoDas 38:08c8fd55a3bb 279 pc.printf("Beam calibrated\r\n");
RemcoDas 43:929cab5358ee 280 btn = false; // button op false
RemcoDas 43:929cab5358ee 281 state = AIM; // next state
RemcoDas 45:520d86583ff6 282 }
RemcoDas 24:ddd69385b55f 283 }
RemcoDas 24:ddd69385b55f 284 }
RichardRoos 35:012b2e045579 285 lcd.cls();
RemcoDas 24:ddd69385b55f 286 break;
RemcoDas 24:ddd69385b55f 287 }
RemcoDas 24:ddd69385b55f 288 case AIM: {
RichardRoos 34:60391fb72629 289 pc.printf("Aim with EMG\r\n"); // terminal
RemcoDas 39:f0f9b3432f43 290 int i = 0; // counter for lcd
RemcoDas 39:f0f9b3432f43 291 R = false;
RemcoDas 39:f0f9b3432f43 292 L = false;
RemcoDas 46:ed8ada80ba17 293 while(state == AIM){
RemcoDas 46:ed8ada80ba17 294 if(controlFlag){ // motor control on GoFlag
RemcoDas 46:ed8ada80ba17 295 controlFlag = false;
RemcoDas 46:ed8ada80ba17 296 checkAim(); // Check position
RemcoDas 46:ed8ada80ba17 297 }
RemcoDas 46:ed8ada80ba17 298 if(emgFlag){ // Go flag EMG sampel
RemcoDas 46:ed8ada80ba17 299 emgFlag = false;
RemcoDas 46:ed8ada80ba17 300 if(controlAim()){
RemcoDas 46:ed8ada80ba17 301 pc.printf("Position chosen\r\n"); // terminal
RemcoDas 46:ed8ada80ba17 302 state = BREAK; // Next state (BREAK)
RemcoDas 46:ed8ada80ba17 303 }
RemcoDas 46:ed8ada80ba17 304 }
RichardRoos 36:549d1ce7b96b 305 if(lcdFlag){ // LCD loopje to project 3 texts on lcd
RichardRoos 35:012b2e045579 306 lcdFlag = false;
RichardRoos 34:60391fb72629 307 if(i%3 == 0){
RemcoDas 44:97f5622db2c4 308 PRINT("Flex left or right half to aim");
RichardRoos 34:60391fb72629 309 }
RichardRoos 34:60391fb72629 310 else if(i%3 == 1){
RemcoDas 44:97f5622db2c4 311 PRINT("Flex both half to freeze");
RichardRoos 34:60391fb72629 312 }
RichardRoos 34:60391fb72629 313 else {
RemcoDas 44:97f5622db2c4 314 PRINT("Flex both full to continue");
RichardRoos 34:60391fb72629 315 }
RichardRoos 34:60391fb72629 316 i++;
RemcoDas 24:ddd69385b55f 317 }
RichardRoos 35:012b2e045579 318 }
RichardRoos 35:012b2e045579 319 lcd.cls();
RemcoDas 24:ddd69385b55f 320 break;
RemcoDas 24:ddd69385b55f 321 }
RemcoDas 33:b4757132437e 322 case BREAK: {
RemcoDas 44:97f5622db2c4 323 pc.printf("Set break percentage, current is: %i\r\n", breakPerc);
RemcoDas 24:ddd69385b55f 324 L = false;
RemcoDas 24:ddd69385b55f 325 R = false;
RemcoDas 44:97f5622db2c4 326 int i = 0; // counter for lcd
RemcoDas 44:97f5622db2c4 327 PRINT("L := -1, R := +1 L+R = continue");
RemcoDas 44:97f5622db2c4 328 wait(1);
RemcoDas 44:97f5622db2c4 329 while(state == BREAK){
RemcoDas 46:ed8ada80ba17 330 if(emgFlag){ // Go flag EMG sampelen
RemcoDas 44:97f5622db2c4 331
RemcoDas 26:d9855716ced7 332 emgFlag = false;
RemcoDas 47:959ef2792024 333 modeL = defMode(emgL, potL, true);
RemcoDas 47:959ef2792024 334 modeR = defMode(emgR, potR, false);
RemcoDas 43:929cab5358ee 335
RemcoDas 44:97f5622db2c4 336 if((modeL > 1) && (modeR > 1) && L && R) { // both modes > 1
RemcoDas 44:97f5622db2c4 337 state = FIRE;
RemcoDas 41:91c8c39d7a33 338 }
RemcoDas 44:97f5622db2c4 339 else if((modeL > 2 && L)|| (modeR > 2 && R)) { // left of right mode 3 = fire
RemcoDas 41:91c8c39d7a33 340 state = FIRE;
RemcoDas 41:91c8c39d7a33 341 }
RemcoDas 43:929cab5358ee 342 else {
RemcoDas 43:929cab5358ee 343 if(modeR < 2) { // modeR is 1
RemcoDas 43:929cab5358ee 344 R = true;
RemcoDas 43:929cab5358ee 345 }
RemcoDas 43:929cab5358ee 346 if(modeL < 2) { // modeL is 1
RemcoDas 43:929cab5358ee 347 L = true;
RemcoDas 43:929cab5358ee 348 }
RemcoDas 43:929cab5358ee 349 if(L && R){ // L and R has been 1
RemcoDas 43:929cab5358ee 350 if((modeL == 2) && L) { // modeL = BREAK lower with one
RemcoDas 46:ed8ada80ba17 351 if(boolBrake){
RemcoDas 46:ed8ada80ba17 352 boolBrake = false;
RemcoDas 43:929cab5358ee 353 }
RemcoDas 43:929cab5358ee 354 L = false;
RemcoDas 43:929cab5358ee 355 }
RemcoDas 43:929cab5358ee 356 else if((modeR == 2) && R) { // modeR = Break +1
RemcoDas 46:ed8ada80ba17 357 if(!boolBrake){
RemcoDas 46:ed8ada80ba17 358 boolBrake = true;
RemcoDas 43:929cab5358ee 359 }
RemcoDas 43:929cab5358ee 360 R = false;
RemcoDas 43:929cab5358ee 361 }
RemcoDas 43:929cab5358ee 362 if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
RemcoDas 46:ed8ada80ba17 363 pc.printf("Current breaking scale: %i \r\n", boolBrake);
RemcoDas 44:97f5622db2c4 364
RemcoDas 43:929cab5358ee 365 }
RemcoDas 43:929cab5358ee 366 }
RemcoDas 46:ed8ada80ba17 367 }
RemcoDas 46:ed8ada80ba17 368 if(lcdFlag){ // every 2 seconds update lcd
RemcoDas 46:ed8ada80ba17 369 lcdFlag = false;
RemcoDas 46:ed8ada80ba17 370 lcd.cls();
RemcoDas 46:ed8ada80ba17 371 lcd.printf("Break scale: %i", boolBrake);
RemcoDas 26:d9855716ced7 372 }
RemcoDas 24:ddd69385b55f 373 }
RemcoDas 24:ddd69385b55f 374 }
RemcoDas 44:97f5622db2c4 375 PRINT("Break fixed"); // lcd
RemcoDas 44:97f5622db2c4 376 pc.printf("Break fixed\r\n"); // terminal
RemcoDas 33:b4757132437e 377 L = false; // modeL must be one for another action can take place
RichardRoos 35:012b2e045579 378 R = false; // modeR ""
RichardRoos 35:012b2e045579 379 lcd.cls();
RemcoDas 24:ddd69385b55f 380 break;
RemcoDas 24:ddd69385b55f 381 }
RemcoDas 24:ddd69385b55f 382 case FIRE: {
RemcoDas 43:929cab5358ee 383 pc.printf("Shooting\r\n"); // terminal
RemcoDas 43:929cab5358ee 384 PRINT("FIRING"); // lcd
RemcoDas 47:959ef2792024 385 pwmM1.write(0.3); // beam motor on
RemcoDas 46:ed8ada80ba17 386 bool breaking = false;
RemcoDas 46:ed8ada80ba17 387 if(boolBrake){
RemcoDas 46:ed8ada80ba17 388 servo.pulsewidth(servoBreak); // Servo to break position
RemcoDas 46:ed8ada80ba17 389 breaking = true; // boolean breaking?
RemcoDas 46:ed8ada80ba17 390 }
RemcoDas 46:ed8ada80ba17 391 //int countBreak = 2100*abs(breakPerc)/10; // Amount of counts where must be breaked
RemcoDas 40:35c7c60d7262 392 while(state == FIRE){ // while in FIRE state
RemcoDas 43:929cab5358ee 393 if(controlFlag){ // BREAK goFlag half rotation beam = breaking
RemcoDas 41:91c8c39d7a33 394 controlFlag = false; // GoFlag
RemcoDas 46:ed8ada80ba17 395 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
RemcoDas 46:ed8ada80ba17 396 if(breaking && boolBrake){
RemcoDas 46:ed8ada80ba17 397 if((counts+2100)%4200 < 100){
RemcoDas 43:929cab5358ee 398 servo.pulsewidth(servoFree); // Servo to free position
RemcoDas 40:35c7c60d7262 399 breaking = false;
RemcoDas 40:35c7c60d7262 400 }
RemcoDas 24:ddd69385b55f 401 }
RemcoDas 46:ed8ada80ba17 402 if(!breaking || !boolBrake){ // Not breaking
RemcoDas 47:959ef2792024 403 if((counts-100)%4200 < 100){ // rotated 1 rev?
RemcoDas 41:91c8c39d7a33 404 pwmM1.write(0); // motor beam off
RemcoDas 41:91c8c39d7a33 405 pc.printf("Beam on startposition\r\n"); // terminal
RemcoDas 41:91c8c39d7a33 406 state = AIM; // Next stage
RemcoDas 40:35c7c60d7262 407 }
RemcoDas 24:ddd69385b55f 408 }
RemcoDas 24:ddd69385b55f 409 }
RichardRoos 36:549d1ce7b96b 410 }
RichardRoos 36:549d1ce7b96b 411 lcd.cls();
RemcoDas 24:ddd69385b55f 412 break;
RemcoDas 24:ddd69385b55f 413 }
Bartvaart 19:6c0245063b96 414 }
Bartvaart 17:cfe44346645c 415 }
RemcoDas 24:ddd69385b55f 416 }