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:
- 56:1ac2487a9610
- Parent:
- 55:a435e46299ea
- Child:
- 57:eea0d7265b22
--- a/main.cpp Wed Oct 28 14:39:21 2015 +0000
+++ b/main.cpp Thu Oct 29 13:48:53 2015 +0000
@@ -31,20 +31,17 @@
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 breakPerc = 0;
-volatile int modeL = 1, modeR = 1;
-const double servoBreak = 0.0016, servoFree = 0.002; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
-const double tControl = 0.05, tLcd = 2; // ticker time (sec)
-const double Fs = 200; //Sample frequency Hz
-double t = 1/ Fs; // time EMGticker
+int maxCounts = 0; // max richt motor counts Aim motor
+int breakPerc = 0; // Break part {0, 1 , 2 ,3}/4 of half revolution
+volatile int modeL = 1, modeR = 1; // modes left and right
+const double servoBreak = 0.0017, servoFree = 0.0020; // Range servo,between servoL en servoR (= pulsewidth pwm servo)
+const double tControl = 0.01, tLcd = 2, tEmg = 0.005; // ticker time (sec)
+const double wM2 = 0.31;
double thresholdlowL= 0, thresholdmidL = 0, thresholdhighL= 0;
double thresholdlowR= 0, thresholdmidR = 0, thresholdhighR= 0;
-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, emgFlag = false, lcdFlag = false, runEmg = true; // Go flags
-bool boolBrake = false;
//----------------------------Functions-----------------------------------------------------------------------
void PRINT(const char* text){
lcd.cls(); // clear LCD scherm
@@ -58,7 +55,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;
}
@@ -101,7 +98,7 @@
}
return mode;
}
-int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
+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){
@@ -112,7 +109,7 @@
}
return mode;
}
-int defMode(AnalogIn& emg, AnalogIn& pot, bool side){ // Determine mode both from EMG and Potentialmeter, ONE of them must be ONE!
+int defMode(AnalogIn& emg, AnalogIn& pot, bool side){// Determine mode both from EMG and Potentialmeter, ONE of them must be ONE!
int emgMode = EMGfilter(emg, side);
int potMode = PotReader(pot);
int mode = 1;
@@ -132,10 +129,10 @@
void setLcdFlag(){ // Goflag EMG
lcdFlag = true;
}
-void btnSetAction(){ // Set knop K64F
+void btnSetAction(){ // Set knop K64F PTC4
btn = true; // GoFlag setknop
}
-void emgOnOff(){ // Set knop K64F, push
+void emgOnOff(){ // Set knop K64F, push PTA4
runEmg = !runEmg; // switch enable emg
pc.printf("EMG is enabled: %i\r\n", runEmg);
}
@@ -145,9 +142,10 @@
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){
- pwmM2.write(0); // Aim motor freeze
+ pwmM2.write(0); // Aim motor freeze
+ PRINT("BOEM! CRASH!"); // LCD
pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
- PRINT("BOEM! CRASH!"); // LCD
+ wait(1);
}
}
}
@@ -157,13 +155,13 @@
}
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
modeL = defMode(emgL, potL, true); // determine mode left
modeR = defMode(emgR, potR, false); // determine mode right
-
+
scope.set(0, modeL);
scope.set(1, modeR);
scope.send(); //send values to HIDScope
@@ -175,7 +173,7 @@
L = true;
}
- if((modeL>2 && L) && (modeR >2 && R)) { // mode L and mode R both 3, and both has been 1
+ if((modeL>1 && L) && (modeR >1 && R)) { // mode L and mode R both 3, and both has been 1
both = true; // Return true
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // next state
@@ -184,7 +182,7 @@
if(aimState != CCW){
aimState = CCW; // Rotate CCW
pc.printf("Rotate -, CCW\r\n");
- PRINT("Rotate CCW");
+ PRINT("Rotate CW");
motorAim(0);
}
}
@@ -192,7 +190,7 @@
if(aimState != CW){
aimState = CW; // Rotate CW
pc.printf("Rotate +, CW\r\n");
- PRINT("Rotate CW");
+ PRINT("Rotate CCW");
motorAim(1);
}
}
@@ -211,12 +209,17 @@
emgSet.mode(PullUp); // Button mode
emgSet.rise(&emgOnOff); // Connect button to function
tickerControl.attach(&setControlFlag,tControl); // ticker control motor
- EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
+ EMGticker.attach(&setEmgFlag, tEmg); // ticker EMG
tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd
pc.printf("\n\n\n\n\n"); // Terminal
pc.printf("---NEW GAME---\r\n");
PRINT("--- NEW GAME ---"); // LCD
+
+ if(potL.read() > 0.3 || potR.read() > 0.3){
+ pc.printf("Potentialmeter is on!!!\r\n");
+ }
+
while(1){ // Run program
switch(state){
case CALIBRATE_EMG: {
@@ -231,16 +234,14 @@
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
+ pwmM2.write(wM2); // percentage motor power
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
+ enc2.reset(); // Reset encoder
state = CALIBRATE_L;
}
}
@@ -248,20 +249,23 @@
break;
}
case CALIBRATE_L: {
+ dirM2.write(1); // direction aim motor, other direction
+ pwmM2.write(wM2); // percentage motor power, turn it on
while(state==CALIBRATE_L){
if(controlFlag){
controlFlag = false;
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
+ maxCounts = abs(enc2.getPulses());
state = CALIBRATE_MID;
+ }
}
}
break;
}
case CALIBRATE_MID: {
+ dirM2.write(0); // direction aim motor
+ pwmM2.write(wM2); // percentage motor power, turn it on
while(state==CALIBRATE_MID){
if(controlFlag){
controlFlag = false;
@@ -399,11 +403,16 @@
servo.pulsewidth(servoBreak); // Servo to break position
breaking = true; // boolean breaking?
countBreak = 2100*breakPerc/4; // Amount of counts where must be breaked
- }
+ }
+ bool rev = false;
while(state == FIRE){ // while in FIRE state
if(controlFlag){ // BREAK goFlag half rotation beam = breaking
controlFlag = false; // GoFlag
int counts = abs(enc1.getPulses())+250; // counts encoder beam, +250 is to correct 10 to 12 o'clock // encoder is 0 on 100 counts before 12 o'clock
+ if((counts%4200 > 2100) && !rev){
+ rev = true;
+ }
+
if(breaking){
if((counts+countBreak)%4200 < 100){ // calculate with remainder, half revolution
servo.pulsewidth(servoFree); // Servo to free position
@@ -411,7 +420,7 @@
}
}
if(!breaking){
- if((counts-100)%4200 < 100){ // rotated 1 rev?
+ if(((counts-100)%4200 < 100) && rev){ // rotated 1 rev?
pwmM1.write(0); // motor beam off
pc.printf("Beam on startposition\r\n"); // terminal
state = AIM; // Next stage