Remco Dasselaar
/
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
Diff: main.cpp
- Revision:
- 55:a435e46299ea
- Parent:
- 54:062b9f36edf2
- Child:
- 56:1ac2487a9610
--- a/main.cpp Wed Oct 28 13:22:20 2015 +0000
+++ b/main.cpp Wed Oct 28 14:39:21 2015 +0000
@@ -26,12 +26,10 @@
// Motor2 met PWM power control and direction
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
-enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_BEAM, AIM, BREAK, FIRE}; // Program states, ACKNOWLEDGEMENT switch: BMT groep 4 2014
+enum spelfase {CALIBRATE_EMG, CALIBRATE_R, CALIBRATE_L, CALIBRATE_MID, CALIBRATE_BEAM, AIM, BREAK, FIRE}; // Program states, ACKNOWLEDGEMENT switch: BMT groep 4 2014
uint8_t state = CALIBRATE_EMG; // first state program
enum aimFase {OFF, CW, CCW}; // Aim motor possible states
-uint8_t aimState = OFF; // first state aim motor
-enum calFase {cR, cL, mid};
-uint8_t calState = cR;
+uint8_t aimState = OFF; // first state aim motor
//-------------------------------Variables---------------------------------------------------------------------
int maxCounts = 0; // max richt motor counts Aim motor
int breakPerc = 0;
@@ -224,54 +222,57 @@
case CALIBRATE_EMG: {
EMGkalibratieL(); // calibrate left EMG, determine thresholds
EMGkalibratieR(); // calibrate right EMG, determine thresholds
- state = CALIBRATE_AIM; // Next state
+ state = CALIBRATE_R; // Next state
break;
}
- case CALIBRATE_AIM: {
+ // following 3 cases kalibrate aim right killswitch, left killswitch, and go to mid position
+ case CALIBRATE_R: {
pwmM2.period(1/100000); // period motor 2
pc.printf("Position is kalibrating\r\n"); // terminal
PRINT("Wait a moment, please"); // LCD
dirM2.write(0); // direction aim motor
pwmM2.write(0.2); // percentage motor power
- bool calibrated = false;
- while(state==CALIBRATE_AIM){
+
+ while(state==CALIBRATE_R){
+ if(controlFlag){
+ controlFlag = false;
+ if((ksRight.read()>0.95)){ // Killswitch?
+ pwmM2.write(0); // Aim motor freeze
+ enc2.reset(); // Reset encoder
+ dirM2.write(1); // direction aim motor, other direction
+ pwmM2.write(0.2); // percentage motor power, turn it on
+ state = CALIBRATE_L;
+ }
+ }
+ }
+ break;
+ }
+ case CALIBRATE_L: {
+ while(state==CALIBRATE_L){
if(controlFlag){
controlFlag = false;
-
- switch(calState){
- case cR:{ // calibrate right killswitch
- if((ksRight.read()>0.95)){ // Killswitch?
- pwmM2.write(0); // Aim motor freeze
- enc2.reset(); // Reset encoder
- dirM2.write(1); // direction aim motor
- pwmM2.write(0.2); // percentage motor power, turn it on
- calState = Cl;
- break;
- }
- }
- case cL:{
- if((ksLeft.read()>0.95)){ // calibrate left killswitch
- pwmM2.write(0); // Aim motor freeze
- maxCounts = abs(enc2.getPulses());
- dirM2.write(0); // direction aim motor
- pwmM2.write(0.2); // percentage motor power, turn it on
- calState = mid;
- break;
- }
- }
- case mid:{
- if(abs(enc2.getPulses()) < (maxCounts/2)){ // rotate aim motor to midi position
- pwmM2.write(0); // Aim motor freeze
- state = CALIBRATE_BEAM; // next state
- break;
- }
- }
- }
+ if((ksLeft.read()>0.95)){ // calibrate left killswitch
+ pwmM2.write(0); // Aim motor freeze
+ maxCounts = abs(enc2.getPulses());
+ dirM2.write(0); // direction aim motor
+ pwmM2.write(0.2); // percentage motor power, turn it on
+ state = CALIBRATE_MID;
}
}
- lcd.cls();
break;
- }
+ }
+ case CALIBRATE_MID: {
+ while(state==CALIBRATE_MID){
+ if(controlFlag){
+ controlFlag = false;
+ if(abs(enc2.getPulses()) < (maxCounts/2)){ // rotate aim motor to midi position
+ pwmM2.write(0); // Aim motor freeze
+ state = CALIBRATE_BEAM; // next state
+ }
+ }
+ }
+ break;
+ }
case CALIBRATE_BEAM: {
pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
dirM1.write(0); // direction beam motor