Richard Roos
/
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
Diff: main.cpp
- Revision:
- 34:60391fb72629
- Parent:
- 33:b4757132437e
- Child:
- 35:012b2e045579
--- a/main.cpp Mon Oct 19 20:14:13 2015 +0000
+++ b/main.cpp Tue Oct 20 08:26:10 2015 +0000
@@ -17,7 +17,7 @@
AnalogIn potL(A2), potR(A3); // Potential meter left and right
AnalogIn KS(A4); // Killswitch
HIDScope scope(6); // Hidscope, amount of scopes
-Ticker EMGticker, tickerControl, tickerBreak; // Ticker for EMG, regulator and break
+Ticker EMGticker, tickerControl, tickerBreak, tickerLcd; // Ticker for EMG, regulator and break
// QEI Encoder(port 1, port 2, ,counts/rev
QEI enc1 (D13, D12, NC, 64), enc2 (D11, D10, NC, 64);
// Motor1 met PWM power control and direction
@@ -26,16 +26,16 @@
// Motor2 met PWM power control and direction
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
-enum spelfase {KALIBRATE_EMG, KALIBRATE_AIM, KALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, ACKNOWLEDGEMENT switch: BMT groep 4 2014
-uint8_t state = KALIBRATE_EMG; // first state program
+enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, 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
//-------------------------------Variables---------------------------------------------------------------------
const int on = 0, off = 1; // on off
-int maxCounts = 42000; // max richt motor counts Aim motor
+int maxCounts = 13000; // max richt motor counts Aim motor
int breakPerc = 0;
const double servoL = 0.001, servoR = 0.0011; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
-const double tControl = 0.005, tBreak = 0.1; // ticker for motor checking
+const double tControl = 0.005, tBreak = 0.1, tLcd = 1; // tickers
const double Fs = 50; //Sample frequency Hz
double t = 1/ Fs; // time EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
@@ -43,7 +43,7 @@
double yL = 0, yR = 0; // y values EMG left and right
volatile bool L = false, R = false; // Booleans for checking if mode. has been 1?
volatile bool btn = false; // Button is pressed?
-volatile bool controlFlag = false, btnFlag = false, breakFlag = false, emgFlag = false; // Go flags
+volatile bool controlFlag = false, btnFlag = false, breakFlag = false, emgFlag = false, lcdFlag = false; // Go flags
//----------------------------Functions-----------------------------------------------------------------------
void flipLed(DigitalOut& led){ //function to flip one LED
led.write(on);
@@ -54,12 +54,13 @@
lcd.cls(); // clear LCD scherm
lcd.printf(text); // print text op lcd
}
-void EMGkalibratieL(){ // Determine thresholds left
- LedB = 1;
- Init();
- double ymin = KalibratieMin(emgL, true);
+void EMGkalibratieL(){ // Determine thresholds left
+ PRINT("EMG LEFT relax muscle");
+ double ymin = KalibratieMin(emgL, true); // Minimum value left EMG, boolean indicates left
wait(1);
- double ymax = KalibratieMax(emgL, true);
+ PRINT("EMG LEFT flex muscle"); // LCD
+ double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left
+ PRINT("EMG LEFT well done!"); // LCD
if((ymax-ymin) < 0.05){ // No EMG connected
ymin = 10;
@@ -72,11 +73,12 @@
pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
}
void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
- LedB = 1;
- Init();
+ PRINT("EMG RIGHT relax muscle");
double ymin = KalibratieMin(emgR, false);
wait(1);
+ PRINT("EMG RIGHT flex muscle");
double ymax = KalibratieMax(emgR, false);
+ PRINT("EMG LEFT well done!");
if((ymax-ymin) < 0.05){
ymin = 10;
@@ -128,7 +130,10 @@
}
void setEmgFlag(){ // Goflag EMG
emgFlag = true;
- }
+ }
+void setLcdFlag(){ // Goflag EMG
+ lcdFlag = true;
+ }
void btnSetAction(){ // Set knop K64F
btn = true; // GoFlag setknop
}
@@ -140,7 +145,7 @@
}
void checkAim(){ // check if Killswitch is on or max counts is reached
double volt = KS.read();
- if(volt> 0.5 || abs(enc2.getPulses()) > maxCounts*/){
+ if(volt> 0.5 || abs(enc2.getPulses()) > (maxCounts -50) || enc2.getPulses() < 0){
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK! \r\n"); // Terminal
PRINT("BOEM! CRASH!"); // LCD
@@ -175,7 +180,7 @@
if(aimState!=OFF){ // only if aim motor is rotating
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // motor state is off
- PRINT("Motor freeze\r\n"); // LCD
+ pc.printf("Motor freeze\r\n"); // LCD
L = false; // Modes must be first 1 for another action
R = false; // ""
}
@@ -183,14 +188,14 @@
else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
if(L){
aimState = CCW; // Rotate CCW
- pc.printf("Rotate -, CCW");
+ pc.printf("Rotate -, CCW\r\n");
motorAim(0);
}
}
else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
if(R){
aimState = CW; // Rotate CW
- PRINT("Rotate +, CW");
+ pc.printf("Rotate +, CW\r\n");
motorAim(1);
}
}
@@ -203,98 +208,107 @@
tickerControl.attach(&setControlFlag,tControl); // ticker control motor
tickerBreak.attach(&setBreakFlag,tBreak); // ticker break
EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
+ tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd
pc.printf("\n\n\n\n\n"); // Terminal
- pc.prinft("---NEW GAME---\r\n)
+ pc.printf("---NEW GAME---\r\n");
PRINT("--- NEW GAME ---"); // LCD
while(1){ // Run program
switch(state){
- case KALIBRATE_EMG: {
- pc.printf("Kalibrate EMG left in 5 seconds\r\n"); // Terminal
- PRINT("EMG LEFT MAX MIN"); // LCD
- EMGkalibratieL(); // Kalibrate left EMG, determine thresholds
-
- pc.printf("Kalibrate EMG right in 5 seconds\r\n"); // Terminal
- PRINT("EMG RIGHT MAX MIN"); // LCD
- EMGkalibratieR(); // Kalibrate right EMG, determine thresholds
-
- state = KALIBRATE_AIM; // Next state
+ case CALIBRATE_EMG: {
+ EMGkalibratieL(); // calibrate left EMG, determine thresholds
+ EMGkalibratieR(); // calibrate right EMG, determine thresholds
+ state = CALIBRATE_AIM; // Next state
break;
}
- case KALIBRATE_AIM: {
+ case CALIBRATE_AIM: {
pwmM2.period(1/100000); // period motor 2
printf("Position is kalibrating\r\n"); // terminal
PRINT("Wait a moment, please"); // LCD
dirM2.write(0); // direction aim motor
pwmM2.write(0.25); // percentage motor power
- bool kalibrated = false;
- while(state==KALIBRATE_AIM){
+ bool calibrated = false; //
+ while(state==CALIBRATE_AIM){
pc.printf("Killswitch: %f\r\n", KS.read()); // terminal
- if(KS.read() > 0.5){ // Killswitch?
+ if(KS.read() > 0.5 && !calibrated){ // Killswitch?
pwmM2.write(0); // Aim motor freeze
enc2.reset(); // Reset encoder
- PRINT("Aim kalibrated");
- //state = KALIBRATE_PEND; // next state
- kalibrated = true;
+ PRINT("Aim calibrated"); // LCD
+ calibrated = true; //
dirM2.write(0); // direction aim motor
pwmM2.write(0.25); // percentage motor power, turn it on
- }
-
- if(controlFlag && kalibrated){ // motor regelen op GoFlag
+ }
+ if(controlFlag && calibrated){ // motor regelen op GoFlag
controlFlag = false;
- if(enc.getPulses() > maxCounts/2){ // rotate aim motor to midi position
+ if(enc1.getPulses() > maxCounts/2){ // rotate aim motor to midi position
pwmM2.write(0); // Aim motor freeze
- state = KALIBRATE_PEND; // next state
+ state = CALIBRATE_PEND; // next state
}
}
}
break;
}
- case KALIBRATE_PEND: {
- pc.printf("Kalibrate pendulum motor with setknop\r\n");
- pwmM1.period(1/100000); // periode pendulum motor
+ case CALIBRATE_PEND: {
+ pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
+ pwmM1.period(1/100000); // period beam motor
servo.period(0.02); // 20ms period servo
- pwmM1.write(0.25); // Turn motor on, low power
- btn = false;
- R = false;
- L = false;
- while(state==KALIBRATE_PEND){
+ //pwmM1.write(0.25); // Turn motor on, low power
+ btn = false; // Button is unpressed
+ R = false; // modeR must become 1
+ L = false; // modeL must become 1
+ PRINT("Calibrate beam");
+ wait(1);
+ PRINT("Flex right half to swing beam");
+ while(state==CALIBRATE_PEND){
if(emgFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
emgFlag = false;
- int modeL = defMode(emgL, potL, true);
- int modeR = defMode(emgR, potR, false);
+ int modeL = defMode(emgL, potL, true); // determine modeL
+ int modeR = defMode(emgR, potR, false); // determine modeR
if(modeR < 2) { // modeR == 1
R = true;
}
if(modeL < 2) { // modeL == 1
L = true;
- }
-
+ }
if (btn || (modeL == 3 && L) || (modeR == 3 && R)){ // If setbutton is on or one mode is 3, and has been 1
pwmM1.write(0); // Motor 1 freeze
enc1.reset(); // encoder 1 reset
- PRINT("Pendulum kalibrated\r\n");
+ PRINT("Beam calibrated");
btn = false; // button op false
state = AIM; // next state
}
else if(modeL == 2){
- pwmM1.write(0); // Pendulum freeze
- PRINT("Pendulum off\r\n");
+ pwmM1.write(0); // beam freeze
+ PRINT("Flex both full to continue"); // LCD
}
- else if(modeL == 3){
- pwmM1.write(0.025); // Pendulum rotate
- PRINT("Pendulum on\r\n");
+ else if(modeR == 2){
+ pwmM1.write(0.025); // beam rotate
+ PRINT("Flex left half to stop beam"); // LCD
}
}
}
break;
}
case AIM: {
- pc.printf("Aim with EMG\r\n");
- while(state == AIM){
+ pc.printf("Aim with EMG\r\n"); // terminal
+ int i = 0; // counter for lcd
+ while(state == AIM){
+ if(lcdFlag){
+ if(i%3 == 0){
+ PRINT("Flex left or right half to aim");
+ }
+ else if(i%3 == 1){
+ PRINT("Flex both half to freeze");
+ }
+ else {
+ PRINT("Flex both full to continue");
+ }
+ i++;
+ }
+
if(controlFlag){ // motor control on GoFlag
controlFlag = false;
checkAim(); // Check position
@@ -303,7 +317,7 @@
emgFlag = false;
if(controlAim()){
pc.printf("Position choosen, adjust shoot velocity\r\n");
- state = REM; // Next state (BREAK)
+ state = BREAK; // Next state (BREAK)
}
}
}
@@ -350,7 +364,8 @@
}
if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
pc.printf("Current breaking scale: %i\r\n", breakPerc);
- PRINT("Break scale is: %i", breakPerc);
+ lcd.cls();
+ lcd.printf("Break scale is: %i", breakPerc);
}
}
}
@@ -362,10 +377,10 @@
case FIRE: {
pc.printf("Shooting\r\n");
servo.pulsewidth(servoL); // BREAK release
- pwmM1.write(0.25); // Pendulum motor on
+ pwmM1.write(0.25); // beam motor on
bool servoPos = true;
while(state == FIRE){ // while in FIRE state
- if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){ // BREAK goFlag half rotation pendulum = breaking
+ if(breakFlag && (abs(enc1.getPulses())-100)%4200 < (2100*abs(breakPerc)/10)){ // BREAK goFlag half rotation beam = breaking
breakFlag = false;
if(servoPos){
servoPos = false;
@@ -378,9 +393,9 @@
}
if(controlFlag){
controlFlag = false;
- if((abs(enc1.getPulses())+50)%4200 < 150){ // rotate pendulum one revolution
- pwmM1.write(0); // motor pendulum off
- pc.printf("Pendulum on startposition\r\n");
+ if((abs(enc1.getPulses())+50)%4200 < 150){ // rotate beam one revolution
+ pwmM1.write(0); // motor beam off
+ pc.printf("beam on startposition\r\n");
state = AIM; // Next stage
}
}