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:
- 39:f0f9b3432f43
- Parent:
- 38:08c8fd55a3bb
- Child:
- 40:35c7c60d7262
diff -r 08c8fd55a3bb -r f0f9b3432f43 main.cpp
--- a/main.cpp Wed Oct 21 10:17:10 2015 +0000
+++ b/main.cpp Wed Oct 21 13:23:30 2015 +0000
@@ -15,8 +15,8 @@
PwmOut servo(D3); // PwmOut servo
AnalogIn emgL(A0), emgR(A1); // Analog input of EMG, left and right
AnalogIn potL(A2), potR(A3); // Potential meter left and right
-AnalogIn KS(A5); // Killswitch
-HIDScope scope(6); // Hidscope, amount of scopes
+AnalogIn ksLeft(A4), ksRight(A5); // Killswitch left and right
+HIDScope scope(2); // Hidscope, amount of scopes
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);
@@ -34,9 +34,9 @@
const int on = 0, off = 1; // on off
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 servoL = 0.001, servoFree = 0.002; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
const double tControl = 0.1, tBreak = 0.1, tLcd = 2; // tickers
-const double Fs = 50; //Sample frequency Hz
+const double Fs = 200; //Sample frequency Hz
double t = 1/ Fs; // time EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
@@ -78,7 +78,7 @@
wait(1);
PRINT("EMG RIGHT flex muscle");
double ymax = KalibratieMax(emgR, false);
- PRINT("EMG LEFT well done!");
+ PRINT("EMG LEFT well done!");
if((ymax-ymin) < 0.05){
ymin = 10;
@@ -144,11 +144,13 @@
breakFlag = true;
}
void checkAim(){ // check if Killswitch is on or max counts is reached
- double volt = KS.read();
- if(volt> 0.5 /*|| abs(enc2.getPulses()) > (maxCounts -50) || enc2.getPulses() < 0*/){
- pwmM2.write(0); // Aim motor freeze
- pc.printf("BOEM! CRASH! KASTUK! %f\r\n", KS.read()); // Terminal
- PRINT("BOEM! CRASH!"); // LCD
+ double volt = ksRight.read();
+ if(ksRight.read() > 0.8 || ksLeft.read() > 0.8){
+ if(pwmM1.read()>0){
+ pwmM2.write(0); // Aim motor freeze
+ pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
+ PRINT("BOEM! CRASH!"); // LCD
+ }
}
}
void motorAim(int dir){ // Rotate Aim motor with given direction
@@ -226,39 +228,42 @@
printf("Position is kalibrating\r\n"); // terminal
PRINT("Wait a moment, please"); // LCD
dirM2.write(0); // direction aim motor
- pwmM2.write(0.1); // percentage motor power
+ pwmM2.write(0.15); // percentage motor power
bool calibrated = false; //
while(state==CALIBRATE_AIM){
if(controlFlag){ // motor regelen op GoFlag
controlFlag = false;
- if(KS.read() > 0.8){ // Killswitch? //2* gedaan, zodat breuk uit de if-statement is
- pwmM2.write(0); // Aim motor freeze
- enc2.reset(); // Reset encoder
- PRINT("Aim calibrated"); // LCD
- pc.printf("Aim calibrated\r\n");
- calibrated = true; //
- state = CALIBRATE_PEND; // next state
- wait(2);
- //dirM2.write(1); // direction aim motor
- //pwmM2.write(0.25); // percentage motor power, turn it on
- }
- pc.printf("Killswitch: %f\r\n", KS.read()); // terminal
- /*
- if(controlFlag && calibrated){ // motor regelen op GoFlag
- controlFlag = false;
- if(enc1.getPulses() > maxCounts/2){ // rotate aim motor to midi position
- pwmM2.write(0); // Aim motor freeze
- state = CALIBRATE_PEND; // next state
- } */
- }
+ if(!calibrated){ // Not calibrated
+ if((ksRight.read() > 0.8)){ // Killswitch?
+ pwmM2.write(0); // Aim motor freeze
+ enc2.reset(); // Reset encoder
+ PRINT("Aim calibrated"); // LCD
+ pc.printf("Aim calibrated\r\n");// Terminal
+ calibrated = true; //
+ //state = CALIBRATE_PEND; // next state
+ wait(1);
+ dirM2.write(1); // direction aim motor
+ pwmM2.write(0.15); // percentage motor power, turn it on
+ }
+ }
+ else { // Calibrated
+ controlFlag = false;
+ checkAim();
+ if(enc1.getPulses() > maxCounts/2){ // rotate aim motor to midi position
+ pwmM2.write(0); // Aim motor freeze
+ state = CALIBRATE_PEND; // next state
+ }
+ }
+ }
}
break;
}
case CALIBRATE_PEND: {
- pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
+ pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
+ dirM1.write(0); // direction beam motor
pwmM1.period(1/100000); // period beam motor
servo.period(0.02); // 20ms period servo
- //pwmM1.write(0.25); // Turn motor on, low power
+ //pwmM1.write(0.2); // Turn motor on, low power
btn = false; // Button is unpressed
R = false; // modeR must become 1
L = false; // modeL must become 1
@@ -304,7 +309,9 @@
}
case AIM: {
pc.printf("Aim with EMG\r\n"); // terminal
- int i = 0; // counter for lcd
+ int i = 0; // counter for lcd
+ R = false;
+ L = false;
while(state == AIM){
if(lcdFlag){ // LCD loopje to project 3 texts on lcd
lcdFlag = false;
@@ -362,17 +369,14 @@
if(modeL < 2) { // modeL is 1
L = true;
}
- if(L && R){
- if((modeL > 1) && modeR > 1) { // both modes
- state = FIRE;
- R = false;
- L = false;
+
+ if((modeL > 1) && modeR > 1) { // both modes > 1
+ state = FIRE;
}
else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
- state = FIRE;
- R = false;
- L = false;
- }
+ state = FIRE;
+ }
+ if(L && R){
else if((modeL == 2) && L) { // modeL = BREAK lower with one
if(breakPerc>1){
breakPerc--;