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:
- 44:97f5622db2c4
- Parent:
- 43:929cab5358ee
- Child:
- 45:520d86583ff6
--- a/main.cpp Thu Oct 22 08:52:33 2015 +0000
+++ b/main.cpp Thu Oct 22 12:14:24 2015 +0000
@@ -9,13 +9,13 @@
#include "Mode.h"
//--------------------Classes------------------------
InterruptIn btnSet(PTC6); // Kalibration button
-DigitalOut ledR(LED_RED), LedB(LED3); // Leds on K64F
+//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
PwmOut servo(D3); // PwmOut servo
AnalogIn emgL(A0), emgR(A1); // Analog input of EMG, left and right
AnalogIn potL(A2), potR(A3); // Potential meter left and right
-DigitalIn ksLeft(PTE24), ksRight(PTE25); // Killswitch left and right
+AnalogIn ksLeft(A5), ksRight(A4); // Killswitch left and right
HIDScope scope(2); // Hidscope, amount of scopes
Ticker EMGticker, tickerControl, tickerLcd; // Ticker for EMG, regulator and break
// QEI Encoder(port 1, port 2, ,counts/rev
@@ -26,7 +26,7 @@
// Motor2 met PWM power control and direction
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
-enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_PEND, AIM, BREAK, FIRE}; // Program states, ACKNOWLEDGEMENT switch: BMT groep 4 2014
+enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_BEAM, AIM, BREAK, FIRE}; // Program states, ACKNOWLEDGEMENT switch: BMT groep 4 2014
uint8_t state = CALIBRATE_EMG; // first state program
enum aimFase {OFF, CW, CCW}; // Aim motor possible states
uint8_t aimState = OFF; // first state aim motor
@@ -134,21 +134,24 @@
void setControlFlag(){ // Go flag setButton
controlFlag = true;
}
-void setBreakFlag(){ // Go flag Break
- breakFlag = true;
- }
-void checkAim(){ // check if Killswitch is on or max counts is reached
- if((ksRight.read()) || (ksLeft.read())){
- if(pwmM1.read()>0){ // Aim motor is running
+
+void checkSide(AnalogIn& ks, int dir){
+ if((dirM2.read() == dir) && (pwmM2.read()>0)){
+ if(ks.read()>0.8){
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
PRINT("BOEM! CRASH!"); // LCD
}
- }
+ }
+ }
+
+void checkAim(){
+ checkSide(ksLeft, 1);
+ checkSide(ksRight, 0);
}
void motorAim(int dir){ // Rotate Aim motor with given direction
dirM2.write(dir);
- pwmM2.write(0.15);
+ pwmM2.write(0.2);
}
bool controlAim(){ // Function to control aim motor with modes
bool both = false; // boolean if both modes are 3
@@ -176,21 +179,24 @@
pwmM2.write(0); // Aim motor freeze
aimState = OFF; // motor state is off
pc.printf("Motor freeze\r\n"); // LCD
+ PRINT("Freeze");
L = false; // Modes must be first 1 for another action
R = false; // ""
}
}
- else if((modeL == 2) && (aimState != CCW && (modeR == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
- if(L){
+ else if((modeR == 2) && (aimState != CCW && (modeL == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
+ if(R){
aimState = CCW; // Rotate CCW
pc.printf("Rotate -, CCW\r\n");
+ PRINT("Rotate CCW");
motorAim(0);
}
}
- else if((modeR == 2) && (aimState != CW && (modeL == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
- if(R){
+ else if((modeL == 2) && (aimState != CW && (modeR == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
+ if(L){
aimState = CW; // Rotate CW
pc.printf("Rotate +, CW\r\n");
+ PRINT("Rotate CW");
motorAim(1);
}
}
@@ -225,29 +231,29 @@
if(controlFlag){
controlFlag = false;
if(!calibrated){ // Not calibrated
- if((ksRight.read())){ // Killswitch?
+ if((ksRight.read()>0.8)){ // Killswitch?
pwmM2.write(0); // Aim motor freeze
enc2.reset(); // Reset encoder
PRINT("Aim calibrated"); // LCD
pc.printf("Aim calibrated\r\n");// Terminal
- calibrated = true;
- wait(1);
+ calibrated = true;
+ pc.printf("Go to midi position\r\n");
dirM2.write(1); // direction aim motor
- pwmM2.write(0.15); // percentage motor power, turn it on
+ pwmM2.write(0.2); // percentage motor power, turn it on
}
}
- else { // Calibrated
+ if(calibrated) { // 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_BEAM; // next state
}
}
}
}
break;
}
- case CALIBRATE_PEND: {
+ case CALIBRATE_BEAM: {
pc.printf("Calibrate beam motor with setknop\r\n"); // Terminal
dirM1.write(0); // direction beam motor
pwmM1.period(1/100000); // period beam motor
@@ -259,7 +265,7 @@
PRINT("Calibrate beam to 10 o'clock");
wait(1);
PRINT("Flex right half to swing beam");
- while(state==CALIBRATE_PEND){
+ while(state==CALIBRATE_BEAM){
if(emgFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
emgFlag = false;
@@ -311,13 +317,13 @@
if(lcdFlag){ // LCD loopje to project 3 texts on lcd
lcdFlag = false;
if(i%3 == 0){
- PRINT("Flex left or right half to aim");
+ PRINT("Flex left or right half to aim");
}
else if(i%3 == 1){
- PRINT("Flex both half to freeze");
+ PRINT("Flex both half to freeze");
}
else {
- PRINT("Flex both full to continue");
+ PRINT("Flex both full to continue");
}
i++;
}
@@ -338,30 +344,25 @@
break;
}
case BREAK: {
- pc.printf("Set break percentage, current is: %.0f\r\n", breakPerc);
+ pc.printf("Set break percentage, current is: %i\r\n", breakPerc);
L = false;
R = false;
- int i = 0; // counter for lcd
- while(state == BREAK){
- if(lcdFlag){ // LCD loop to project text on LCD
- lcdFlag = false;
- if(i%2 == 0){
- PRINT("Flex L or R to adjust break");
- }
- else {
- PRINT("Flex both to continue");
- }
- i++;
- }
+ int i = 0; // counter for lcd
+ 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);
+
emgFlag = false;
int modeL = defMode(emgL, potL, true);
int modeR = defMode(emgR, potR, false);
- if((modeL > 1) && modeR > 1) { // both modes > 1
- state = FIRE;
+ if((modeL > 1) && (modeR > 1) && L && R) { // both modes > 1
+ state = FIRE;
}
- else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
+ else if((modeL > 2 && L)|| (modeR > 2 && R)) { // left of right mode 3 = fire
state = FIRE;
}
else {
@@ -386,13 +387,14 @@
}
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);
+
}
}
}
}
}
+ PRINT("Break fixed"); // lcd
+ pc.printf("Break fixed\r\n"); // terminal
L = false; // modeL must be one for another action can take place
R = false; // modeR ""
lcd.cls();
@@ -408,15 +410,15 @@
while(state == FIRE){ // while in FIRE state
if(controlFlag){ // BREAK goFlag half rotation beam = breaking
controlFlag = false; // GoFlag
- 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
+ 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 < 150){
+ if((counts+countBreak)%4200 < 100){
servo.pulsewidth(servoFree); // Servo to free position
breaking = false;
}
}
if(!breaking){ // Not breaking
- if(counts%4200 < 400){ // rotated 1 rev?
+ if(counts%4200 < 100){ // rotated 1 rev?
pwmM1.write(0); // motor beam off
pc.printf("Beam on startposition\r\n"); // terminal
state = AIM; // Next stage