Total control sjoel robot
Dependencies: HIDScope MODSERIAL QEI TextLCD mbed
main.cpp@29:9610df479f89, 2015-10-17 (annotated)
- Committer:
- RemcoDas
- Date:
- Sat Oct 17 14:13:57 2015 +0000
- Revision:
- 29:9610df479f89
- Parent:
- 28:e6d2fe0e593e
- Child:
- 30:8ae855348d22
EMG haalt links en rechts door elkaar, komt wrs door filterdisigns.cpp en filter.cpp
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Bartvaart | 0:557b1ff83a8a | 1 | #include "mbed.h" |
RemcoDas | 24:ddd69385b55f | 2 | #include "QEI.h" |
RemcoDas | 24:ddd69385b55f | 3 | #include "MODSERIAL.h" |
RemcoDas | 24:ddd69385b55f | 4 | #include "TextLCD.h" |
Bartvaart | 7:040591b3f019 | 5 | #include "HIDScope.h" |
Bartvaart | 5:b400209df739 | 6 | #include "Filterdesigns.h" |
Bartvaart | 17:cfe44346645c | 7 | #include "Kalibratie.h" |
Bartvaart | 18:eec0880fcded | 8 | #include "Mode.h" |
RemcoDas | 24:ddd69385b55f | 9 | //--------------------Classes------------------------ |
RemcoDas | 24:ddd69385b55f | 10 | InterruptIn btnSet(PTC6); // kalibreer knop |
RemcoDas | 25:c97d079e07f3 | 11 | DigitalOut ledR(LED_RED), LedB(LED3); // Led op moederbord |
RemcoDas | 24:ddd69385b55f | 12 | MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty |
RemcoDas | 24:ddd69385b55f | 13 | TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD scherm op binnenste rij van K64F, rs, e, d4-d7 |
RemcoDas | 24:ddd69385b55f | 14 | PwmOut servo(D3); // PwmOut servo |
RemcoDas | 27:f62e450bb411 | 15 | AnalogIn emgL(A0), emgR(A1); // Analog input van emg kabels links en rechts |
RemcoDas | 27:f62e450bb411 | 16 | AnalogIn potL(A2), potR(A3); // Intensiteit 'EMG' signaal door potmeter |
RemcoDas | 24:ddd69385b55f | 17 | AnalogIn KS(A4); // Killswitch |
RemcoDas | 25:c97d079e07f3 | 18 | HIDScope scope(6); // 3 scopes |
RemcoDas | 26:d9855716ced7 | 19 | Ticker EMGticker, tickerRegelaar, tickerRem; // regelaar button en rem ticker |
Bartvaart | 0:557b1ff83a8a | 20 | |
RemcoDas | 24:ddd69385b55f | 21 | // QEI Encoder(poort 1, poort 2, ,counts/rev |
RemcoDas | 25:c97d079e07f3 | 22 | QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64); |
RemcoDas | 24:ddd69385b55f | 23 | // Motor1 met PWM aansturen en richting aangeven |
RemcoDas | 24:ddd69385b55f | 24 | PwmOut pwmM1(D6); |
RemcoDas | 24:ddd69385b55f | 25 | DigitalOut dirM1(D7); |
RemcoDas | 24:ddd69385b55f | 26 | // Motor2 met PWM aansturen en richting aangeven |
RemcoDas | 24:ddd69385b55f | 27 | PwmOut pwmM2(D5); |
RemcoDas | 24:ddd69385b55f | 28 | DigitalOut dirM2(D4); |
RemcoDas | 27:f62e450bb411 | 29 | enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE}; // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014 |
RemcoDas | 27:f62e450bb411 | 30 | uint8_t state = KALIBRATE_EMG; // eerste spelfase |
RemcoDas | 24:ddd69385b55f | 31 | enum aimFase {OFF, CW, CCW}; // Aim motor mogelijkheden |
RemcoDas | 24:ddd69385b55f | 32 | uint8_t aimState = OFF; // mogelijkheid begin |
RemcoDas | 25:c97d079e07f3 | 33 | //-------------------------------Variables--------------------------------------------------------------------- |
RemcoDas | 25:c97d079e07f3 | 34 | const int on = 0, off = 1; // aan / uit |
RemcoDas | 25:c97d079e07f3 | 35 | int maxCounts = 2100; // max richt motor counts Aim motor |
RemcoDas | 25:c97d079e07f3 | 36 | int remPerc = 0; |
RemcoDas | 25:c97d079e07f3 | 37 | |
RemcoDas | 24:ddd69385b55f | 38 | const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R (= pulsewidth pwm servo) |
RemcoDas | 26:d9855716ced7 | 39 | const double tRegelaar = 0.005, tRem = 0.1; // tijd ticker voor regelaar en knoppen/EMG |
RemcoDas | 27:f62e450bb411 | 40 | const double Fs = 50; //Sample frequentie Hz |
RemcoDas | 25:c97d079e07f3 | 41 | double t = 1/ Fs; // tijd EMGticker |
RemcoDas | 27:f62e450bb411 | 42 | double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0; |
RemcoDas | 27:f62e450bb411 | 43 | double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0; |
RemcoDas | 25:c97d079e07f3 | 44 | |
RemcoDas | 27:f62e450bb411 | 45 | volatile bool L = false, R = false, RH = false; // Booleans voor knop link en knop rechts |
RemcoDas | 25:c97d079e07f3 | 46 | volatile bool btn = false; // boolean om programma te runnen, drukknop ingedrukt |
RemcoDas | 25:c97d079e07f3 | 47 | volatile bool regelaarFlag = false, btnFlag = false, remFlag = false; // Go flags |
RemcoDas | 25:c97d079e07f3 | 48 | volatile bool emgFlag = false; |
RemcoDas | 24:ddd69385b55f | 49 | //----------------------------Functions----------------------------------------------------------------------- |
RemcoDas | 25:c97d079e07f3 | 50 | void flipLed(DigitalOut& led){ //function to flip one LED |
RemcoDas | 25:c97d079e07f3 | 51 | led.write(on); |
RemcoDas | 25:c97d079e07f3 | 52 | wait(0.2); |
RemcoDas | 25:c97d079e07f3 | 53 | led.write(off); |
RemcoDas | 25:c97d079e07f3 | 54 | } |
RemcoDas | 25:c97d079e07f3 | 55 | void PRINT(const char* text){ // putty en lcd print |
RemcoDas | 25:c97d079e07f3 | 56 | lcd.cls(); // clear LCD scherm |
RemcoDas | 25:c97d079e07f3 | 57 | lcd.printf(text); // print text op lcd |
RemcoDas | 25:c97d079e07f3 | 58 | pc.printf(text); // print text op terminal |
RemcoDas | 25:c97d079e07f3 | 59 | } |
RemcoDas | 27:f62e450bb411 | 60 | void EMGkalibratieL(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn |
RemcoDas | 27:f62e450bb411 | 61 | LedB = 1; |
RemcoDas | 27:f62e450bb411 | 62 | Init(); |
RemcoDas | 27:f62e450bb411 | 63 | double ymin = KalibratieMin(emgL); |
RemcoDas | 27:f62e450bb411 | 64 | wait(1); |
RemcoDas | 27:f62e450bb411 | 65 | double ymax = KalibratieMax(emgL); |
RemcoDas | 27:f62e450bb411 | 66 | |
RemcoDas | 27:f62e450bb411 | 67 | if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten |
RemcoDas | 27:f62e450bb411 | 68 | ymin = 10; |
RemcoDas | 27:f62e450bb411 | 69 | ymax = 10; |
RemcoDas | 27:f62e450bb411 | 70 | } |
RemcoDas | 27:f62e450bb411 | 71 | thresholdlowL = 8 * ymin; // standaardwaarde |
RemcoDas | 27:f62e450bb411 | 72 | thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker |
RemcoDas | 27:f62e450bb411 | 73 | thresholdhighL = 0.8 * ymax; |
RemcoDas | 27:f62e450bb411 | 74 | |
RemcoDas | 27:f62e450bb411 | 75 | pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); //bugfix |
RemcoDas | 27:f62e450bb411 | 76 | } |
RemcoDas | 27:f62e450bb411 | 77 | void EMGkalibratieR(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn |
RemcoDas | 25:c97d079e07f3 | 78 | LedB = 1; |
RemcoDas | 24:ddd69385b55f | 79 | Init(); |
RemcoDas | 27:f62e450bb411 | 80 | double ymin = KalibratieMin(emgR); |
Bartvaart | 19:6c0245063b96 | 81 | wait(1); |
RemcoDas | 27:f62e450bb411 | 82 | double ymax = KalibratieMax(emgR); |
RemcoDas | 27:f62e450bb411 | 83 | |
RemcoDas | 27:f62e450bb411 | 84 | if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten |
RemcoDas | 27:f62e450bb411 | 85 | ymin = 10; |
RemcoDas | 27:f62e450bb411 | 86 | ymax = 10; |
RemcoDas | 29:9610df479f89 | 87 | } |
RemcoDas | 27:f62e450bb411 | 88 | thresholdlowR = 8 * ymin; // standaardwaarde |
RemcoDas | 27:f62e450bb411 | 89 | thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker |
RemcoDas | 27:f62e450bb411 | 90 | thresholdhighR = 0.8 * ymax; |
Bartvaart | 22:c1811e60bfce | 91 | |
RemcoDas | 27:f62e450bb411 | 92 | pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix |
RemcoDas | 24:ddd69385b55f | 93 | } |
RemcoDas | 27:f62e450bb411 | 94 | int EMGfilter(AnalogIn& emg, bool side){ |
RemcoDas | 25:c97d079e07f3 | 95 | double u = emg.read(); // uitlezen emg signaal |
RemcoDas | 27:f62e450bb411 | 96 | double y = Filterdesigns(u); // signaal filteren |
RemcoDas | 27:f62e450bb411 | 97 | int mode = 1; |
RemcoDas | 27:f62e450bb411 | 98 | if(side){ // linker EMG |
RemcoDas | 27:f62e450bb411 | 99 | mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) |
RemcoDas | 27:f62e450bb411 | 100 | } |
RemcoDas | 27:f62e450bb411 | 101 | else { // rechter EMG |
RemcoDas | 27:f62e450bb411 | 102 | mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 ) |
RemcoDas | 27:f62e450bb411 | 103 | } |
RemcoDas | 25:c97d079e07f3 | 104 | return mode; |
RemcoDas | 26:d9855716ced7 | 105 | } |
RemcoDas | 26:d9855716ced7 | 106 | int PotReader(AnalogIn& pot){ // potmeter uitlezen en mode bepalen (thresholds) |
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 | 27:f62e450bb411 | 117 | int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // bepaal mode uit emg en pot |
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 | 26:d9855716ced7 | 121 | if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1 |
RemcoDas | 26:d9855716ced7 | 122 | if(emgMode > potMode){ // maximum van emg en 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 | 25:c97d079e07f3 | 131 | void setEmgFlag(){ |
RemcoDas | 25:c97d079e07f3 | 132 | emgFlag = true; |
RemcoDas | 24:ddd69385b55f | 133 | } |
RemcoDas | 24:ddd69385b55f | 134 | void btnSetAction(){ // Set knop K64F |
RemcoDas | 24:ddd69385b55f | 135 | btn = true; // GoFlag setknop |
RemcoDas | 26:d9855716ced7 | 136 | } |
RemcoDas | 24:ddd69385b55f | 137 | void setRegelaarFlag(){ // Go flag button controle |
RemcoDas | 24:ddd69385b55f | 138 | regelaarFlag = true; |
RemcoDas | 24:ddd69385b55f | 139 | } |
RemcoDas | 24:ddd69385b55f | 140 | void setRemFlag(){ // Go flag rem |
RemcoDas | 24:ddd69385b55f | 141 | remFlag = true; |
RemcoDas | 24:ddd69385b55f | 142 | } |
RemcoDas | 24:ddd69385b55f | 143 | void checkAim(){ // controleer of killer switch geraakt is en of max aantal counts niet bereikt is |
RemcoDas | 27:f62e450bb411 | 144 | /*double volt = KS.read(); |
RemcoDas | 27:f62e450bb411 | 145 | if(volt> 0.5 /*|| abs(enc2.getPulses()) > maxCounts*///){ |
RemcoDas | 27:f62e450bb411 | 146 | /*pwmM2.write(0); // Aim motor stilzetten |
RemcoDas | 24:ddd69385b55f | 147 | pc.printf("BOEM! CRASH! KASTUK! \r\n"); |
RemcoDas | 27:f62e450bb411 | 148 | }*/ |
RemcoDas | 24:ddd69385b55f | 149 | } |
RemcoDas | 24:ddd69385b55f | 150 | void motorAim(int dir){ // Aim motor laten draaien met gegeven direction |
RemcoDas | 24:ddd69385b55f | 151 | dirM2.write(dir); // richting |
RemcoDas | 24:ddd69385b55f | 152 | pwmM2.write(0.5); // width aanpassen |
RemcoDas | 24:ddd69385b55f | 153 | } |
RemcoDas | 24:ddd69385b55f | 154 | bool controlAim(){ // Function om aim motor aan te sturen |
RemcoDas | 26:d9855716ced7 | 155 | bool both = false; // boolean of beide knoppen ingedrukt zijn |
RemcoDas | 27:f62e450bb411 | 156 | int modeL = defMode(emgL, potL, true); |
RemcoDas | 27:f62e450bb411 | 157 | int modeR = defMode(emgR, potR, false); |
RemcoDas | 27:f62e450bb411 | 158 | |
RemcoDas | 27:f62e450bb411 | 159 | scope.set(0, modeL); |
RemcoDas | 27:f62e450bb411 | 160 | scope.set(1, modeR); |
RemcoDas | 27:f62e450bb411 | 161 | scope.send(); //stuur de waardes naar HIDscope |
RemcoDas | 27:f62e450bb411 | 162 | |
RemcoDas | 28:e6d2fe0e593e | 163 | if(modeR < 2 && !R){ // controleer of mode 1 is geweest |
RemcoDas | 27:f62e450bb411 | 164 | R = true; |
RemcoDas | 27:f62e450bb411 | 165 | } |
RemcoDas | 28:e6d2fe0e593e | 166 | if(modeL < 2 && !L){ |
RemcoDas | 28:e6d2fe0e593e | 167 | L = true; |
RemcoDas | 24:ddd69385b55f | 168 | } |
RemcoDas | 28:e6d2fe0e593e | 169 | |
RemcoDas | 28:e6d2fe0e593e | 170 | if((modeL>2) && (modeR >2)) { // mode L en mode R beide > 2 |
RemcoDas | 26:d9855716ced7 | 171 | both = true; // Return true |
RemcoDas | 26:d9855716ced7 | 172 | pwmM2.write(0); // Aim motor stilzetten |
RemcoDas | 28:e6d2fe0e593e | 173 | aimState = OFF; |
RemcoDas | 28:e6d2fe0e593e | 174 | } |
RemcoDas | 28:e6d2fe0e593e | 175 | else if((modeR == 2) && (modeL == 2)) { |
RemcoDas | 28:e6d2fe0e593e | 176 | if(aimState!=OFF){ |
RemcoDas | 28:e6d2fe0e593e | 177 | pwmM2.write(0); // Aim motor stilzetten |
RemcoDas | 28:e6d2fe0e593e | 178 | aimState = OFF; |
RemcoDas | 28:e6d2fe0e593e | 179 | PRINT("Motor freeze\r\n"); |
RemcoDas | 28:e6d2fe0e593e | 180 | L = false; |
RemcoDas | 28:e6d2fe0e593e | 181 | R = false; |
RemcoDas | 28:e6d2fe0e593e | 182 | } |
RemcoDas | 24:ddd69385b55f | 183 | } |
RemcoDas | 28:e6d2fe0e593e | 184 | else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND richting is niet CCW AND modeR = 1 |
RemcoDas | 28:e6d2fe0e593e | 185 | if(L){ |
RemcoDas | 28:e6d2fe0e593e | 186 | aimState = CCW; // draai CCW |
RemcoDas | 28:e6d2fe0e593e | 187 | pc.printf("Turn negative (CCW)\r\n"); |
RemcoDas | 28:e6d2fe0e593e | 188 | motorAim(0); |
RemcoDas | 28:e6d2fe0e593e | 189 | } |
RemcoDas | 24:ddd69385b55f | 190 | } |
RemcoDas | 28:e6d2fe0e593e | 191 | else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND richting is niet CW AND modeL = 1 |
RemcoDas | 28:e6d2fe0e593e | 192 | if(R){ |
RemcoDas | 28:e6d2fe0e593e | 193 | aimState = CW; // draai CW |
RemcoDas | 28:e6d2fe0e593e | 194 | PRINT("Turn positive (CW)\r\n"); |
RemcoDas | 28:e6d2fe0e593e | 195 | motorAim(1); |
RemcoDas | 28:e6d2fe0e593e | 196 | } |
RemcoDas | 27:f62e450bb411 | 197 | } |
RemcoDas | 24:ddd69385b55f | 198 | return both; |
RemcoDas | 24:ddd69385b55f | 199 | } |
RemcoDas | 24:ddd69385b55f | 200 | int main(){ |
RemcoDas | 24:ddd69385b55f | 201 | flipLed(ledR); // test of code begint |
RemcoDas | 24:ddd69385b55f | 202 | btnSet.mode(PullUp); // Button mode |
RemcoDas | 24:ddd69385b55f | 203 | btnSet.rise(&btnSetAction); // Setknop aan functie koppellen |
RemcoDas | 26:d9855716ced7 | 204 | tickerRegelaar.attach(&setRegelaarFlag,tRegelaar); // ticker regelaar motor |
RemcoDas | 26:d9855716ced7 | 205 | tickerRem.attach(&setRemFlag,tRem); // ticker rem |
RemcoDas | 26:d9855716ced7 | 206 | EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H |
RemcoDas | 25:c97d079e07f3 | 207 | |
RemcoDas | 24:ddd69385b55f | 208 | pc.printf("\n\n\n\n\n"); |
RemcoDas | 25:c97d079e07f3 | 209 | PRINT("--- NEW GAME ---\r\n"); |
RemcoDas | 26:d9855716ced7 | 210 | while(1){ // Programma door laten lopen |
RemcoDas | 24:ddd69385b55f | 211 | switch(state){ |
RemcoDas | 27:f62e450bb411 | 212 | case KALIBRATE_EMG: { |
RemcoDas | 27:f62e450bb411 | 213 | PRINT("Kalibrate EMG left in 5 seconds\r\n"); |
RemcoDas | 27:f62e450bb411 | 214 | |
RemcoDas | 27:f62e450bb411 | 215 | EMGkalibratieL(); |
RemcoDas | 24:ddd69385b55f | 216 | |
RemcoDas | 27:f62e450bb411 | 217 | PRINT("Kalibrate EMG right in 5 seconds\r\n"); |
RemcoDas | 27:f62e450bb411 | 218 | EMGkalibratieR(); |
RemcoDas | 24:ddd69385b55f | 219 | |
RemcoDas | 24:ddd69385b55f | 220 | state = KALIBRATE_AIM; |
RemcoDas | 24:ddd69385b55f | 221 | break; |
RemcoDas | 24:ddd69385b55f | 222 | } |
RemcoDas | 24:ddd69385b55f | 223 | case KALIBRATE_AIM: { |
RemcoDas | 24:ddd69385b55f | 224 | pwmM2.period(1/100000); // aanstuurperiode motor 2 |
RemcoDas | 24:ddd69385b55f | 225 | PRINT("Kalibrate aim motor\r\n"); |
RemcoDas | 24:ddd69385b55f | 226 | wait(1); |
RemcoDas | 24:ddd69385b55f | 227 | pc.printf("Use L and R to move pendulum to equilibruim position\r\n L+R = OK\r\n"); |
RemcoDas | 24:ddd69385b55f | 228 | while(state==KALIBRATE_AIM){ |
RemcoDas | 24:ddd69385b55f | 229 | if(regelaarFlag){ // motor regelen op GoFlag |
RemcoDas | 24:ddd69385b55f | 230 | regelaarFlag = false; |
RemcoDas | 26:d9855716ced7 | 231 | checkAim(); // Controleer positie |
RemcoDas | 29:9610df479f89 | 232 | } |
RemcoDas | 25:c97d079e07f3 | 233 | if(emgFlag){ // Go flag EMG sampelen |
RemcoDas | 26:d9855716ced7 | 234 | emgFlag = false; |
RemcoDas | 24:ddd69385b55f | 235 | if(controlAim()){ // Buttons met control, if true = beide knoppen = bevestigen |
RemcoDas | 24:ddd69385b55f | 236 | enc2.reset(); // resetknop = encoder 1 resetten |
RemcoDas | 24:ddd69385b55f | 237 | pc.printf("Positie gekalibreerd, kalibreer nu slinger, encoder counts: %i\r\n", enc2.getPulses()); |
RemcoDas | 24:ddd69385b55f | 238 | state = KALIBRATE_PEND; // volgende state |
RemcoDas | 24:ddd69385b55f | 239 | } |
RemcoDas | 24:ddd69385b55f | 240 | } |
RemcoDas | 24:ddd69385b55f | 241 | } |
RemcoDas | 24:ddd69385b55f | 242 | break; |
RemcoDas | 24:ddd69385b55f | 243 | } |
RemcoDas | 24:ddd69385b55f | 244 | case KALIBRATE_PEND: { |
RemcoDas | 24:ddd69385b55f | 245 | pc.printf("Kalibrate pendulum motor with setknop\r\n"); |
RemcoDas | 24:ddd69385b55f | 246 | pwmM1.period(1/100000); // aanstuurperiode motor 1 |
RemcoDas | 24:ddd69385b55f | 247 | servo.period(0.02); // 20ms period aanstuurperiode |
RemcoDas | 24:ddd69385b55f | 248 | pwmM1.write(0.5); // Motor aanzetten, laag vermogen |
RemcoDas | 24:ddd69385b55f | 249 | btn = false; |
RemcoDas | 24:ddd69385b55f | 250 | while(state==KALIBRATE_PEND){ |
RemcoDas | 26:d9855716ced7 | 251 | if(regelaarFlag){ |
RemcoDas | 24:ddd69385b55f | 252 | pc.printf(""); // lege regel printen, anders doet setknop het niet |
RemcoDas | 26:d9855716ced7 | 253 | regelaarFlag = false; |
RemcoDas | 24:ddd69385b55f | 254 | if (btn){ // Als setknop ingedrukt is reset |
RemcoDas | 24:ddd69385b55f | 255 | pwmM1.write(0); // Motor 1 stilzetten |
RemcoDas | 24:ddd69385b55f | 256 | enc1.reset(); // encoder 1 resetten |
RemcoDas | 26:d9855716ced7 | 257 | PRINT("Pendulum kalibrated\r\n"); |
RemcoDas | 24:ddd69385b55f | 258 | btn = false; // knop op false |
RemcoDas | 24:ddd69385b55f | 259 | state = AIM; // volgende fase |
RemcoDas | 24:ddd69385b55f | 260 | } |
RemcoDas | 24:ddd69385b55f | 261 | } |
RemcoDas | 24:ddd69385b55f | 262 | } |
RemcoDas | 24:ddd69385b55f | 263 | break; |
RemcoDas | 24:ddd69385b55f | 264 | } |
RemcoDas | 24:ddd69385b55f | 265 | case AIM: { |
RemcoDas | 24:ddd69385b55f | 266 | pc.printf("Aim with EMG\r\n"); |
RemcoDas | 24:ddd69385b55f | 267 | while(state == AIM){ |
RemcoDas | 24:ddd69385b55f | 268 | if(regelaarFlag){ // motor regelen op GoFlag |
RemcoDas | 24:ddd69385b55f | 269 | regelaarFlag = false; |
RemcoDas | 24:ddd69385b55f | 270 | checkAim(); // Controleer positie |
RemcoDas | 24:ddd69385b55f | 271 | } |
RemcoDas | 25:c97d079e07f3 | 272 | if(emgFlag){ // Go flag EMG sampelen |
RemcoDas | 26:d9855716ced7 | 273 | emgFlag = false; |
RemcoDas | 24:ddd69385b55f | 274 | if(controlAim()){ |
RemcoDas | 24:ddd69385b55f | 275 | pc.printf("Position choosen, adjust shoot velocity\r\n"); |
RemcoDas | 24:ddd69385b55f | 276 | state = REM; // volgende state (REM) |
RemcoDas | 24:ddd69385b55f | 277 | } |
RemcoDas | 24:ddd69385b55f | 278 | } |
RemcoDas | 24:ddd69385b55f | 279 | } |
RemcoDas | 24:ddd69385b55f | 280 | break; |
RemcoDas | 24:ddd69385b55f | 281 | } |
RemcoDas | 24:ddd69385b55f | 282 | case REM: { |
RemcoDas | 24:ddd69385b55f | 283 | pc.printf("Set break percentage, current is: %.0f\r\n", remPerc); |
RemcoDas | 24:ddd69385b55f | 284 | L = false; |
RemcoDas | 24:ddd69385b55f | 285 | R = false; |
RemcoDas | 26:d9855716ced7 | 286 | while(state == REM){ |
RemcoDas | 26:d9855716ced7 | 287 | if(emgFlag){ // Go flag EMG sampelen |
RemcoDas | 26:d9855716ced7 | 288 | emgFlag = false; |
RemcoDas | 27:f62e450bb411 | 289 | int modeL = defMode(emgL, potL, true); |
RemcoDas | 27:f62e450bb411 | 290 | int modeR = defMode(emgR, potR, false); |
RemcoDas | 27:f62e450bb411 | 291 | |
RemcoDas | 26:d9855716ced7 | 292 | if(modeR < 2) { // R is niet aan |
RemcoDas | 24:ddd69385b55f | 293 | R = true; |
RemcoDas | 24:ddd69385b55f | 294 | } |
RemcoDas | 26:d9855716ced7 | 295 | if(modeL < 2) { // L is niet aan |
RemcoDas | 24:ddd69385b55f | 296 | L = true; |
RemcoDas | 24:ddd69385b55f | 297 | } |
RemcoDas | 24:ddd69385b55f | 298 | if(L && R){ |
RemcoDas | 27:f62e450bb411 | 299 | if((modeL > 1) && modeR > 1) { // beide aangespannenR = false; |
RemcoDas | 24:ddd69385b55f | 300 | state = FIRE; |
RemcoDas | 24:ddd69385b55f | 301 | } |
RemcoDas | 26:d9855716ced7 | 302 | else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten |
RemcoDas | 24:ddd69385b55f | 303 | state = FIRE; |
RemcoDas | 26:d9855716ced7 | 304 | } |
RemcoDas | 27:f62e450bb411 | 305 | else if((modeL == 2) && L) { // links ingedrukt = rem verlagen |
RemcoDas | 24:ddd69385b55f | 306 | if(remPerc>1){ |
RemcoDas | 26:d9855716ced7 | 307 | remPerc--; // Rem percentage verlagen met 1 |
RemcoDas | 24:ddd69385b55f | 308 | } |
RemcoDas | 26:d9855716ced7 | 309 | L = false; |
RemcoDas | 24:ddd69385b55f | 310 | } |
RemcoDas | 27:f62e450bb411 | 311 | else if((modeR == 2) && R) { // Rechts half ingedrukt = rem verhogen |
RemcoDas | 27:f62e450bb411 | 312 | if(remPerc<10){ |
RemcoDas | 26:d9855716ced7 | 313 | remPerc++; // rem percentage verhogen met 1 |
RemcoDas | 24:ddd69385b55f | 314 | } |
RemcoDas | 24:ddd69385b55f | 315 | R = false; |
RemcoDas | 26:d9855716ced7 | 316 | } |
RemcoDas | 26:d9855716ced7 | 317 | if(modeL > 1 || modeR > 1){ // Print rem scale als een of beide knoppen aan zijn |
RemcoDas | 26:d9855716ced7 | 318 | pc.printf("Current breaking scale: %i\r\n", remPerc); |
RemcoDas | 26:d9855716ced7 | 319 | } |
RemcoDas | 26:d9855716ced7 | 320 | } |
RemcoDas | 24:ddd69385b55f | 321 | } |
RemcoDas | 24:ddd69385b55f | 322 | } |
RemcoDas | 24:ddd69385b55f | 323 | L = false; |
RemcoDas | 24:ddd69385b55f | 324 | R = false; |
RemcoDas | 24:ddd69385b55f | 325 | break; |
RemcoDas | 24:ddd69385b55f | 326 | } |
RemcoDas | 24:ddd69385b55f | 327 | case FIRE: { |
RemcoDas | 24:ddd69385b55f | 328 | pc.printf("Shooting\r\n"); |
RemcoDas | 24:ddd69385b55f | 329 | servo.pulsewidth(servoL); // schrijfrem release |
RemcoDas | 24:ddd69385b55f | 330 | pwmM1.write(0.5); // motor aanzetten |
RemcoDas | 24:ddd69385b55f | 331 | bool servoPos = true; |
RemcoDas | 24:ddd69385b55f | 332 | while(state == FIRE){ // Zolang in FIRE state |
RemcoDas | 27:f62e450bb411 | 333 | if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100/**abs(remPerc)/10*/)){ // Rem goFlag en half rondje = remmen |
RemcoDas | 24:ddd69385b55f | 334 | remFlag = false; |
RemcoDas | 24:ddd69385b55f | 335 | if(servoPos){ // |
RemcoDas | 24:ddd69385b55f | 336 | servoPos = false; |
RemcoDas | 24:ddd69385b55f | 337 | servo.pulsewidth(servoL); // links |
RemcoDas | 24:ddd69385b55f | 338 | } |
RemcoDas | 24:ddd69385b55f | 339 | else{ |
RemcoDas | 24:ddd69385b55f | 340 | servoPos = true; |
RemcoDas | 24:ddd69385b55f | 341 | servo.pulsewidth(servoR); // rechts |
RemcoDas | 24:ddd69385b55f | 342 | } |
RemcoDas | 24:ddd69385b55f | 343 | } |
RemcoDas | 24:ddd69385b55f | 344 | if(regelaarFlag){ |
RemcoDas | 26:d9855716ced7 | 345 | regelaarFlag = false; |
RemcoDas | 24:ddd69385b55f | 346 | if((abs(enc1.getPulses())+50)%4200 < 150){ // Slinger 1 rondje laten draaien |
RemcoDas | 24:ddd69385b55f | 347 | pwmM1.write(0); // motor uitzetten |
RemcoDas | 26:d9855716ced7 | 348 | PRINT("Pendulum on startposition\r\n"); |
RemcoDas | 24:ddd69385b55f | 349 | state = AIM; // Aim is volgende fase |
RemcoDas | 24:ddd69385b55f | 350 | } |
RemcoDas | 24:ddd69385b55f | 351 | } |
RemcoDas | 24:ddd69385b55f | 352 | } |
RemcoDas | 24:ddd69385b55f | 353 | break; |
RemcoDas | 24:ddd69385b55f | 354 | } |
Bartvaart | 19:6c0245063b96 | 355 | } |
Bartvaart | 17:cfe44346645c | 356 | } |
RemcoDas | 24:ddd69385b55f | 357 | } |