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:
- 52:85ddaee35185
- Parent:
- 51:dcbfdf3b9468
- Child:
- 53:4a0857fbbb34
--- a/main.cpp Wed Oct 28 12:47:38 2015 +0000
+++ b/main.cpp Wed Oct 28 12:58:30 2015 +0000
@@ -31,12 +31,11 @@
enum aimFase {OFF, CW, CCW}; // Aim motor possible states
uint8_t aimState = OFF; // first state aim motor
//-------------------------------Variables---------------------------------------------------------------------
-int maxCounts = 0; // max richt motor counts Aim motor
+int maxCounts = 13000; // max richt motor counts Aim motor
int breakPerc = 0;
volatile int modeL = 1, modeR = 1;
-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 tControl = 0.05, 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;
@@ -45,6 +44,7 @@
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,7 +67,7 @@
thresholdhighL = 0.8 * ymax; // Highest threshold
pc.printf("ymaxL = %f en yminL = %f \r\n",ymax, ymin);
- wait(0.5);
+ wait(1);
}
void EMGkalibratieR(){ // Determine thresholds right EMG, same as left
PRINT("EMG RIGHT relax muscle");
@@ -86,7 +86,7 @@
thresholdhighR = 0.8 * ymax;
pc.printf("ymaxR = %f en yminR = %f \r\n",ymax, ymin); // terminal
- wait(0.5);
+ wait(1);
}
int EMGfilter(AnalogIn& emg, bool side){
double u = emg.read(); // read emg signal (left or right EMG)
@@ -104,7 +104,7 @@
int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
double volt = pot.read();
int mode = 1;
- if(volt > 0.8){
+ if(volt > 0.95){
mode = 3;
}
else if(volt>0.35 && volt<0.65){
@@ -144,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.95){
+ if(ks.read()>0.8){
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
PRINT("BOEM! CRASH!"); // LCD
@@ -157,7 +157,7 @@
}
void motorAim(int dir){ // Rotate Aim motor with given direction
dirM2.write(dir);
- pwmM2.write(wM2);
+ pwmM2.write(0.2);
}
bool controlAim(){ // Function to control aim motor with modes
bool both = false; // boolean if both modes are 3
@@ -230,40 +230,30 @@
pc.printf("Position is kalibrating\r\n"); // terminal
PRINT("Wait a moment, please"); // LCD
dirM2.write(0); // direction aim motor
- pwmM2.write(wM2); // percentage motor power
- volatile bool calibratedR = false;
- volatile bool calibratedL = false;
+ pwmM2.write(0.2); // percentage motor power
+ bool calibrated = false;
while(state==CALIBRATE_AIM){
if(controlFlag){
controlFlag = false;
- //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?
+ if(!calibrated){ // Not calibrated
+ if((ksRight.read()>0.8)){ // Killswitch?
pwmM2.write(0); // Aim motor freeze
- enc2.reset(); // Reset encoder
- calibratedR = true;
+ 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");
dirM2.write(1); // direction aim motor
- 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
+ pwmM2.write(0.2); // percentage motor power, turn it on
}
- }
- else { // Botch sides calibrated
+ }
+ if(calibrated) { // Calibrated
//checkAim(); // Check killswitches
- if(abs(enc2.getPulses()) < (maxCounts/2) && (pwmM2.read()>0)){ // rotate aim motor to midi position
+ if(abs(enc2.getPulses()) > 700){ // 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();
@@ -388,13 +378,13 @@
case FIRE: {
pc.printf("Shooting\r\n"); // terminal
PRINT("FIRING"); // lcd
- pwmM1.write(wM1); // beam motor on
+ pwmM1.write(0.3); // 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/3; // Amount of counts where must be breaked
+ countBreak = 2100*breakPerc/4; // Amount of counts where must be breaked
}
while(state == FIRE){ // while in FIRE state
if(controlFlag){ // BREAK goFlag half rotation beam = breaking