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