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@56:1ac2487a9610, 2015-10-29 (annotated)

Committer:
RemcoDas
Date:
Thu Oct 29 13:48:53 2015 +0000
Revision:
56:1ac2487a9610
Parent:
55:a435e46299ea
Child:
57:eea0d7265b22
FinalFinalFinal

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