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:
- 43:929cab5358ee
- Parent:
- 42:18cb904dab56
- Child:
- 44:97f5622db2c4
--- a/main.cpp Thu Oct 22 08:14:20 2015 +0000
+++ b/main.cpp Thu Oct 22 08:52:33 2015 +0000
@@ -17,7 +17,7 @@
AnalogIn potL(A2), potR(A3); // Potential meter left and right
DigitalIn ksLeft(PTE24), ksRight(PTE25); // Killswitch left and right
HIDScope scope(2); // Hidscope, amount of scopes
-Ticker EMGticker, tickerControl, tickerBreak, tickerLcd; // Ticker for EMG, regulator and break
+Ticker EMGticker, tickerControl, 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
@@ -31,25 +31,19 @@
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 = 13000; // max richt motor counts Aim motor
+int maxCounts = 13000; // max richt motor counts Aim motor
int breakPerc = 0;
const double servoBreak = 0.0017, 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 = 200; //Sample frequency Hz
-double t = 1/ Fs; // time EMGticker
+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;
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, breakFlag = false, emgFlag = false, lcdFlag = false; // Go flags
+volatile bool controlFlag = false, btnFlag = false, emgFlag = false, lcdFlag = false; // Go flags
//----------------------------Functions-----------------------------------------------------------------------
-void flipLed(DigitalOut& led){ //function to flip one LED
- led.write(on);
- wait(0.2);
- led.write(off);
- }
void PRINT(const char* text){
lcd.cls(); // clear LCD scherm
lcd.printf(text); // print text op lcd
@@ -188,26 +182,24 @@
}
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
+ aimState = CCW; // 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
+ aimState = CW; // Rotate CW
pc.printf("Rotate +, CW\r\n");
motorAim(1);
}
}
return both;
}
-int main(){
- flipLed(ledR); // test if code begins
+int main(){
btnSet.mode(PullUp); // Button mode
btnSet.rise(&btnSetAction); // Connect button to function
- tickerControl.attach(&setControlFlag,tControl); // ticker control motor
- tickerBreak.attach(&setBreakFlag,tBreak); // ticker break
+ tickerControl.attach(&setControlFlag,tControl); // ticker control motor
EMGticker.attach(&setEmgFlag, t); // ticker EMG, 500H
tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd
@@ -230,28 +222,25 @@
pwmM2.write(0.15); // percentage motor power
bool calibrated = false; //
while(state==CALIBRATE_AIM){
- if(controlFlag){ // motor regelen op GoFlag
+ if(controlFlag){
controlFlag = false;
if(!calibrated){ // Not calibrated
- if((ksRight.read())){ // Killswitch?
+ if((ksRight.read())){ // 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
+ calibrated = true;
wait(1);
dirM2.write(1); // direction aim motor
pwmM2.write(0.15); // percentage motor power, turn it on
}
}
- else { // Calibrated
- controlFlag = false;
- checkAim();
- pc.printf("aim counts: %i\r\n", abs(enc2.getPulses()));
- if(abs(enc2.getPulses()) > 600){ // rotate aim motor to midi position
+ else { // Calibrated
+ checkAim(); // Check killswitches
+ if(abs(enc2.getPulses()) > 600){ // rotate aim motor to midi position
pwmM2.write(0); // Aim motor freeze
- state = CALIBRATE_PEND; // next state
+ state = CALIBRATE_PEND; // next state
}
}
}
@@ -263,11 +252,11 @@
dirM1.write(0); // direction beam motor
pwmM1.period(1/100000); // period beam motor
servo.period(0.02); // 20ms period servo
- //pwmM1.write(0.2); // 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
- PRINT("Calibrate beam to 10 o'clock");
+ PRINT("Calibrate beam to 10 o'clock");
wait(1);
PRINT("Flex right half to swing beam");
while(state==CALIBRATE_PEND){
@@ -285,24 +274,26 @@
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
+ pwmM1.write(0); // Motor 1 freeze
+ enc1.reset(); // encoder 1 reset
PRINT("Beam calibrated");
pc.printf("Beam calibrated\r\n");
- btn = false; // button op false
- state = AIM; // next state
+ btn = false; // button op false
+ state = AIM; // next state
}
else if(modeL == 2){
- if(pwmM1.read()> 0){ // beam is moving
- pwmM1.write(0); // beam freeze
- PRINT("Flex both full to continue"); // LCD
+ if(pwmM1.read()> 0){ // beam is moving
+ pwmM1.write(0); // beam freeze
+ PRINT("Relax"); // LCD
+ wait(1);
+ PRINT("Flex both full to continue"); // LCD
pc.printf("Beam freeze\r\n");
}
}
else if(modeR == 2){
- if(pwmM1.read()== 0){ // beam is freezed
- //pwmM1.write(0.2); // beam rotate
- PRINT("Flex left half to stop beam"); // LCD
+ if(pwmM1.read()== 0){ // beam is freezed
+ //pwmM1.write(0.2); // beam rotate
+ PRINT("Flex left half to stop beam"); // LCD
pc.printf("Beam move\r\n");
}
}
@@ -366,38 +357,39 @@
emgFlag = false;
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
-
- if(modeR < 2) { // modeR is 1
- R = true;
- }
- if(modeL < 2) { // modeL is 1
- L = true;
- }
-
- if((modeL > 1) && modeR > 1) { // both modes > 1
+
+ if((modeL > 1) && modeR > 1) { // both modes > 1
state = FIRE;
}
- else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
+ else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
state = FIRE;
}
- if(L && R){
- if((modeL == 2) && L) { // modeL = BREAK lower with one
- if(breakPerc>1){
- breakPerc--;
- }
- L = false;
- }
- else if((modeR == 2) && R) { // modeR = Break +1
- if(breakPerc<10){
- breakPerc++;
- }
- R = false;
- }
- if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
- pc.printf("Current breaking scale: %i\r\n", breakPerc);
- lcd.cls();
- lcd.printf("Break scale: %i", breakPerc);
- }
+ else {
+ if(modeR < 2) { // modeR is 1
+ R = true;
+ }
+ if(modeL < 2) { // modeL is 1
+ L = true;
+ }
+ if(L && R){ // L and R has been 1
+ if((modeL == 2) && L) { // modeL = BREAK lower with one
+ if(breakPerc>1){
+ breakPerc--;
+ }
+ L = false;
+ }
+ else if((modeR == 2) && R) { // modeR = Break +1
+ if(breakPerc<10){
+ breakPerc++;
+ }
+ R = false;
+ }
+ if(modeL > 1 || modeR > 1){ // Print BREAK scale if one of both modes > 1
+ pc.printf("Current breaking scale: %i\r\n", breakPerc);
+ lcd.cls();
+ lcd.printf("Break scale: %i", breakPerc);
+ }
+ }
}
}
}
@@ -407,24 +399,24 @@
break;
}
case FIRE: {
- pc.printf("Shooting\r\n");
- PRINT("FIRING");
- pwmM1.write(0.25); // beam motor on
+ pc.printf("Shooting\r\n"); // terminal
+ PRINT("FIRING"); // lcd
+ pwmM1.write(0.2); // beam motor on
servo.pulsewidth(servoBreak); // Servo to break position
bool breaking = true; // boolean breaking?
int countBreak = 2100*abs(breakPerc)/10; // Amount of counts where must be breaked
while(state == FIRE){ // while in FIRE state
- if(controlFlag){ // BREAK goFlag half rotation beam = breaking
+ if(controlFlag){ // BREAK goFlag half rotation beam = breaking
controlFlag = false; // GoFlag
- int counts = abs(enc1.getPulses())+100;
- if(breaking){ // encoder is 0 on 100 counts before 12 o'clock
+ int counts = abs(enc1.getPulses())+100; // counts encoder beam, +100 is to correct 10 to 12 o'clock // encoder is 0 on 100 counts before 12 o'clock
+ if(breaking){
if((counts+countBreak)%4200 < 150){
- servo.pulsewidth(servoFree); // Servo to free position
+ servo.pulsewidth(servoFree); // Servo to free position
breaking = false;
}
}
- else{ // Not breaking
- if(counts%4200 < 400){ // rotated 1 rev?
+ if(!breaking){ // Not breaking
+ if(counts%4200 < 400){ // rotated 1 rev?
pwmM1.write(0); // motor beam off
pc.printf("Beam on startposition\r\n"); // terminal
state = AIM; // Next stage