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@31:074b9d03d816, 2015-10-19 (annotated)

Committer:
RemcoDas
Date:
Mon Oct 19 10:30:32 2015 +0000
Revision:
31:074b9d03d816
Parent:
30:8ae855348d22
Child:
32:40691708c68c
Final met pendulum kalibrate 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 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 30:8ae855348d22 18 HIDScope scope(6);
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 30:8ae855348d22 44 double yL = 0, yR = 0; // y waarden EMG links en rechts
RemcoDas 25:c97d079e07f3 45
RemcoDas 27:f62e450bb411 46 volatile bool L = false, R = false, RH = 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 27:f62e450bb411 61 void EMGkalibratieL(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
RemcoDas 27:f62e450bb411 62 LedB = 1;
RemcoDas 27:f62e450bb411 63 Init();
RemcoDas 30:8ae855348d22 64 double ymin = KalibratieMin(emgL, true);
RemcoDas 27:f62e450bb411 65 wait(1);
RemcoDas 30:8ae855348d22 66 double ymax = KalibratieMax(emgL, true);
RemcoDas 27:f62e450bb411 67
RemcoDas 27:f62e450bb411 68 if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
RemcoDas 27:f62e450bb411 69 ymin = 10;
RemcoDas 27:f62e450bb411 70 ymax = 10;
RemcoDas 27:f62e450bb411 71 }
RemcoDas 30:8ae855348d22 72 thresholdlowL = 4 * ymin; // standaardwaarde
RemcoDas 27:f62e450bb411 73 thresholdmidL = 0.5 * ymax; // afhankelijk van max output gebruiker
RemcoDas 27:f62e450bb411 74 thresholdhighL = 0.8 * ymax;
RemcoDas 27:f62e450bb411 75
RemcoDas 27:f62e450bb411 76 pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin); //bugfix
RemcoDas 27:f62e450bb411 77 }
RemcoDas 27:f62e450bb411 78 void EMGkalibratieR(){ // Bepalen thresholds, aangenomen dat spieren links en rechts hetzelfde zijn
RemcoDas 25:c97d079e07f3 79 LedB = 1;
RemcoDas 24:ddd69385b55f 80 Init();
RemcoDas 30:8ae855348d22 81 double ymin = KalibratieMin(emgR, false);
Bartvaart 19:6c0245063b96 82 wait(1);
RemcoDas 30:8ae855348d22 83 double ymax = KalibratieMax(emgR, false);
RemcoDas 27:f62e450bb411 84
RemcoDas 27:f62e450bb411 85 if((ymax-ymin) < 0.05){ // voor als er geen kabels in de EMG zitten
RemcoDas 27:f62e450bb411 86 ymin = 10;
RemcoDas 27:f62e450bb411 87 ymax = 10;
RemcoDas 29:9610df479f89 88 }
RemcoDas 30:8ae855348d22 89 thresholdlowR = 4 * ymin; // standaardwaarde
RemcoDas 27:f62e450bb411 90 thresholdmidR = 0.5 * ymax; // afhankelijk van max output gebruiker
RemcoDas 27:f62e450bb411 91 thresholdhighR = 0.8 * ymax;
Bartvaart 22:c1811e60bfce 92
RemcoDas 27:f62e450bb411 93 pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); //bugfix
RemcoDas 24:ddd69385b55f 94 }
RemcoDas 27:f62e450bb411 95 int EMGfilter(AnalogIn& emg, bool side){
RemcoDas 30:8ae855348d22 96 double u = emg.read(); // uitlezen emg signaal (linker of rechter EMG)
RemcoDas 27:f62e450bb411 97 int mode = 1;
RemcoDas 30:8ae855348d22 98 if(side){
RemcoDas 30:8ae855348d22 99 double y = FilterdesignsLeft(u); // signaal filteren // linker EMG
RemcoDas 27:f62e450bb411 100 mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
RemcoDas 27:f62e450bb411 101 }
RemcoDas 30:8ae855348d22 102 else {
RemcoDas 30:8ae855348d22 103 double y = FilterdesignsRight(u); // signaal filteren // rechter EMG
RemcoDas 27:f62e450bb411 104 mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); //bepaald welk signaal de motor krijgt (1, 2 ,3 )
RemcoDas 27:f62e450bb411 105 }
RemcoDas 25:c97d079e07f3 106 return mode;
RemcoDas 26:d9855716ced7 107 }
RemcoDas 26:d9855716ced7 108 int PotReader(AnalogIn& pot){ // potmeter uitlezen en mode bepalen (thresholds)
RemcoDas 26:d9855716ced7 109 double volt = pot.read();
RemcoDas 26:d9855716ced7 110 int mode = 1;
RemcoDas 26:d9855716ced7 111 if(volt > 0.8){
RemcoDas 26:d9855716ced7 112 mode = 3;
RemcoDas 26:d9855716ced7 113 }
RemcoDas 26:d9855716ced7 114 else if(volt>0.35 && volt<0.65){
RemcoDas 26:d9855716ced7 115 mode = 2;
RemcoDas 26:d9855716ced7 116 }
RemcoDas 26:d9855716ced7 117 return mode;
RemcoDas 26:d9855716ced7 118 }
RemcoDas 27:f62e450bb411 119 int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // bepaal mode uit emg en pot
RemcoDas 27:f62e450bb411 120 int emgMode = EMGfilter(emg, side);
RemcoDas 26:d9855716ced7 121 int potMode = PotReader(pot);
RemcoDas 26:d9855716ced7 122 int mode = 1;
RemcoDas 26:d9855716ced7 123 if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
RemcoDas 26:d9855716ced7 124 if(emgMode > potMode){ // maximum van emg en pot
RemcoDas 26:d9855716ced7 125 mode = emgMode;
RemcoDas 26:d9855716ced7 126 }
RemcoDas 26:d9855716ced7 127 else{
RemcoDas 26:d9855716ced7 128 mode = potMode;
RemcoDas 26:d9855716ced7 129 }
RemcoDas 27:f62e450bb411 130 }
RemcoDas 26:d9855716ced7 131 return mode;
RemcoDas 26:d9855716ced7 132 }
RemcoDas 25:c97d079e07f3 133 void setEmgFlag(){
RemcoDas 25:c97d079e07f3 134 emgFlag = true;
RemcoDas 24:ddd69385b55f 135 }
RemcoDas 24:ddd69385b55f 136 void btnSetAction(){ // Set knop K64F
RemcoDas 24:ddd69385b55f 137 btn = true; // GoFlag setknop
RemcoDas 26:d9855716ced7 138 }
RemcoDas 24:ddd69385b55f 139 void setRegelaarFlag(){ // Go flag button controle
RemcoDas 24:ddd69385b55f 140 regelaarFlag = true;
RemcoDas 24:ddd69385b55f 141 }
RemcoDas 24:ddd69385b55f 142 void setRemFlag(){ // Go flag rem
RemcoDas 24:ddd69385b55f 143 remFlag = true;
RemcoDas 24:ddd69385b55f 144 }
RemcoDas 24:ddd69385b55f 145 void checkAim(){ // controleer of killer switch geraakt is en of max aantal counts niet bereikt is
RemcoDas 27:f62e450bb411 146 /*double volt = KS.read();
RemcoDas 27:f62e450bb411 147 if(volt> 0.5 /*|| abs(enc2.getPulses()) > maxCounts*///){
RemcoDas 27:f62e450bb411 148 /*pwmM2.write(0); // Aim motor stilzetten
RemcoDas 24:ddd69385b55f 149 pc.printf("BOEM! CRASH! KASTUK! \r\n");
RemcoDas 27:f62e450bb411 150 }*/
RemcoDas 24:ddd69385b55f 151 }
RemcoDas 24:ddd69385b55f 152 void motorAim(int dir){ // Aim motor laten draaien met gegeven direction
RemcoDas 24:ddd69385b55f 153 dirM2.write(dir); // richting
RemcoDas 30:8ae855348d22 154 pwmM2.write(0.25); // width aanpassen
RemcoDas 24:ddd69385b55f 155 }
RemcoDas 24:ddd69385b55f 156 bool controlAim(){ // Function om aim motor aan te sturen
RemcoDas 26:d9855716ced7 157 bool both = false; // boolean of beide knoppen ingedrukt zijn
RemcoDas 30:8ae855348d22 158
RemcoDas 27:f62e450bb411 159 int modeL = defMode(emgL, potL, true);
RemcoDas 27:f62e450bb411 160 int modeR = defMode(emgR, potR, false);
RemcoDas 27:f62e450bb411 161
RemcoDas 27:f62e450bb411 162 scope.set(0, modeL);
RemcoDas 27:f62e450bb411 163 scope.set(1, modeR);
RemcoDas 27:f62e450bb411 164 scope.send(); //stuur de waardes naar HIDscope
RemcoDas 27:f62e450bb411 165
RemcoDas 28:e6d2fe0e593e 166 if(modeR < 2 && !R){ // controleer of mode 1 is geweest
RemcoDas 27:f62e450bb411 167 R = true;
RemcoDas 27:f62e450bb411 168 }
RemcoDas 28:e6d2fe0e593e 169 if(modeL < 2 && !L){
RemcoDas 28:e6d2fe0e593e 170 L = true;
RemcoDas 24:ddd69385b55f 171 }
RemcoDas 28:e6d2fe0e593e 172
RemcoDas 30:8ae855348d22 173 if((modeL>2) && (modeR >2 && R && L)) { // mode L en mode R beide > 2
RemcoDas 26:d9855716ced7 174 both = true; // Return true
RemcoDas 26:d9855716ced7 175 pwmM2.write(0); // Aim motor stilzetten
RemcoDas 28:e6d2fe0e593e 176 aimState = OFF;
RemcoDas 28:e6d2fe0e593e 177 }
RemcoDas 28:e6d2fe0e593e 178 else if((modeR == 2) && (modeL == 2)) {
RemcoDas 28:e6d2fe0e593e 179 if(aimState!=OFF){
RemcoDas 28:e6d2fe0e593e 180 pwmM2.write(0); // Aim motor stilzetten
RemcoDas 28:e6d2fe0e593e 181 aimState = OFF;
RemcoDas 28:e6d2fe0e593e 182 PRINT("Motor freeze\r\n");
RemcoDas 28:e6d2fe0e593e 183 L = false;
RemcoDas 28:e6d2fe0e593e 184 R = false;
RemcoDas 28:e6d2fe0e593e 185 }
RemcoDas 24:ddd69385b55f 186 }
RemcoDas 28:e6d2fe0e593e 187 else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND richting is niet CCW AND modeR = 1
RemcoDas 28:e6d2fe0e593e 188 if(L){
RemcoDas 28:e6d2fe0e593e 189 aimState = CCW; // draai CCW
RemcoDas 28:e6d2fe0e593e 190 pc.printf("Turn negative (CCW)\r\n");
RemcoDas 28:e6d2fe0e593e 191 motorAim(0);
RemcoDas 28:e6d2fe0e593e 192 }
RemcoDas 24:ddd69385b55f 193 }
RemcoDas 28:e6d2fe0e593e 194 else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND richting is niet CW AND modeL = 1
RemcoDas 28:e6d2fe0e593e 195 if(R){
RemcoDas 28:e6d2fe0e593e 196 aimState = CW; // draai CW
RemcoDas 28:e6d2fe0e593e 197 PRINT("Turn positive (CW)\r\n");
RemcoDas 28:e6d2fe0e593e 198 motorAim(1);
RemcoDas 28:e6d2fe0e593e 199 }
RemcoDas 27:f62e450bb411 200 }
RemcoDas 24:ddd69385b55f 201 return both;
RemcoDas 24:ddd69385b55f 202 }
RemcoDas 24:ddd69385b55f 203 int main(){
RemcoDas 24:ddd69385b55f 204 flipLed(ledR); // test of code begint
RemcoDas 24:ddd69385b55f 205 btnSet.mode(PullUp); // Button mode
RemcoDas 24:ddd69385b55f 206 btnSet.rise(&btnSetAction); // Setknop aan functie koppellen
RemcoDas 26:d9855716ced7 207 tickerRegelaar.attach(&setRegelaarFlag,tRegelaar); // ticker regelaar motor
RemcoDas 26:d9855716ced7 208 tickerRem.attach(&setRemFlag,tRem); // ticker rem
RemcoDas 26:d9855716ced7 209 EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
RemcoDas 25:c97d079e07f3 210
RemcoDas 24:ddd69385b55f 211 pc.printf("\n\n\n\n\n");
RemcoDas 25:c97d079e07f3 212 PRINT("--- NEW GAME ---\r\n");
RemcoDas 26:d9855716ced7 213 while(1){ // Programma door laten lopen
RemcoDas 24:ddd69385b55f 214 switch(state){
RemcoDas 27:f62e450bb411 215 case KALIBRATE_EMG: {
RemcoDas 27:f62e450bb411 216 PRINT("Kalibrate EMG left in 5 seconds\r\n");
RemcoDas 27:f62e450bb411 217
RemcoDas 27:f62e450bb411 218 EMGkalibratieL();
RemcoDas 24:ddd69385b55f 219
RemcoDas 27:f62e450bb411 220 PRINT("Kalibrate EMG right in 5 seconds\r\n");
RemcoDas 27:f62e450bb411 221 EMGkalibratieR();
RemcoDas 24:ddd69385b55f 222
RemcoDas 24:ddd69385b55f 223 state = KALIBRATE_AIM;
RemcoDas 24:ddd69385b55f 224 break;
RemcoDas 24:ddd69385b55f 225 }
RemcoDas 24:ddd69385b55f 226 case KALIBRATE_AIM: {
RemcoDas 24:ddd69385b55f 227 pwmM2.period(1/100000); // aanstuurperiode motor 2
RemcoDas 24:ddd69385b55f 228 PRINT("Kalibrate aim motor\r\n");
RemcoDas 24:ddd69385b55f 229 wait(1);
RemcoDas 24:ddd69385b55f 230 pc.printf("Use L and R to move pendulum to equilibruim position\r\n L+R = OK\r\n");
RemcoDas 24:ddd69385b55f 231 while(state==KALIBRATE_AIM){
RemcoDas 24:ddd69385b55f 232 if(regelaarFlag){ // motor regelen op GoFlag
RemcoDas 24:ddd69385b55f 233 regelaarFlag = false;
RemcoDas 26:d9855716ced7 234 checkAim(); // Controleer positie
RemcoDas 29:9610df479f89 235 }
RemcoDas 25:c97d079e07f3 236 if(emgFlag){ // Go flag EMG sampelen
RemcoDas 26:d9855716ced7 237 emgFlag = false;
RemcoDas 24:ddd69385b55f 238 if(controlAim()){ // Buttons met control, if true = beide knoppen = bevestigen
RemcoDas 24:ddd69385b55f 239 enc2.reset(); // resetknop = encoder 1 resetten
RemcoDas 24:ddd69385b55f 240 pc.printf("Positie gekalibreerd, kalibreer nu slinger, encoder counts: %i\r\n", enc2.getPulses());
RemcoDas 24:ddd69385b55f 241 state = KALIBRATE_PEND; // volgende state
RemcoDas 24:ddd69385b55f 242 }
RemcoDas 24:ddd69385b55f 243 }
RemcoDas 24:ddd69385b55f 244 }
RemcoDas 24:ddd69385b55f 245 break;
RemcoDas 24:ddd69385b55f 246 }
RemcoDas 24:ddd69385b55f 247 case KALIBRATE_PEND: {
RemcoDas 24:ddd69385b55f 248 pc.printf("Kalibrate pendulum motor with setknop\r\n");
RemcoDas 24:ddd69385b55f 249 pwmM1.period(1/100000); // aanstuurperiode motor 1
RemcoDas 24:ddd69385b55f 250 servo.period(0.02); // 20ms period aanstuurperiode
RemcoDas 30:8ae855348d22 251 pwmM1.write(0.25); // Motor aanzetten, laag vermogen
RemcoDas 24:ddd69385b55f 252 btn = false;
RemcoDas 24:ddd69385b55f 253 while(state==KALIBRATE_PEND){
RemcoDas 31:074b9d03d816 254 if(emgFlag){
RemcoDas 24:ddd69385b55f 255 pc.printf(""); // lege regel printen, anders doet setknop het niet
RemcoDas 31:074b9d03d816 256 emgFlag = false;
RemcoDas 30:8ae855348d22 257
RemcoDas 31:074b9d03d816 258 int modeL = defMode(emgL, potL, true);
RemcoDas 31:074b9d03d816 259 int modeR = defMode(emgR, potR, false);
RemcoDas 31:074b9d03d816 260
RemcoDas 31:074b9d03d816 261 if (btn || (modeL == 3) || (modeR == 3)){ // Als setknop ingedrukt is reset
RemcoDas 24:ddd69385b55f 262 pwmM1.write(0); // Motor 1 stilzetten
RemcoDas 24:ddd69385b55f 263 enc1.reset(); // encoder 1 resetten
RemcoDas 26:d9855716ced7 264 PRINT("Pendulum kalibrated\r\n");
RemcoDas 24:ddd69385b55f 265 btn = false; // knop op false
RemcoDas 24:ddd69385b55f 266 state = AIM; // volgende fase
RemcoDas 24:ddd69385b55f 267 }
RemcoDas 31:074b9d03d816 268 else if(modeL == 2){
RemcoDas 31:074b9d03d816 269 pwmM1.write(0); // Pendulum stilzetten
RemcoDas 31:074b9d03d816 270 PRINT("Pendulum off\r\n");
RemcoDas 31:074b9d03d816 271 }
RemcoDas 31:074b9d03d816 272 else if(modeL == 3){
RemcoDas 31:074b9d03d816 273 pwmM1.write(0.025); // Pendulum aanzetten
RemcoDas 31:074b9d03d816 274 PRINT("Pendulum on\r\n");
RemcoDas 31:074b9d03d816 275 }
RemcoDas 24:ddd69385b55f 276 }
RemcoDas 24:ddd69385b55f 277 }
RemcoDas 24:ddd69385b55f 278 break;
RemcoDas 24:ddd69385b55f 279 }
RemcoDas 24:ddd69385b55f 280 case AIM: {
RemcoDas 24:ddd69385b55f 281 pc.printf("Aim with EMG\r\n");
RemcoDas 24:ddd69385b55f 282 while(state == AIM){
RemcoDas 24:ddd69385b55f 283 if(regelaarFlag){ // motor regelen op GoFlag
RemcoDas 24:ddd69385b55f 284 regelaarFlag = false;
RemcoDas 24:ddd69385b55f 285 checkAim(); // Controleer positie
RemcoDas 24:ddd69385b55f 286 }
RemcoDas 25:c97d079e07f3 287 if(emgFlag){ // Go flag EMG sampelen
RemcoDas 26:d9855716ced7 288 emgFlag = false;
RemcoDas 24:ddd69385b55f 289 if(controlAim()){
RemcoDas 24:ddd69385b55f 290 pc.printf("Position choosen, adjust shoot velocity\r\n");
RemcoDas 24:ddd69385b55f 291 state = REM; // volgende state (REM)
RemcoDas 24:ddd69385b55f 292 }
RemcoDas 24:ddd69385b55f 293 }
RemcoDas 24:ddd69385b55f 294 }
RemcoDas 24:ddd69385b55f 295 break;
RemcoDas 24:ddd69385b55f 296 }
RemcoDas 24:ddd69385b55f 297 case REM: {
RemcoDas 24:ddd69385b55f 298 pc.printf("Set break percentage, current is: %.0f\r\n", remPerc);
RemcoDas 24:ddd69385b55f 299 L = false;
RemcoDas 24:ddd69385b55f 300 R = false;
RemcoDas 26:d9855716ced7 301 while(state == REM){
RemcoDas 26:d9855716ced7 302 if(emgFlag){ // Go flag EMG sampelen
RemcoDas 26:d9855716ced7 303 emgFlag = false;
RemcoDas 27:f62e450bb411 304 int modeL = defMode(emgL, potL, true);
RemcoDas 27:f62e450bb411 305 int modeR = defMode(emgR, potR, false);
RemcoDas 27:f62e450bb411 306
RemcoDas 26:d9855716ced7 307 if(modeR < 2) { // R is niet aan
RemcoDas 24:ddd69385b55f 308 R = true;
RemcoDas 24:ddd69385b55f 309 }
RemcoDas 26:d9855716ced7 310 if(modeL < 2) { // L is niet aan
RemcoDas 24:ddd69385b55f 311 L = true;
RemcoDas 24:ddd69385b55f 312 }
RemcoDas 24:ddd69385b55f 313 if(L && R){
RemcoDas 27:f62e450bb411 314 if((modeL > 1) && modeR > 1) { // beide aangespannenR = false;
RemcoDas 24:ddd69385b55f 315 state = FIRE;
RemcoDas 30:8ae855348d22 316 R = false;
RemcoDas 30:8ae855348d22 317 L = false;
RemcoDas 24:ddd69385b55f 318 }
RemcoDas 26:d9855716ced7 319 else if(modeL > 2 || modeR > 2) {// links of rechts vol ingedrukt = schieten
RemcoDas 24:ddd69385b55f 320 state = FIRE;
RemcoDas 30:8ae855348d22 321 R = false;
RemcoDas 30:8ae855348d22 322 L = false;
RemcoDas 26:d9855716ced7 323 }
RemcoDas 27:f62e450bb411 324 else if((modeL == 2) && L) { // links ingedrukt = rem verlagen
RemcoDas 24:ddd69385b55f 325 if(remPerc>1){
RemcoDas 26:d9855716ced7 326 remPerc--; // Rem percentage verlagen met 1
RemcoDas 24:ddd69385b55f 327 }
RemcoDas 26:d9855716ced7 328 L = false;
RemcoDas 24:ddd69385b55f 329 }
RemcoDas 27:f62e450bb411 330 else if((modeR == 2) && R) { // Rechts half ingedrukt = rem verhogen
RemcoDas 27:f62e450bb411 331 if(remPerc<10){
RemcoDas 26:d9855716ced7 332 remPerc++; // rem percentage verhogen met 1
RemcoDas 24:ddd69385b55f 333 }
RemcoDas 24:ddd69385b55f 334 R = false;
RemcoDas 26:d9855716ced7 335 }
RemcoDas 26:d9855716ced7 336 if(modeL > 1 || modeR > 1){ // Print rem scale als een of beide knoppen aan zijn
RemcoDas 26:d9855716ced7 337 pc.printf("Current breaking scale: %i\r\n", remPerc);
RemcoDas 26:d9855716ced7 338 }
RemcoDas 26:d9855716ced7 339 }
RemcoDas 24:ddd69385b55f 340 }
RemcoDas 24:ddd69385b55f 341 }
RemcoDas 24:ddd69385b55f 342 L = false;
RemcoDas 24:ddd69385b55f 343 R = false;
RemcoDas 24:ddd69385b55f 344 break;
RemcoDas 24:ddd69385b55f 345 }
RemcoDas 24:ddd69385b55f 346 case FIRE: {
RemcoDas 24:ddd69385b55f 347 pc.printf("Shooting\r\n");
RemcoDas 24:ddd69385b55f 348 servo.pulsewidth(servoL); // schrijfrem release
RemcoDas 30:8ae855348d22 349 pwmM1.write(0.25); // motor aanzetten
RemcoDas 24:ddd69385b55f 350 bool servoPos = true;
RemcoDas 24:ddd69385b55f 351 while(state == FIRE){ // Zolang in FIRE state
RemcoDas 30:8ae855348d22 352 if(remFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(remPerc)/10)){ // Rem goFlag en half rondje = remmen
RemcoDas 24:ddd69385b55f 353 remFlag = false;
RemcoDas 24:ddd69385b55f 354 if(servoPos){ //
RemcoDas 24:ddd69385b55f 355 servoPos = false;
RemcoDas 24:ddd69385b55f 356 servo.pulsewidth(servoL); // links
RemcoDas 24:ddd69385b55f 357 }
RemcoDas 24:ddd69385b55f 358 else{
RemcoDas 24:ddd69385b55f 359 servoPos = true;
RemcoDas 24:ddd69385b55f 360 servo.pulsewidth(servoR); // rechts
RemcoDas 24:ddd69385b55f 361 }
RemcoDas 24:ddd69385b55f 362 }
RemcoDas 24:ddd69385b55f 363 if(regelaarFlag){
RemcoDas 26:d9855716ced7 364 regelaarFlag = false;
RemcoDas 24:ddd69385b55f 365 if((abs(enc1.getPulses())+50)%4200 < 150){ // Slinger 1 rondje laten draaien
RemcoDas 24:ddd69385b55f 366 pwmM1.write(0); // motor uitzetten
RemcoDas 26:d9855716ced7 367 PRINT("Pendulum on startposition\r\n");
RemcoDas 24:ddd69385b55f 368 state = AIM; // Aim is volgende fase
RemcoDas 24:ddd69385b55f 369 }
RemcoDas 24:ddd69385b55f 370 }
RemcoDas 24:ddd69385b55f 371 }
RemcoDas 24:ddd69385b55f 372 break;
RemcoDas 24:ddd69385b55f 373 }
Bartvaart 19:6c0245063b96 374 }
Bartvaart 17:cfe44346645c 375 }
RemcoDas 24:ddd69385b55f 376 }