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:
- 46:ed8ada80ba17
- Parent:
- 45:520d86583ff6
- Child:
- 47:959ef2792024
diff -r 520d86583ff6 -r ed8ada80ba17 main.cpp
--- a/main.cpp Thu Oct 22 19:59:08 2015 +0000
+++ b/main.cpp Fri Oct 23 10:23:42 2015 +0000
@@ -9,6 +9,7 @@
#include "Mode.h"
//--------------------Classes------------------------
InterruptIn btnSet(PTC6); // Kalibration button
+InterruptIn emgSet(PTA4); // EMG on/off button
//DigitalOut ledR(LED_RED), LedB(LED3); // Leds on K64F
MODSERIAL pc(USBTX, USBRX); // Modserial voor Putty
TextLCD lcd(PTC5, PTC7, PTC0, PTC9, PTC8, PTC1); // LCD screen on inner row of K64F, rs, e, d4-d7
@@ -33,7 +34,7 @@
//-------------------------------Variables---------------------------------------------------------------------
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 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 Fs = 200; //Sample frequency Hz
double t = 1/ Fs; // time EMGticker
@@ -42,7 +43,9 @@
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; // Go flags
+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
@@ -56,7 +59,7 @@
double ymax = KalibratieMax(emgL, true); // Maxium value left EMG, boolean indicates left
PRINT("EMG LEFT well done!"); // LCD
- if((ymax-ymin) < 0.05){ // No EMG connected
+ if((ymax-ymin) < 0.05 || !runEmg){ // No EMG connected
ymin = 10;
ymax = 10;
}
@@ -74,7 +77,7 @@
double ymax = KalibratieMax(emgR, false);
PRINT("EMG LEFT well done!");
- if((ymax-ymin) < 0.05){
+ if((ymax-ymin) < 0.05|| !runEmg){
ymin = 10;
ymax = 10;
}
@@ -130,11 +133,14 @@
}
void btnSetAction(){ // Set knop K64F
btn = true; // GoFlag setknop
+ }
+void emgOnOff(){ // Set knop K64F, push
+ runEmg = !runEmg; // switch enable emg
+ pc.printf("EMG is enabled: &i\r\n", runEmg);
}
void setControlFlag(){ // Go flag setButton
controlFlag = true;
}
-
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){
@@ -143,8 +149,7 @@
PRINT("BOEM! CRASH!"); // LCD
}
}
- }
-
+ }
void checkAim(){ // check if killswitch clashes with direction motor
checkSide(ksLeft, 1);
checkSide(ksRight, 0);
@@ -169,16 +174,16 @@
L = true;
}
- if((modeL>2 && L) || (modeR >2 && R)) { // mode L and mode R both 3, and both has been 1 herefore
+ if((modeL>2 && L) && (modeR >2 && R)) { // mode L and mode R both 3, and both has been 1 herefore
both = true; // Return true
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // next state
}
- else if((modeR == 2) && (modeL == 2)) { // modes are both 2
+ /*else if((modeR == 2 && R) && (modeL == 2 && L)) { // modes are both 2
both = true; // Return true
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // next state
- }
+ }*/
else if((modeR == 2)) { // modeR ==2
if(aimState != CCW){
aimState = CCW; // Rotate CCW
@@ -206,7 +211,9 @@
}
int main(){
btnSet.mode(PullUp); // Button mode
- btnSet.rise(&btnSetAction); // Connect button to function
+ btnSet.rise(&btnSetAction); // Connect button to function
+ 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
tickerLcd.attach(&setLcdFlag,tLcd); // ticker lcd
@@ -227,7 +234,7 @@
printf("Position is kalibrating\r\n"); // terminal
PRINT("Wait a moment, please"); // LCD
dirM2.write(0); // direction aim motor
- pwmM2.write(0.15); // percentage motor power
+ pwmM2.write(0.2); // percentage motor power
bool calibrated = false; //
while(state==CALIBRATE_AIM){
if(controlFlag){
@@ -260,17 +267,13 @@
dirM1.write(0); // direction beam motor
pwmM1.period(1/100000); // period beam motor
servo.period(0.02); // 20ms period servo
- pwmM1.write(0.1); // Turn motor on, low power
btn = false; // Button is unpressed
- PRINT("Calibrate beam to 10 o'clock");
- wait(1);
- PRINT("Push button to continue");
+ PRINT("Calibrate beam to 10 o'clock");
while(state==CALIBRATE_BEAM){
if(controlFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
controlFlag = false;
if(btn){ // If setbutton is on or one mode is 3, and has been 1
- pwmM1.write(0); // Motor 1 freeze
enc1.reset(); // encoder 1 reset
PRINT("Beam calibrated");
pc.printf("Beam calibrated\r\n");
@@ -287,7 +290,18 @@
int i = 0; // counter for lcd
R = false;
L = false;
- while(state == AIM){
+ while(state == AIM){
+ if(controlFlag){ // motor control on GoFlag
+ controlFlag = false;
+ checkAim(); // Check position
+ }
+ if(emgFlag){ // Go flag EMG sampel
+ emgFlag = false;
+ if(controlAim()){
+ pc.printf("Position chosen\r\n"); // terminal
+ state = BREAK; // Next state (BREAK)
+ }
+ }
if(lcdFlag){ // LCD loopje to project 3 texts on lcd
lcdFlag = false;
if(i%3 == 0){
@@ -300,18 +314,6 @@
PRINT("Flex both full to continue");
}
i++;
- }
-
- if(controlFlag){ // motor control on GoFlag
- controlFlag = false;
- checkAim(); // Check position
- }
- if(emgFlag){ // Go flag EMG sampel
- emgFlag = false;
- if(controlAim()){
- pc.printf("Position chosen\r\n"); // terminal
- state = BREAK; // Next state (BREAK)
- }
}
}
lcd.cls();
@@ -325,9 +327,7 @@
PRINT("L := -1, R := +1 L+R = continue");
wait(1);
while(state == BREAK){
- if(emgFlag){ // Go flag EMG sampelen
- lcd.cls();
- lcd.printf("Break scale: %i", breakPerc);
+ if(emgFlag){ // Go flag EMG sampelen
emgFlag = false;
int modeL = defMode(emgL, potL, true);
@@ -348,22 +348,27 @@
}
if(L && R){ // L and R has been 1
if((modeL == 2) && L) { // modeL = BREAK lower with one
- if(breakPerc>1){
- breakPerc--;
+ if(boolBrake){
+ boolBrake = false;
}
L = false;
}
else if((modeR == 2) && R) { // modeR = Break +1
- if(breakPerc<10){
- breakPerc++;
+ if(!boolBrake){
+ boolBrake = true;
}
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);
+ pc.printf("Current breaking scale: %i \r\n", boolBrake);
}
}
+ }
+ if(lcdFlag){ // every 2 seconds update lcd
+ lcdFlag = false;
+ lcd.cls();
+ lcd.printf("Break scale: %i", boolBrake);
}
}
}
@@ -377,21 +382,24 @@
case FIRE: {
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
+ pwmM1.write(0.25); // beam motor on
+ bool breaking = false;
+ if(boolBrake){
+ servo.pulsewidth(servoBreak); // Servo to break position
+ 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
controlFlag = false; // GoFlag
- int counts = abs(enc1.getPulses())+250; // 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 < 100){
+ 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(breaking && boolBrake){
+ if((counts+2100)%4200 < 100){
servo.pulsewidth(servoFree); // Servo to free position
breaking = false;
}
}
- if(!breaking){ // Not breaking
+ if(!breaking || !boolBrake){ // Not breaking
if(counts%4200 < 100){ // rotated 1 rev?
pwmM1.write(0); // motor beam off
pc.printf("Beam on startposition\r\n"); // terminal