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@26:d9855716ced7, 2015-10-16 (annotated)

Committer:
RemcoDas
Date:
Fri Oct 16 09:12:51 2015 +0000
Revision:
26:d9855716ced7
Parent:
25:c97d079e07f3
Child:
27:f62e450bb411
Samengevoegd, enkele kalibratie

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