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:
- 51:dcbfdf3b9468
- Parent:
- 50:16314b798754
- Child:
- 52:85ddaee35185
--- a/main.cpp Wed Oct 28 09:28:48 2015 +0000
+++ b/main.cpp Wed Oct 28 12:47:38 2015 +0000
@@ -31,11 +31,12 @@
enum aimFase {OFF, CW, CCW}; // Aim motor possible states
uint8_t aimState = OFF; // first state aim motor
//-------------------------------Variables---------------------------------------------------------------------
-int maxCounts = 13000; // max richt motor counts Aim motor
+int maxCounts = 0; // max richt motor counts Aim motor
int breakPerc = 0;
volatile int modeL = 1, modeR = 1;
-const double servoBreak = 0.00165, servoFree = 0.002; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
-const double tControl = 0.05, tLcd = 2; // ticker time (sec)
+const double wM1 = 0.3, wM2 = 0.25; // Power motors
+const double servoBreak = 0.0016, servoFree = 0.002; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
+const double tControl = 0.1, tLcd = 2; // ticker time (sec)
const double Fs = 200; //Sample frequency Hz
double t = 1/ Fs; // time EMGticker
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
@@ -44,7 +45,6 @@
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, emgFlag = false, lcdFlag = false, runEmg = true; // Go flags
-bool boolBrake = false;
//----------------------------Functions-----------------------------------------------------------------------
void PRINT(const char* text){
lcd.cls(); // clear LCD scherm
@@ -58,7 +58,7 @@
double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left
PRINT("EMG LEFT well done!"); // LCD
- if((ymax-ymin) < 0.05 || !runEmg){ // No EMG connected or button pushed
+ if((ymax-ymin) < 0.05 || !runEmg){ // No EMG connected or button pushed
ymin = 10;
ymax = 10;
}
@@ -67,6 +67,7 @@
thresholdhighL = 0.8 * ymax; // Highest threshold
pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
+ wait(0.5);
}
void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
PRINT("EMG RIGHT relax muscle");
@@ -85,6 +86,7 @@
thresholdhighR = 0.8 * ymax;
pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal
+ wait(0.5);
}
int EMGfilter(AnalogIn& emg, bool side){
double u = emg.read(); // read emg signal (left or right EMG)
@@ -142,7 +144,7 @@
}
void checkSide(AnalogIn& ks, int dir){
if((dirM2.read() == dir) && (pwmM2.read()>0)){ // direction motor clashes with killswitch and motor is on
- if(ks.read()>0.8){
+ if(ks.read()>0.95){
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
PRINT("BOEM! CRASH!"); // LCD
@@ -155,7 +157,7 @@
}
void motorAim(int dir){ // Rotate Aim motor with given direction
dirM2.write(dir);
- pwmM2.write(0.2);
+ pwmM2.write(wM2);
}
bool controlAim(){ // Function to control aim motor with modes
bool both = false; // boolean if both modes are 3
@@ -225,33 +227,43 @@
}
case CALIBRATE_AIM: {
pwmM2.period(1/100000); // period motor 2
- printf("Position is kalibrating\r\n"); // terminal
+ 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;
+ pwmM2.write(wM2); // percentage motor power
+ volatile bool calibratedR = false;
+ volatile bool calibratedL = false;
while(state==CALIBRATE_AIM){
if(controlFlag){
controlFlag = false;
- if(!calibrated){ // Not calibrated
- if((ksRight.read()>0.8)){ // Killswitch?
+ //pc.printf("ksleft: %f, ksRight: %f, boolL: %i, boolR: %i, counts: %i\r\n", ksLeft.read(), ksRight.read(), calibratedL, calibratedR, abs(enc2.getPulses()));
+ if(calibratedR == false){ // Not calibrated
+ if((ksRight.read()>0.95)){ // Killswitch?
pwmM2.write(0); // Aim motor freeze
- enc2.reset(); // Reset encoder
- PRINT("Aim calibrated"); // LCD
- pc.printf("Aim calibrated\r\n");// Terminal
- calibrated = true;
- pc.printf("Go to midi position\r\n");
+ enc2.reset(); // Reset encoder
+ calibratedR = true;
dirM2.write(1); // direction aim motor
- pwmM2.write(0.2); // percentage motor power, turn it on
+ pwmM2.write(wM2); // percentage motor power, turn it on
+ }
+ }
+ else if(calibratedL == false){ // Right switch kalibrated
+ if((ksLeft.read()>0.95)){
+ pwmM2.write(0); // Aim motor freeze
+ maxCounts = abs(enc2.getPulses());
+ calibratedL = true;
+ dirM2.write(0); // direction aim motor
+ pwmM2.write(0.2); // percentage motor power, turn it on
}
- }
- if(calibrated) { // Calibrated
- checkAim(); // Check killswitches
- if(abs(enc2.getPulses()) > 600){ // rotate aim motor to midi position
+ }
+ else { // Botch sides calibrated
+ //checkAim(); // Check killswitches
+ if(abs(enc2.getPulses()) < (maxCounts/2) && (pwmM2.read()>0)){ // rotate aim motor to midi position
pwmM2.write(0); // Aim motor freeze
state = CALIBRATE_BEAM; // next state
+ PRINT("Aim calibrated"); // LCD
+ pc.printf("Aim calibrated\r\n");// Terminal
}
- }
+ }
}
}
lcd.cls();
@@ -300,7 +312,7 @@
if(lcdFlag){ // LCD loop to project 3 texts on lcd
lcdFlag = false;
if(i%3 == 0){
- PRINT("Flex left or right half to aim");
+ PRINT("Flex L or R half to aim");
}
else if(i%3 == 1){
PRINT("Flex both half to freeze");
@@ -376,13 +388,13 @@
case FIRE: {
pc.printf("Shooting\r\n"); // terminal
PRINT("FIRING"); // lcd
- pwmM1.write(0.3); // beam motor on
+ pwmM1.write(wM1); // beam motor on
bool breaking = false;
int countBreak = 0;
if(breakPerc > 0){
servo.pulsewidth(servoBreak); // Servo to break position
breaking = true; // boolean breaking?
- countBreak = 2100*breakPerc/4; // Amount of counts where must be breaked
+ countBreak = 2100*breakPerc/3; // Amount of counts where must be breaked
}
while(state == FIRE){ // while in FIRE state
if(controlFlag){ // BREAK goFlag half rotation beam = breaking