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@44:97f5622db2c4, 2015-10-22 (annotated)

Committer:
RemcoDas
Date:
Thu Oct 22 12:14:24 2015 +0000
Revision:
44:97f5622db2c4
Parent:
43:929cab5358ee
Child:
45:520d86583ff6
Werkend met EMG niet vasthouden met aimen

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