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@33:b4757132437e, 2015-10-19 (annotated)

Committer:
RemcoDas
Date:
Mon Oct 19 20:14:13 2015 +0000
Revision:
33:b4757132437e
Parent:
32:40691708c68c
Child:
34:60391fb72629
Documentatie bijgewerkt naar English

Who changed what in which revision?

UserRevisionLine numberNew 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
RemcoDas 24:ddd69385b55f 18 AnalogIn KS(A4); // Killswitch
RemcoDas 33:b4757132437e 19 HIDScope scope(6); // Hidscope, amount of scopes
RemcoDas 33:b4757132437e 20 Ticker EMGticker, tickerControl, tickerBreak; // 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);
RemcoDas 33:b4757132437e 29 enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, ACKNOWLEDGEMENT switch: BMT groep 4 2014
RemcoDas 33:b4757132437e 30 uint8_t state = KALIBRATE_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
RemcoDas 33:b4757132437e 35 int maxCounts = 42000; // 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)
RemcoDas 33:b4757132437e 38 const double tControl = 0.005, tBreak = 0.1; // ticker for motor checking
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?
RemcoDas 33:b4757132437e 46 volatile bool controlFlag = false, btnFlag = false, breakFlag = false, emgFlag = 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 }
RemcoDas 33:b4757132437e 57 void EMGkalibratieL(){ // Determine thresholds left
RemcoDas 27:f62e450bb411 58 LedB = 1;
RemcoDas 27:f62e450bb411 59 Init();
RemcoDas 30:8ae855348d22 60 double ymin = KalibratieMin(emgL, true);
RemcoDas 27:f62e450bb411 61 wait(1);
RemcoDas 30:8ae855348d22 62 double ymax = KalibratieMax(emgL, true);
RemcoDas 27:f62e450bb411 63
RemcoDas 33:b4757132437e 64 if((ymax-ymin) < 0.05){ // No EMG connected
RemcoDas 27:f62e450bb411 65 ymin = 10;
RemcoDas 27:f62e450bb411 66 ymax = 10;
RemcoDas 27:f62e450bb411 67 }
RemcoDas 33:b4757132437e 68 thresholdlowL = 4 * ymin; // Lowest threshold
RemcoDas 33:b4757132437e 69 thresholdmidL = 0.5 * ymax; // Midi threshold
RemcoDas 33:b4757132437e 70 thresholdhighL = 0.8 * ymax; // Highest threshold
RemcoDas 27:f62e450bb411 71
RemcoDas 33:b4757132437e 72 pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
RemcoDas 27:f62e450bb411 73 }
RemcoDas 33:b4757132437e 74 void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
RemcoDas 25:c97d079e07f3 75 LedB = 1;
RemcoDas 24:ddd69385b55f 76 Init();
RemcoDas 30:8ae855348d22 77 double ymin = KalibratieMin(emgR, false);
Bartvaart 19:6c0245063b96 78 wait(1);
RemcoDas 30:8ae855348d22 79 double ymax = KalibratieMax(emgR, false);
RemcoDas 27:f62e450bb411 80
RemcoDas 33:b4757132437e 81 if((ymax-ymin) < 0.05){
RemcoDas 27:f62e450bb411 82 ymin = 10;
RemcoDas 27:f62e450bb411 83 ymax = 10;
RemcoDas 29:9610df479f89 84 }
RemcoDas 33:b4757132437e 85 thresholdlowR = 4 * ymin;
RemcoDas 33:b4757132437e 86 thresholdmidR = 0.5 * ymax;
RemcoDas 27:f62e450bb411 87 thresholdhighR = 0.8 * ymax;
Bartvaart 22:c1811e60bfce 88
RemcoDas 33:b4757132437e 89 pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal
RemcoDas 24:ddd69385b55f 90 }
RemcoDas 27:f62e450bb411 91 int EMGfilter(AnalogIn& emg, bool side){
RemcoDas 33:b4757132437e 92 double u = emg.read(); // read emg signal (left or right EMG)
RemcoDas 27:f62e450bb411 93 int mode = 1;
RemcoDas 30:8ae855348d22 94 if(side){
RemcoDas 33:b4757132437e 95 double y = FilterdesignsLeft(u); // filter signal left EMG
RemcoDas 33:b4757132437e 96 mode = Mode(y, thresholdlowL, thresholdmidL, thresholdhighL); // Determine mode with thresholds (1, 2, 3)
RemcoDas 27:f62e450bb411 97 }
RemcoDas 30:8ae855348d22 98 else {
RemcoDas 33:b4757132437e 99 double y = FilterdesignsRight(u);
RemcoDas 33:b4757132437e 100 mode = Mode(y, thresholdlowR, thresholdmidR, thresholdhighR); // right EMG
RemcoDas 27:f62e450bb411 101 }
RemcoDas 25:c97d079e07f3 102 return mode;
RemcoDas 26:d9855716ced7 103 }
RemcoDas 33:b4757132437e 104 int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
RemcoDas 26:d9855716ced7 105 double volt = pot.read();
RemcoDas 26:d9855716ced7 106 int mode = 1;
RemcoDas 26:d9855716ced7 107 if(volt > 0.8){
RemcoDas 26:d9855716ced7 108 mode = 3;
RemcoDas 26:d9855716ced7 109 }
RemcoDas 26:d9855716ced7 110 else if(volt>0.35 && volt<0.65){
RemcoDas 26:d9855716ced7 111 mode = 2;
RemcoDas 26:d9855716ced7 112 }
RemcoDas 26:d9855716ced7 113 return mode;
RemcoDas 26:d9855716ced7 114 }
RemcoDas 33:b4757132437e 115 int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // Determine mode both from EMG and Potentialmeter, ONE of them must be ONE!
RemcoDas 27:f62e450bb411 116 int emgMode = EMGfilter(emg, side);
RemcoDas 26:d9855716ced7 117 int potMode = PotReader(pot);
RemcoDas 26:d9855716ced7 118 int mode = 1;
RemcoDas 33:b4757132437e 119 if(!(emgMode==1) != !(potMode==1)){ // emgMode = 1 XOR potMode = 1
RemcoDas 33:b4757132437e 120 if(emgMode > potMode){ // maximum of emg and pot
RemcoDas 26:d9855716ced7 121 mode = emgMode;
RemcoDas 26:d9855716ced7 122 }
RemcoDas 26:d9855716ced7 123 else{
RemcoDas 26:d9855716ced7 124 mode = potMode;
RemcoDas 26:d9855716ced7 125 }
RemcoDas 27:f62e450bb411 126 }
RemcoDas 26:d9855716ced7 127 return mode;
RemcoDas 26:d9855716ced7 128 }
RemcoDas 33:b4757132437e 129 void setEmgFlag(){ // Goflag EMG
RemcoDas 25:c97d079e07f3 130 emgFlag = true;
RemcoDas 24:ddd69385b55f 131 }
RemcoDas 24:ddd69385b55f 132 void btnSetAction(){ // Set knop K64F
RemcoDas 24:ddd69385b55f 133 btn = true; // GoFlag setknop
RemcoDas 26:d9855716ced7 134 }
RemcoDas 33:b4757132437e 135 void setControlFlag(){ // Go flag setButton
RemcoDas 33:b4757132437e 136 controlFlag = true;
RemcoDas 24:ddd69385b55f 137 }
RemcoDas 33:b4757132437e 138 void setBreakFlag(){ // Go flag Break
RemcoDas 33:b4757132437e 139 breakFlag = true;
RemcoDas 24:ddd69385b55f 140 }
RemcoDas 33:b4757132437e 141 void checkAim(){ // check if Killswitch is on or max counts is reached
RemcoDas 33:b4757132437e 142 double volt = KS.read();
RemcoDas 33:b4757132437e 143 if(volt> 0.5 || abs(enc2.getPulses()) > maxCounts*/){
RemcoDas 33:b4757132437e 144 pwmM2.write(0); // Aim motor freeze
RemcoDas 33:b4757132437e 145 pc.printf("BOEM! CRASH! KASTUK! \r\n"); // Terminal
RemcoDas 33:b4757132437e 146 PRINT("BOEM! CRASH!"); // LCD
RemcoDas 33:b4757132437e 147 }
RemcoDas 24:ddd69385b55f 148 }
RemcoDas 33:b4757132437e 149 void motorAim(int dir){ // Rotate Aim motor with given direction
RemcoDas 33:b4757132437e 150 dirM2.write(dir);
RemcoDas 33:b4757132437e 151 pwmM2.write(0.25);
RemcoDas 24:ddd69385b55f 152 }
RemcoDas 33:b4757132437e 153 bool controlAim(){ // Function to control aim motor with modes
RemcoDas 33:b4757132437e 154 bool both = false; // boolean if both modes are 3
RemcoDas 27:f62e450bb411 155 int modeL = defMode(emgL, potL, true);
RemcoDas 27:f62e450bb411 156 int modeR = defMode(emgR, potR, false);
RemcoDas 27:f62e450bb411 157
RemcoDas 27:f62e450bb411 158 scope.set(0, modeL);
RemcoDas 27:f62e450bb411 159 scope.set(1, modeR);
RemcoDas 33:b4757132437e 160 scope.send(); //send values to HIDScope
RemcoDas 27:f62e450bb411 161
RemcoDas 33:b4757132437e 162 if(modeR < 2 && !R){ // control if mode has been 1
RemcoDas 27:f62e450bb411 163 R = true;
RemcoDas 27:f62e450bb411 164 }
RemcoDas 28:e6d2fe0e593e 165 if(modeL < 2 && !L){
RemcoDas 28:e6d2fe0e593e 166 L = true;
RemcoDas 24:ddd69385b55f 167 }
RemcoDas 28:e6d2fe0e593e 168
RemcoDas 33:b4757132437e 169 if((modeL>2) && (modeR >2 && R && L)) { // mode L and mode R both 3, and both has been 1 herefore
RemcoDas 26:d9855716ced7 170 both = true; // Return true
RemcoDas 33:b4757132437e 171 pwmM2.write(0); // Aim motor freeze
RemcoDas 33:b4757132437e 172 aimState = OFF; // next state
RemcoDas 28:e6d2fe0e593e 173 }
RemcoDas 33:b4757132437e 174 else if((modeR == 2) && (modeL == 2)) { // modes are both 2
RemcoDas 33:b4757132437e 175 if(aimState!=OFF){ // only if aim motor is rotating
RemcoDas 33:b4757132437e 176 pwmM2.write(0); // Aim motor freeze
RemcoDas 33:b4757132437e 177 aimState = OFF; // motor state is off
RemcoDas 33:b4757132437e 178 PRINT("Motor freeze\r\n"); // LCD
RemcoDas 33:b4757132437e 179 L = false; // Modes must be first 1 for another action
RemcoDas 33:b4757132437e 180 R = false; // ""
RemcoDas 28:e6d2fe0e593e 181 }
RemcoDas 24:ddd69385b55f 182 }
RemcoDas 33:b4757132437e 183 else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
RemcoDas 28:e6d2fe0e593e 184 if(L){
RemcoDas 33:b4757132437e 185 aimState = CCW; // Rotate CCW
RemcoDas 33:b4757132437e 186 pc.printf("Rotate -, CCW");
RemcoDas 28:e6d2fe0e593e 187 motorAim(0);
RemcoDas 28:e6d2fe0e593e 188 }
RemcoDas 24:ddd69385b55f 189 }
RemcoDas 33:b4757132437e 190 else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
RemcoDas 28:e6d2fe0e593e 191 if(R){
RemcoDas 33:b4757132437e 192 aimState = CW; // Rotate CW
RemcoDas 33:b4757132437e 193 PRINT("Rotate +, CW");
RemcoDas 28:e6d2fe0e593e 194 motorAim(1);
RemcoDas 28:e6d2fe0e593e 195 }
RemcoDas 27:f62e450bb411 196 }
RemcoDas 24:ddd69385b55f 197 return both;
RemcoDas 24:ddd69385b55f 198 }
RemcoDas 24:ddd69385b55f 199 int main(){
RemcoDas 33:b4757132437e 200 flipLed(ledR); // test if code begins
RemcoDas 24:ddd69385b55f 201 btnSet.mode(PullUp); // Button mode
RemcoDas 33:b4757132437e 202 btnSet.rise(&btnSetAction); // Connect button to function
RemcoDas 33:b4757132437e 203 tickerControl.attach(&setControlFlag,tControl); // ticker control motor
RemcoDas 33:b4757132437e 204 tickerBreak.attach(&setBreakFlag,tBreak); // ticker break
RemcoDas 26:d9855716ced7 205 EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
RemcoDas 25:c97d079e07f3 206
RemcoDas 33:b4757132437e 207 pc.printf("\n\n\n\n\n"); // Terminal
RemcoDas 33:b4757132437e 208 pc.prinft("---NEW GAME---\r\n)
RemcoDas 33:b4757132437e 209 PRINT("--- NEW GAME ---"); // LCD
RemcoDas 33:b4757132437e 210 while(1){ // Run program
RemcoDas 24:ddd69385b55f 211 switch(state){
RemcoDas 27:f62e450bb411 212 case KALIBRATE_EMG: {
RemcoDas 33:b4757132437e 213 pc.printf("Kalibrate EMG left in 5 seconds\r\n"); // Terminal
RemcoDas 33:b4757132437e 214 PRINT("EMG LEFT MAX MIN"); // LCD
RemcoDas 33:b4757132437e 215 EMGkalibratieL(); // Kalibrate left EMG, determine thresholds
RemcoDas 24:ddd69385b55f 216
RemcoDas 33:b4757132437e 217 pc.printf("Kalibrate EMG right in 5 seconds\r\n"); // Terminal
RemcoDas 33:b4757132437e 218 PRINT("EMG RIGHT MAX MIN"); // LCD
RemcoDas 33:b4757132437e 219 EMGkalibratieR(); // Kalibrate right EMG, determine thresholds
RemcoDas 24:ddd69385b55f 220
RemcoDas 33:b4757132437e 221 state = KALIBRATE_AIM; // Next state
RemcoDas 24:ddd69385b55f 222 break;
RemcoDas 24:ddd69385b55f 223 }
RemcoDas 24:ddd69385b55f 224 case KALIBRATE_AIM: {
RemcoDas 33:b4757132437e 225 pwmM2.period(1/100000); // period motor 2
RemcoDas 33:b4757132437e 226 printf("Position is kalibrating\r\n"); // terminal
RemcoDas 33:b4757132437e 227 PRINT("Wait a moment, please"); // LCD
RemcoDas 33:b4757132437e 228 dirM2.write(0); // direction aim motor
RemcoDas 33:b4757132437e 229 pwmM2.write(0.25); // percentage motor power
RemcoDas 33:b4757132437e 230 bool kalibrated = false;
RemcoDas 33:b4757132437e 231 while(state==KALIBRATE_AIM){
RemcoDas 33:b4757132437e 232 pc.printf("Killswitch: %f\r\n", KS.read()); // terminal
RemcoDas 33:b4757132437e 233 if(KS.read() > 0.5){ // Killswitch?
RemcoDas 33:b4757132437e 234 pwmM2.write(0); // Aim motor freeze
RemcoDas 33:b4757132437e 235 enc2.reset(); // Reset encoder
RemcoDas 33:b4757132437e 236 PRINT("Aim kalibrated");
RemcoDas 33:b4757132437e 237 //state = KALIBRATE_PEND; // next state
RemcoDas 33:b4757132437e 238 kalibrated = true;
RemcoDas 33:b4757132437e 239 dirM2.write(0); // direction aim motor
RemcoDas 33:b4757132437e 240 pwmM2.write(0.25); // percentage motor power, turn it on
RemcoDas 32:40691708c68c 241 }
RemcoDas 32:40691708c68c 242
RemcoDas 33:b4757132437e 243 if(controlFlag && kalibrated){ // motor regelen op GoFlag
RemcoDas 33:b4757132437e 244 controlFlag = false;
RemcoDas 33:b4757132437e 245 if(enc.getPulses() > maxCounts/2){ // rotate aim motor to midi position
RemcoDas 33:b4757132437e 246 pwmM2.write(0); // Aim motor freeze
RemcoDas 33:b4757132437e 247 state = KALIBRATE_PEND; // next state
RemcoDas 33:b4757132437e 248 }
RemcoDas 33:b4757132437e 249 }
RemcoDas 24:ddd69385b55f 250 }
RemcoDas 24:ddd69385b55f 251 break;
RemcoDas 24:ddd69385b55f 252 }
RemcoDas 24:ddd69385b55f 253 case KALIBRATE_PEND: {
RemcoDas 24:ddd69385b55f 254 pc.printf("Kalibrate pendulum motor with setknop\r\n");
RemcoDas 33:b4757132437e 255 pwmM1.period(1/100000); // periode pendulum motor
RemcoDas 33:b4757132437e 256 servo.period(0.02); // 20ms period servo
RemcoDas 33:b4757132437e 257 pwmM1.write(0.25); // Turn motor on, low power
RemcoDas 33:b4757132437e 258 btn = false;
RemcoDas 33:b4757132437e 259 R = false;
RemcoDas 33:b4757132437e 260 L = false;
RemcoDas 24:ddd69385b55f 261 while(state==KALIBRATE_PEND){
RemcoDas 31:074b9d03d816 262 if(emgFlag){
RemcoDas 24:ddd69385b55f 263 pc.printf(""); // lege regel printen, anders doet setknop het niet
RemcoDas 31:074b9d03d816 264 emgFlag = false;
RemcoDas 30:8ae855348d22 265
RemcoDas 31:074b9d03d816 266 int modeL = defMode(emgL, potL, true);
RemcoDas 31:074b9d03d816 267 int modeR = defMode(emgR, potR, false);
RemcoDas 33:b4757132437e 268
RemcoDas 33:b4757132437e 269 if(modeR < 2) { // modeR == 1
RemcoDas 33:b4757132437e 270 R = true;
RemcoDas 33:b4757132437e 271 }
RemcoDas 33:b4757132437e 272 if(modeL < 2) { // modeL == 1
RemcoDas 33:b4757132437e 273 L = true;
RemcoDas 33:b4757132437e 274 }
RemcoDas 31:074b9d03d816 275
RemcoDas 33:b4757132437e 276 if (btn || (modeL == 3 && L) || (modeR == 3 && R)){ // If setbutton is on or one mode is 3, and has been 1
RemcoDas 33:b4757132437e 277 pwmM1.write(0); // Motor 1 freeze
RemcoDas 33:b4757132437e 278 enc1.reset(); // encoder 1 reset
RemcoDas 26:d9855716ced7 279 PRINT("Pendulum kalibrated\r\n");
RemcoDas 33:b4757132437e 280 btn = false; // button op false
RemcoDas 33:b4757132437e 281 state = AIM; // next state
RemcoDas 24:ddd69385b55f 282 }
RemcoDas 31:074b9d03d816 283 else if(modeL == 2){
RemcoDas 33:b4757132437e 284 pwmM1.write(0); // Pendulum freeze
RemcoDas 31:074b9d03d816 285 PRINT("Pendulum off\r\n");
RemcoDas 31:074b9d03d816 286 }
RemcoDas 31:074b9d03d816 287 else if(modeL == 3){
RemcoDas 33:b4757132437e 288 pwmM1.write(0.025); // Pendulum rotate
RemcoDas 31:074b9d03d816 289 PRINT("Pendulum on\r\n");
RemcoDas 31:074b9d03d816 290 }
RemcoDas 24:ddd69385b55f 291 }
RemcoDas 24:ddd69385b55f 292 }
RemcoDas 24:ddd69385b55f 293 break;
RemcoDas 24:ddd69385b55f 294 }
RemcoDas 24:ddd69385b55f 295 case AIM: {
RemcoDas 24:ddd69385b55f 296 pc.printf("Aim with EMG\r\n");
RemcoDas 24:ddd69385b55f 297 while(state == AIM){
RemcoDas 33:b4757132437e 298 if(controlFlag){ // motor control on GoFlag
RemcoDas 33:b4757132437e 299 controlFlag = false;
RemcoDas 33:b4757132437e 300 checkAim(); // Check position
RemcoDas 24:ddd69385b55f 301 }
RemcoDas 33:b4757132437e 302 if(emgFlag){ // Go flag EMG sampel
RemcoDas 26:d9855716ced7 303 emgFlag = false;
RemcoDas 24:ddd69385b55f 304 if(controlAim()){
RemcoDas 24:ddd69385b55f 305 pc.printf("Position choosen, adjust shoot velocity\r\n");
RemcoDas 33:b4757132437e 306 state = REM; // Next state (BREAK)
RemcoDas 24:ddd69385b55f 307 }
RemcoDas 24:ddd69385b55f 308 }
RemcoDas 24:ddd69385b55f 309 }
RemcoDas 24:ddd69385b55f 310 break;
RemcoDas 24:ddd69385b55f 311 }
RemcoDas 33:b4757132437e 312 case BREAK: {
RemcoDas 33:b4757132437e 313 pc.printf("Set break percentage, current is: %.0f\r\n", breakPerc);
RemcoDas 24:ddd69385b55f 314 L = false;
RemcoDas 24:ddd69385b55f 315 R = false;
RemcoDas 33:b4757132437e 316 while(state == BREAK){
RemcoDas 26:d9855716ced7 317 if(emgFlag){ // Go flag EMG sampelen
RemcoDas 26:d9855716ced7 318 emgFlag = false;
RemcoDas 27:f62e450bb411 319 int modeL = defMode(emgL, potL, true);
RemcoDas 27:f62e450bb411 320 int modeR = defMode(emgR, potR, false);
RemcoDas 27:f62e450bb411 321
RemcoDas 33:b4757132437e 322 if(modeR < 2) { // modeR is 1
RemcoDas 24:ddd69385b55f 323 R = true;
RemcoDas 24:ddd69385b55f 324 }
RemcoDas 33:b4757132437e 325 if(modeL < 2) { // modeL is 1
RemcoDas 24:ddd69385b55f 326 L = true;
RemcoDas 24:ddd69385b55f 327 }
RemcoDas 24:ddd69385b55f 328 if(L && R){
RemcoDas 33:b4757132437e 329 if((modeL > 1) && modeR > 1) { // both modes
RemcoDas 24:ddd69385b55f 330 state = FIRE;
RemcoDas 30:8ae855348d22 331 R = false;
RemcoDas 30:8ae855348d22 332 L = false;
RemcoDas 24:ddd69385b55f 333 }
RemcoDas 33:b4757132437e 334 else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
RemcoDas 24:ddd69385b55f 335 state = FIRE;
RemcoDas 30:8ae855348d22 336 R = false;
RemcoDas 30:8ae855348d22 337 L = false;
RemcoDas 26:d9855716ced7 338 }
RemcoDas 33:b4757132437e 339 else if((modeL == 2) && L) { // modeL = BREAK lower with one
RemcoDas 33:b4757132437e 340 if(breakPerc>1){
RemcoDas 33:b4757132437e 341 breakPerc--;
RemcoDas 24:ddd69385b55f 342 }
RemcoDas 26:d9855716ced7 343 L = false;
RemcoDas 24:ddd69385b55f 344 }
RemcoDas 33:b4757132437e 345 else if((modeR == 2) && R) { // modeR = Break +1
RemcoDas 33:b4757132437e 346 if(breakPerc<10){
RemcoDas 33:b4757132437e 347 breakPerc++;
RemcoDas 24:ddd69385b55f 348 }
RemcoDas 24:ddd69385b55f 349 R = false;
RemcoDas 26:d9855716ced7 350 }
RemcoDas 33:b4757132437e 351 if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
RemcoDas 33:b4757132437e 352 pc.printf("Current breaking scale: %i\r\n", breakPerc);
RemcoDas 33:b4757132437e 353 PRINT("Break scale is: %i", breakPerc);
RemcoDas 26:d9855716ced7 354 }
RemcoDas 26:d9855716ced7 355 }
RemcoDas 24:ddd69385b55f 356 }
RemcoDas 24:ddd69385b55f 357 }
RemcoDas 33:b4757132437e 358 L = false; // modeL must be one for another action can take place
RemcoDas 33:b4757132437e 359 R = false; // modeR ""
RemcoDas 24:ddd69385b55f 360 break;
RemcoDas 24:ddd69385b55f 361 }
RemcoDas 24:ddd69385b55f 362 case FIRE: {
RemcoDas 24:ddd69385b55f 363 pc.printf("Shooting\r\n");
RemcoDas 33:b4757132437e 364 servo.pulsewidth(servoL); // BREAK release
RemcoDas 33:b4757132437e 365 pwmM1.write(0.25); // Pendulum motor on
RemcoDas 24:ddd69385b55f 366 bool servoPos = true;
RemcoDas 33:b4757132437e 367 while(state == FIRE){ // while in FIRE state
RemcoDas 33:b4757132437e 368 if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){ // BREAK goFlag half rotation pendulum = breaking
RemcoDas 33:b4757132437e 369 breakFlag = false;
RemcoDas 33:b4757132437e 370 if(servoPos){
RemcoDas 24:ddd69385b55f 371 servoPos = false;
RemcoDas 33:b4757132437e 372 servo.pulsewidth(servoL); // left
RemcoDas 24:ddd69385b55f 373 }
RemcoDas 24:ddd69385b55f 374 else{
RemcoDas 24:ddd69385b55f 375 servoPos = true;
RemcoDas 33:b4757132437e 376 servo.pulsewidth(servoR); // right
RemcoDas 24:ddd69385b55f 377 }
RemcoDas 24:ddd69385b55f 378 }
RemcoDas 33:b4757132437e 379 if(controlFlag){
RemcoDas 33:b4757132437e 380 controlFlag = false;
RemcoDas 33:b4757132437e 381 if((abs(enc1.getPulses())+50)%4200 < 150){ // rotate pendulum one revolution
RemcoDas 33:b4757132437e 382 pwmM1.write(0); // motor pendulum off
RemcoDas 33:b4757132437e 383 pc.printf("Pendulum on startposition\r\n");
RemcoDas 33:b4757132437e 384 state = AIM; // Next stage
RemcoDas 24:ddd69385b55f 385 }
RemcoDas 24:ddd69385b55f 386 }
RemcoDas 24:ddd69385b55f 387 }
RemcoDas 24:ddd69385b55f 388 break;
RemcoDas 24:ddd69385b55f 389 }
Bartvaart 19:6c0245063b96 390 }
Bartvaart 17:cfe44346645c 391 }
RemcoDas 24:ddd69385b55f 392 }