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@24:ddd69385b55f, 2015-10-15 (annotated)

Committer:
RemcoDas
Date:
Thu Oct 15 11:13:32 2015 +0000
Revision:
24:ddd69385b55f
Parent:
23:855c4bcb2284
Child:
25:c97d079e07f3
Control en 1 EMG

Who changed what in which revision?

UserRevisionLine numberNew 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 24:ddd69385b55f 11 DigitalOut ledR(LED_RED); // 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 24:ddd69385b55f 15 AnalogIn btnL(A2), btnR(A3); // push buttons, links en rechts
RemcoDas 24:ddd69385b55f 16 AnalogIn ptmrL(A2), ptmrR(A3); // Intensiteit 'EMG' signaal door potmeter
RemcoDas 24:ddd69385b55f 17 AnalogIn KS(A4); // Killswitch
RemcoDas 24:ddd69385b55f 18 Ticker tickerRegelaar, tickerBtn, tickerRem; // regelaar button en rem ticker
RemcoDas 24:ddd69385b55f 19 Timer EMGTimer; // timer voor EMG meting
RemcoDas 24:ddd69385b55f 20 // EMG spul:
RemcoDas 24:ddd69385b55f 21 AnalogIn emg(A0); //Analog input van emg kabels Links
RemcoDas 24:ddd69385b55f 22 AnalogIn emgR(A1); //Analog input van emg kabels Rechts
Bartvaart 18:eec0880fcded 23 HIDScope scope(3); //3 scopes
Bartvaart 0:557b1ff83a8a 24 Ticker EMGticker;
Bartvaart 17:cfe44346645c 25 DigitalOut LedBlue(LED3);
Bartvaart 22:c1811e60bfce 26 DigitalIn button(PTA4);
Bartvaart 0:557b1ff83a8a 27
RemcoDas 24:ddd69385b55f 28 // QEI Encoder(poort 1, poort 2, ,counts/rev
RemcoDas 24:ddd69385b55f 29 QEI enc1 (D13, D12, NC, 64);
RemcoDas 24:ddd69385b55f 30 QEI enc2 (D11, D10, NC, 64);
RemcoDas 24:ddd69385b55f 31 // Motor1 met PWM aansturen en richting aangeven
RemcoDas 24:ddd69385b55f 32 PwmOut pwmM1(D6);
RemcoDas 24:ddd69385b55f 33 DigitalOut dirM1(D7);
RemcoDas 24:ddd69385b55f 34 // Motor2 met PWM aansturen en richting aangeven
RemcoDas 24:ddd69385b55f 35 PwmOut pwmM2(D5);
RemcoDas 24:ddd69385b55f 36 DigitalOut dirM2(D4);
RemcoDas 24:ddd69385b55f 37
RemcoDas 24:ddd69385b55f 38 enum spelfase {KALIBRATE_EMG1, KALIBRATE_AIM, KALIBRATE_PEND, AIM, REM, FIRE}; // Spelfases, ACKNOWLEDGEMENT: BMT groep 4 2014
RemcoDas 24:ddd69385b55f 39 uint8_t state = KALIBRATE_EMG1; // eerste spelfase
RemcoDas 24:ddd69385b55f 40
RemcoDas 24:ddd69385b55f 41 enum aimFase {OFF, CW, CCW}; // Aim motor mogelijkheden
RemcoDas 24:ddd69385b55f 42 uint8_t aimState = OFF; // mogelijkheid begin
RemcoDas 24:ddd69385b55f 43 //-------------------------------Variables---------------------------------------------------------------------
RemcoDas 24:ddd69385b55f 44 const double servoL = 0.001, servoR = 0.0011; // uitwijking servo, bereik tussen L en R (= pulsewidth pwm servo)
RemcoDas 24:ddd69385b55f 45 const int on = 0, off = 1; // aan / uit
RemcoDas 24:ddd69385b55f 46 volatile bool btn = false; // boolean om programma te runnen, drukknop ingedrukt
RemcoDas 24:ddd69385b55f 47 int maxCounts = 2100; // max richt motor counts Aim motor
RemcoDas 24:ddd69385b55f 48 volatile bool regelaarFlag = false, btnFlag = false, remFlag = false; // Go flags
RemcoDas 24:ddd69385b55f 49 const double tRegelaar = 0.005, tBtn = 0.005, tRem = 0.1; // tijd ticker voor regelaar en knoppen/EMG
RemcoDas 24:ddd69385b55f 50 int remPerc = 0;
RemcoDas 24:ddd69385b55f 51 volatile bool L = false, R = false; // Booleans voor knop link en knop rechts
RemcoDas 24:ddd69385b55f 52 volatile int emgModeL = 1, emgModeR = 1; // emg signaal waarden
RemcoDas 24:ddd69385b55f 53
Bartvaart 0:557b1ff83a8a 54 //Sample frequentie
Bartvaart 22:c1811e60bfce 55 double Fs = 200; //Hz
Bartvaart 1:98be4152a539 56 double t = 1/ Fs; // voor EMGticker
Bartvaart 5:b400209df739 57
RemcoDas 24:ddd69385b55f 58 bool readymax = 0, readymin = 0;
RemcoDas 24:ddd69385b55f 59 double ymin, ymax;
RemcoDas 24:ddd69385b55f 60 double thresholdlow, thresholdmid, thresholdhigh;
RemcoDas 24:ddd69385b55f 61 bool go_flag = false;
Bartvaart 18:eec0880fcded 62
RemcoDas 24:ddd69385b55f 63 //----------------------------Functions-----------------------------------------------------------------------
RemcoDas 24:ddd69385b55f 64 void EMGkalibratie(){
Bartvaart 18:eec0880fcded 65 LedBlue = 1;
RemcoDas 24:ddd69385b55f 66 Init();
Bartvaart 20:11e1244ad2ad 67 ymin = KalibratieMin(readymin);
Bartvaart 19:6c0245063b96 68 wait(1);
Bartvaart 20:11e1244ad2ad 69 ymax = KalibratieMax(readymax);
Bartvaart 19:6c0245063b96 70
Bartvaart 18:eec0880fcded 71 // bepalen van thresholds voor aan/uit
Bartvaart 22:c1811e60bfce 72 thresholdlow = 8 * ymin; // standaardwaarde
Bartvaart 20:11e1244ad2ad 73 thresholdmid = 0.5 * ymax; // afhankelijk van max output gebruiker
Bartvaart 21:8fe8419de3e9 74 thresholdhigh = 0.8 * ymax;
Bartvaart 22:c1811e60bfce 75
Bartvaart 22:c1811e60bfce 76 pc.printf("ymax = %f en ymin = %f \n",ymax, ymin); //bugfix
RemcoDas 24:ddd69385b55f 77 }
Bartvaart 7:040591b3f019 78
RemcoDas 24:ddd69385b55f 79 void EMGfilter(){
Bartvaart 7:040591b3f019 80 //uitlezen emg signaal
Bartvaart 7:040591b3f019 81 double u = emg.read();
Bartvaart 20:11e1244ad2ad 82 double y = Filterdesigns(u);
Bartvaart 18:eec0880fcded 83 //pc.printf("%f \n",y); //bugfix
Bartvaart 7:040591b3f019 84 // Plotten in HIDscope
RemcoDas 24:ddd69385b55f 85 emgModeL = Mode(y, ymax, thresholdlow, thresholdmid, thresholdhigh); //bepaald welk signaal de motor krijgt (aan, uit, middel)
Bartvaart 15:c1a8f28d6583 86 scope.set(0,u); //ongefilterde waarde naar scope 1
Bartvaart 15:c1a8f28d6583 87 scope.set(1,y); //gefilterde waarde naar scope 2
RemcoDas 24:ddd69385b55f 88 scope.set(2,emgModeL);
Bartvaart 7:040591b3f019 89 scope.send(); //stuur de waardes naar HIDscope
Bartvaart 22:c1811e60bfce 90 }
Bartvaart 22:c1811e60bfce 91
RemcoDas 24:ddd69385b55f 92 void Go_flag(){
RemcoDas 24:ddd69385b55f 93 go_flag = true;
RemcoDas 24:ddd69385b55f 94 }
RemcoDas 24:ddd69385b55f 95 // -----
RemcoDas 24:ddd69385b55f 96
RemcoDas 24:ddd69385b55f 97 void flipLed(DigitalOut& led){ //function to flip one LED
RemcoDas 24:ddd69385b55f 98 led.write(on);
RemcoDas 24:ddd69385b55f 99 wait(0.2);
RemcoDas 24:ddd69385b55f 100 led.write(off);
RemcoDas 24:ddd69385b55f 101 }
RemcoDas 24:ddd69385b55f 102 void PRINT(const char* text){ // putty en lcd print
RemcoDas 24:ddd69385b55f 103 lcd.cls(); // clear LCD scherm
RemcoDas 24:ddd69385b55f 104 lcd.printf(text); // print text op lcd
RemcoDas 24:ddd69385b55f 105 pc.printf(text); // print text op terminal
RemcoDas 24:ddd69385b55f 106 }
RemcoDas 24:ddd69385b55f 107 void btnSetAction(){ // Set knop K64F
RemcoDas 24:ddd69385b55f 108 btn = true; // GoFlag setknop
RemcoDas 24:ddd69385b55f 109 }
RemcoDas 24:ddd69385b55f 110 void setBtnFlag(){ // Go flag regelaar Aim motor
RemcoDas 24:ddd69385b55f 111 btnFlag = true;
RemcoDas 24:ddd69385b55f 112 }
RemcoDas 24:ddd69385b55f 113 void setRegelaarFlag(){ // Go flag button controle
RemcoDas 24:ddd69385b55f 114 regelaarFlag = true;
RemcoDas 24:ddd69385b55f 115 }
RemcoDas 24:ddd69385b55f 116 void setRemFlag(){ // Go flag rem
RemcoDas 24:ddd69385b55f 117 remFlag = true;
RemcoDas 24:ddd69385b55f 118 }
RemcoDas 24:ddd69385b55f 119 void checkAim(){ // controleer of killer switch geraakt is en of max aantal counts niet bereikt is
RemcoDas 24:ddd69385b55f 120 double volt = KS.read();
RemcoDas 24:ddd69385b55f 121 if(volt< 0.5 /*|| abs(enc2.getPulses()) > maxCounts*/){
RemcoDas 24:ddd69385b55f 122 pwmM2.write(0); // Aim motor stilzetten
RemcoDas 24:ddd69385b55f 123 pc.printf("BOEM! CRASH! KASTUK! \r\n");
RemcoDas 24:ddd69385b55f 124 }
RemcoDas 24:ddd69385b55f 125 }
RemcoDas 24:ddd69385b55f 126 void motorAim(int dir){ // Aim motor laten draaien met gegeven direction
RemcoDas 24:ddd69385b55f 127 dirM2.write(dir); // richting
RemcoDas 24:ddd69385b55f 128 pwmM2.write(0.5); // width aanpassen
RemcoDas 24:ddd69385b55f 129 }
RemcoDas 24:ddd69385b55f 130 bool controlAim(){ // Function om aim motor aan te sturen
RemcoDas 24:ddd69385b55f 131 bool both = false; // boolean of beide knoppen ingedrukt zijn
RemcoDas 24:ddd69385b55f 132
RemcoDas 24:ddd69385b55f 133 if(!(ptmrL.read() >0.3 && ptmrR.read()>0.3) != !(emgModeL!=1 && emgModeR!=1)) { // Potmeters L en R beide aan XOR EMG beide aangespannen
RemcoDas 24:ddd69385b55f 134 R = false;
RemcoDas 24:ddd69385b55f 135 L = false;
RemcoDas 24:ddd69385b55f 136 pwmM2.write(0); // Aim motor stilzetten
RemcoDas 24:ddd69385b55f 137 aimState = OFF;
RemcoDas 24:ddd69385b55f 138 }
RemcoDas 24:ddd69385b55f 139 else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3)) && L) { // Potmeter L vol ingedrukt XOR emgModeL is 3
RemcoDas 24:ddd69385b55f 140 both = true; // Return true
RemcoDas 24:ddd69385b55f 141 pwmM2.write(0); // Aim motor stilzetten
RemcoDas 24:ddd69385b55f 142 aimState = OFF;
RemcoDas 24:ddd69385b55f 143 L = false;
RemcoDas 24:ddd69385b55f 144 }
RemcoDas 24:ddd69385b55f 145 else if((!(ptmrR.read() > 0.8) != !(emgModeR == 3)) && aimState!=OFF) { // Potmeter R vol ingedrukt XOR emgModeR == 3
RemcoDas 24:ddd69385b55f 146 R = false;
RemcoDas 24:ddd69385b55f 147 pwmM2.write(0); // Aim motor stilzetten
RemcoDas 24:ddd69385b55f 148 aimState = OFF;
RemcoDas 24:ddd69385b55f 149 PRINT("Motor freeze\r\n");
RemcoDas 24:ddd69385b55f 150 }
RemcoDas 24:ddd69385b55f 151 else if((!(ptmrL.read() > 0.3 && ptmrL.read() < 0.6) != !(emgModeL == 2)) && aimState != CCW && L) { // Potmeter L half XOR emgModeL ==2 AND richting is niet CCW
RemcoDas 24:ddd69385b55f 152 aimState = CCW; // draai CCW
RemcoDas 24:ddd69385b55f 153 pc.printf("Turn negative (CCW)\r\n");
RemcoDas 24:ddd69385b55f 154 motorAim(0);
RemcoDas 24:ddd69385b55f 155 }
RemcoDas 24:ddd69385b55f 156 else if((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && aimState != CW && R) { // Potmeter R half XOR emgModeR == 2 AND richting is niet CW
RemcoDas 24:ddd69385b55f 157 aimState = CW; // draai CW
RemcoDas 24:ddd69385b55f 158 PRINT("Turn positive (CW)\r\n");
RemcoDas 24:ddd69385b55f 159 motorAim(1);
RemcoDas 24:ddd69385b55f 160 }
RemcoDas 24:ddd69385b55f 161 if(ptmrR.read() < 0.1 && emgModeR == 1 && !R){ // R is niet aan
RemcoDas 24:ddd69385b55f 162 R = true;
RemcoDas 24:ddd69385b55f 163 }
RemcoDas 24:ddd69385b55f 164 if(ptmrL.read() < 0.1 && emgModeL == 1 && !L){ // L is niet aan
RemcoDas 24:ddd69385b55f 165 L = true;
RemcoDas 24:ddd69385b55f 166 }
RemcoDas 24:ddd69385b55f 167 return both;
RemcoDas 24:ddd69385b55f 168 }
RemcoDas 24:ddd69385b55f 169 int main(){
RemcoDas 24:ddd69385b55f 170 flipLed(ledR); // test of code begint
RemcoDas 24:ddd69385b55f 171 btnSet.mode(PullUp); // Button mode
RemcoDas 24:ddd69385b55f 172 btnSet.rise(&btnSetAction); // Setknop aan functie koppellen
RemcoDas 24:ddd69385b55f 173 tickerRegelaar.attach(&setRegelaarFlag,tRegelaar); // ticker regelaar motor
RemcoDas 24:ddd69385b55f 174 tickerBtn.attach(&setBtnFlag,tBtn); // ticker knop controle
RemcoDas 24:ddd69385b55f 175 tickerRem.attach(&setRemFlag,tRem); // ticker rem
RemcoDas 24:ddd69385b55f 176
RemcoDas 24:ddd69385b55f 177 EMGticker.attach(&Go_flag, t); // ticker EMG, 500H
RemcoDas 24:ddd69385b55f 178
RemcoDas 24:ddd69385b55f 179 lcd.cls();
RemcoDas 24:ddd69385b55f 180 lcd.printf("NEW GAME \n HELLO!");
RemcoDas 24:ddd69385b55f 181
RemcoDas 24:ddd69385b55f 182 pc.printf("\n\n\n\n\n");
RemcoDas 24:ddd69385b55f 183 pc.printf("--- NEW GAME ---\r\n");
RemcoDas 24:ddd69385b55f 184 while(1){ // Programma door laten lopen
RemcoDas 24:ddd69385b55f 185 switch(state){
RemcoDas 24:ddd69385b55f 186 case KALIBRATE_EMG1: {
RemcoDas 24:ddd69385b55f 187 pc.printf("Kalibrate EMG signal in 5 seconds\r\n");
RemcoDas 24:ddd69385b55f 188 lcd.cls();
RemcoDas 24:ddd69385b55f 189 lcd.printf("Kalibrate EMG\n in 5 sec.");
RemcoDas 24:ddd69385b55f 190
Bartvaart 22:c1811e60bfce 191 EMGkalibratie();
RemcoDas 24:ddd69385b55f 192
RemcoDas 24:ddd69385b55f 193 state = KALIBRATE_AIM;
RemcoDas 24:ddd69385b55f 194 break;
RemcoDas 24:ddd69385b55f 195 }
RemcoDas 24:ddd69385b55f 196 case KALIBRATE_AIM: {
RemcoDas 24:ddd69385b55f 197 pwmM2.period(1/100000); // aanstuurperiode motor 2
RemcoDas 24:ddd69385b55f 198 PRINT("Kalibrate aim motor\r\n");
RemcoDas 24:ddd69385b55f 199 wait(1);
RemcoDas 24:ddd69385b55f 200 pc.printf("Use L and R to move pendulum to equilibruim position\r\n L+R = OK\r\n");
RemcoDas 24:ddd69385b55f 201 while(state==KALIBRATE_AIM){
RemcoDas 24:ddd69385b55f 202 if(regelaarFlag){ // motor regelen op GoFlag
RemcoDas 24:ddd69385b55f 203 regelaarFlag = false;
RemcoDas 24:ddd69385b55f 204 checkAim(); // Controleer positie
RemcoDas 24:ddd69385b55f 205 }
RemcoDas 24:ddd69385b55f 206
RemcoDas 24:ddd69385b55f 207 if(go_flag){ // Go flag EMG sampelen
RemcoDas 24:ddd69385b55f 208 go_flag = false;
RemcoDas 24:ddd69385b55f 209 EMGfilter();
RemcoDas 24:ddd69385b55f 210 }
RemcoDas 24:ddd69385b55f 211 if(btnFlag){ // Go flag button controle?
RemcoDas 24:ddd69385b55f 212 btnFlag = false;
RemcoDas 24:ddd69385b55f 213 if(controlAim()){ // Buttons met control, if true = beide knoppen = bevestigen
RemcoDas 24:ddd69385b55f 214 enc2.reset(); // resetknop = encoder 1 resetten
RemcoDas 24:ddd69385b55f 215 pc.printf("Positie gekalibreerd, kalibreer nu slinger, encoder counts: %i\r\n", enc2.getPulses());
RemcoDas 24:ddd69385b55f 216 state = KALIBRATE_PEND; // volgende state
RemcoDas 24:ddd69385b55f 217 }
RemcoDas 24:ddd69385b55f 218 }
RemcoDas 24:ddd69385b55f 219 }
RemcoDas 24:ddd69385b55f 220 break;
RemcoDas 24:ddd69385b55f 221 }
RemcoDas 24:ddd69385b55f 222 case KALIBRATE_PEND: {
RemcoDas 24:ddd69385b55f 223 pc.printf("Kalibrate pendulum motor with setknop\r\n");
RemcoDas 24:ddd69385b55f 224 pwmM1.period(1/100000); // aanstuurperiode motor 1
RemcoDas 24:ddd69385b55f 225 servo.period(0.02); // 20ms period aanstuurperiode
RemcoDas 24:ddd69385b55f 226 pwmM1.write(0.5); // Motor aanzetten, laag vermogen
RemcoDas 24:ddd69385b55f 227 btn = false;
RemcoDas 24:ddd69385b55f 228 while(state==KALIBRATE_PEND){
RemcoDas 24:ddd69385b55f 229 if(btnFlag){
RemcoDas 24:ddd69385b55f 230 pc.printf(""); // lege regel printen, anders doet setknop het niet
RemcoDas 24:ddd69385b55f 231 btnFlag = false;
RemcoDas 24:ddd69385b55f 232 if (btn){ // Als setknop ingedrukt is reset
RemcoDas 24:ddd69385b55f 233 pwmM1.write(0); // Motor 1 stilzetten
RemcoDas 24:ddd69385b55f 234 enc1.reset(); // encoder 1 resetten
RemcoDas 24:ddd69385b55f 235 pc.printf("Pendulum kalibrated\r\n");
RemcoDas 24:ddd69385b55f 236 btn = false; // knop op false
RemcoDas 24:ddd69385b55f 237 state = AIM; // volgende fase
RemcoDas 24:ddd69385b55f 238 }
RemcoDas 24:ddd69385b55f 239 }
RemcoDas 24:ddd69385b55f 240 }
RemcoDas 24:ddd69385b55f 241 break;
RemcoDas 24:ddd69385b55f 242 }
RemcoDas 24:ddd69385b55f 243 case AIM: {
RemcoDas 24:ddd69385b55f 244 pc.printf("Aim with EMG\r\n");
RemcoDas 24:ddd69385b55f 245 while(state == AIM){
RemcoDas 24:ddd69385b55f 246 if(regelaarFlag){ // motor regelen op GoFlag
RemcoDas 24:ddd69385b55f 247 regelaarFlag = false;
RemcoDas 24:ddd69385b55f 248 checkAim(); // Controleer positie
RemcoDas 24:ddd69385b55f 249 }
RemcoDas 24:ddd69385b55f 250 if(go_flag){ // Go flag EMG sampelen
RemcoDas 24:ddd69385b55f 251 go_flag = false;
RemcoDas 24:ddd69385b55f 252 EMGfilter();
RemcoDas 24:ddd69385b55f 253 }
RemcoDas 24:ddd69385b55f 254 if(btnFlag){ // Go flag button controle?
RemcoDas 24:ddd69385b55f 255 btnFlag = false;
RemcoDas 24:ddd69385b55f 256 if(controlAim()){
RemcoDas 24:ddd69385b55f 257 pc.printf("Position choosen, adjust shoot velocity\r\n");
RemcoDas 24:ddd69385b55f 258 state = REM; // volgende state (REM)
RemcoDas 24:ddd69385b55f 259 }
RemcoDas 24:ddd69385b55f 260 }
RemcoDas 24:ddd69385b55f 261 }
RemcoDas 24:ddd69385b55f 262 break;
RemcoDas 24:ddd69385b55f 263 }
RemcoDas 24:ddd69385b55f 264 case REM: {
RemcoDas 24:ddd69385b55f 265 pc.printf("Set break percentage, current is: %.0f\r\n", remPerc);
RemcoDas 24:ddd69385b55f 266 L = false;
RemcoDas 24:ddd69385b55f 267 R = false;
RemcoDas 24:ddd69385b55f 268 while(state == REM){
RemcoDas 24:ddd69385b55f 269 if(go_flag){ // Go flag EMG sampelen
RemcoDas 24:ddd69385b55f 270 go_flag = false;
RemcoDas 24:ddd69385b55f 271 EMGfilter();
RemcoDas 24:ddd69385b55f 272 }
RemcoDas 24:ddd69385b55f 273 if(btnFlag){ // Go flag button controle? == rem aansturen
RemcoDas 24:ddd69385b55f 274 btnFlag = false;
RemcoDas 24:ddd69385b55f 275
RemcoDas 24:ddd69385b55f 276 if((ptmrR.read() < 0.1)) { // R is niet aan
RemcoDas 24:ddd69385b55f 277 R = true;
RemcoDas 24:ddd69385b55f 278 }
RemcoDas 24:ddd69385b55f 279 if((ptmrL.read() < 0.1)) { // L is niet aan
RemcoDas 24:ddd69385b55f 280 L = true;
RemcoDas 24:ddd69385b55f 281 }
RemcoDas 24:ddd69385b55f 282 if(L && R){
RemcoDas 24:ddd69385b55f 283 if(!(ptmrL.read() >0.3 && ptmrR.read()>0.3) != !(emgModeL!=1 && emgModeR!=1)) { // beide aangespannenR = false;
RemcoDas 24:ddd69385b55f 284 state = FIRE;
RemcoDas 24:ddd69385b55f 285 }
RemcoDas 24:ddd69385b55f 286 else if(!(ptmrL.read() >= 0.8 != !(emgModeL == 3))) { // links vol ingedrukt = schieten
RemcoDas 24:ddd69385b55f 287 state = FIRE;
RemcoDas 24:ddd69385b55f 288 }
RemcoDas 24:ddd69385b55f 289 else if(!(ptmrR.read() > 0.8) != !(emgModeR == 3)) { //rechts vol ingedrukt = schieten
RemcoDas 24:ddd69385b55f 290 state = FIRE;
RemcoDas 24:ddd69385b55f 291 }
RemcoDas 24:ddd69385b55f 292 else if((!(ptmrL.read() > 0.3 && ptmrL.read() < 0.6) != !(emgModeL == 2)) && L) { // links ingedrukt = rem verlagen
RemcoDas 24:ddd69385b55f 293 if(remPerc>1){
RemcoDas 24:ddd69385b55f 294 remPerc--; // Rem percentage verlagen met 1
RemcoDas 24:ddd69385b55f 295 }
RemcoDas 24:ddd69385b55f 296 L = false;
RemcoDas 24:ddd69385b55f 297
RemcoDas 24:ddd69385b55f 298 }
RemcoDas 24:ddd69385b55f 299 else if((!(ptmrR.read() > 0.3 && ptmrR.read() < 0.6) != !(emgModeR == 2)) && R) { // Rechts half ingedrukt = rem verhogen
RemcoDas 24:ddd69385b55f 300 if(remPerc<9){
RemcoDas 24:ddd69385b55f 301 remPerc++; // rem percentage verhogen met 1
RemcoDas 24:ddd69385b55f 302 }
RemcoDas 24:ddd69385b55f 303 R = false;
RemcoDas 24:ddd69385b55f 304 }
RemcoDas 24:ddd69385b55f 305
RemcoDas 24:ddd69385b55f 306 if((ptmrL.read() > 0.3 || ptmrR.read() > 0.3) || (emgModeL != 1 || emgModeR !=1)){ // Print rem scale als een of beide knoppen aan zijn
RemcoDas 24:ddd69385b55f 307 pc.printf("Current breaking scale: %i\r\n", abs(remPerc));
RemcoDas 24:ddd69385b55f 308 }
RemcoDas 24:ddd69385b55f 309 }
RemcoDas 24:ddd69385b55f 310 }
RemcoDas 24:ddd69385b55f 311 }
RemcoDas 24:ddd69385b55f 312 L = false;
RemcoDas 24:ddd69385b55f 313 R = false;
RemcoDas 24:ddd69385b55f 314 break;
RemcoDas 24:ddd69385b55f 315 }
RemcoDas 24:ddd69385b55f 316 case FIRE: {
RemcoDas 24:ddd69385b55f 317 pc.printf("Shooting\r\n");
RemcoDas 24:ddd69385b55f 318 servo.pulsewidth(servoL); // schrijfrem release
RemcoDas 24:ddd69385b55f 319 pwmM1.write(0.5); // motor aanzetten
RemcoDas 24:ddd69385b55f 320 bool servoPos = true;
RemcoDas 24:ddd69385b55f 321 while(state == FIRE){ // Zolang in FIRE state
RemcoDas 24:ddd69385b55f 322 if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(remPerc)/10)){ // Rem goFlag en half rondje = remmen
RemcoDas 24:ddd69385b55f 323 remFlag = false;
RemcoDas 24:ddd69385b55f 324 if(servoPos){ //
RemcoDas 24:ddd69385b55f 325 servoPos = false;
RemcoDas 24:ddd69385b55f 326 servo.pulsewidth(servoL); // links
RemcoDas 24:ddd69385b55f 327 }
RemcoDas 24:ddd69385b55f 328 else{
RemcoDas 24:ddd69385b55f 329 servoPos = true;
RemcoDas 24:ddd69385b55f 330 servo.pulsewidth(servoR); // rechts
RemcoDas 24:ddd69385b55f 331 }
RemcoDas 24:ddd69385b55f 332 }
RemcoDas 24:ddd69385b55f 333 if(regelaarFlag){
RemcoDas 24:ddd69385b55f 334 regelaarFlag = false;
RemcoDas 24:ddd69385b55f 335 //pc.printf("Slinger encoder: %i\r\n", enc1.getPulses()%4200);
RemcoDas 24:ddd69385b55f 336 if((abs(enc1.getPulses())+50)%4200 < 150){ // Slinger 1 rondje laten draaien
RemcoDas 24:ddd69385b55f 337 pwmM1.write(0); // motor uitzetten
RemcoDas 24:ddd69385b55f 338 pc.printf("Pendulum on startposition\r\n");
RemcoDas 24:ddd69385b55f 339 state = AIM; // Aim is volgende fase
RemcoDas 24:ddd69385b55f 340 }
RemcoDas 24:ddd69385b55f 341 }
RemcoDas 24:ddd69385b55f 342 }
RemcoDas 24:ddd69385b55f 343 break;
RemcoDas 24:ddd69385b55f 344 }
Bartvaart 19:6c0245063b96 345 }
Bartvaart 17:cfe44346645c 346 }
RemcoDas 24:ddd69385b55f 347 }