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:
- 41:91c8c39d7a33
- Parent:
- 40:35c7c60d7262
- Child:
- 42:18cb904dab56
--- a/main.cpp Wed Oct 21 14:17:22 2015 +0000
+++ b/main.cpp Thu Oct 22 08:07:27 2015 +0000
@@ -26,8 +26,8 @@
// Motor2 met PWM power control and direction
PwmOut pwmM2(D5);
DigitalOut dirM2(D4);
-enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_PEND, AIM, BREAK, FIRE}; // Proframstates, ACKNOWLEDGEMENT switch: BMT groep 4 2014
-uint8_t state = CALIBRATE_EMG; // first state program
+enum spelfase {CALIBRATE_EMG, CALIBRATE_AIM, CALIBRATE_PEND, 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
//-------------------------------Variables---------------------------------------------------------------------
@@ -76,9 +76,9 @@
PRINT("EMG RIGHT relax muscle");
double ymin = KalibratieMin(emgR, false);
wait(1);
- PRINT("EMG RIGHT flex muscle");
+ PRINT("EMG RIGHT flex muscle");
double ymax = KalibratieMax(emgR, false);
- PRINT("EMG LEFT well done!");
+ PRINT("EMG LEFT well done!");
if((ymax-ymin) < 0.05){
ymin = 10;
@@ -105,7 +105,7 @@
}
int PotReader(AnalogIn& pot){ // read potentialmeter and determine its mode (1 = default, 2, 3)
double volt = pot.read();
- int mode = 1;
+ int mode = 1;
if(volt > 0.8){
mode = 3;
}
@@ -143,10 +143,9 @@
void setBreakFlag(){ // Go flag Break
breakFlag = true;
}
-void checkAim(){ // check if Killswitch is on or max counts is reached
- double volt = ksRight.read();
- if(ksRight.read() > 0.8 || ksLeft.read() > 0.8){
- if(pwmM1.read()>0){
+void checkAim(){ // check if Killswitch is on or max counts is reached
+ if((ksRight.read() > 0.8) || (ksLeft.read() > 0.8)){
+ if(pwmM1.read()>0){ // Aim motor is running
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
PRINT("BOEM! CRASH!"); // LCD
@@ -155,12 +154,12 @@
}
void motorAim(int dir){ // Rotate Aim motor with given direction
dirM2.write(dir);
- pwmM2.write(0.1);
+ pwmM2.write(0.15);
}
bool controlAim(){ // Function to control aim motor with modes
bool both = false; // boolean if both modes are 3
- int modeL = defMode(emgL, potL, true);
- int modeR = defMode(emgR, potR, false);
+ int modeL = defMode(emgL, potL, true); // determine mode left
+ int modeR = defMode(emgR, potR, false); // determine mode right
scope.set(0, modeL);
scope.set(1, modeR);
@@ -248,8 +247,9 @@
}
else { // Calibrated
controlFlag = false;
- checkAim();
- if(enc1.getPulses() > maxCounts/2){ // rotate aim motor to midi position
+ checkAim();
+ pc.printf("aim counts: %i\r\n", abs(enc2.getPulses()));
+ if(abs(enc2.getPulses()) > 600){ // rotate aim motor to midi position
pwmM2.write(0); // Aim motor freeze
state = CALIBRATE_PEND; // next state
}
@@ -293,14 +293,18 @@
state = AIM; // next state
}
else if(modeL == 2){
- pwmM1.write(0); // beam freeze
- PRINT("Flex both full to continue"); // LCD
- pc.printf("Beam freeze\r\n");
+ if(pwmM1.read()> 0){ // beam is moving
+ pwmM1.write(0); // beam freeze
+ PRINT("Flex both full to continue"); // LCD
+ pc.printf("Beam freeze\r\n");
+ }
}
else if(modeR == 2){
- pwmM1.write(0.025); // beam rotate
- PRINT("Flex left half to stop beam"); // LCD
- pc.printf("Beam move\r\n");
+ 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");
+ }
}
}
}
@@ -370,14 +374,14 @@
L = true;
}
- if((modeL > 1) && modeR > 1) { // both modes > 1
- state = FIRE;
- }
- else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
- state = FIRE;
- }
+ if((modeL > 1) && modeR > 1) { // both modes > 1
+ state = FIRE;
+ }
+ else if(modeL > 2 || modeR > 2) { // left of right mode 3 = fire
+ state = FIRE;
+ }
if(L && R){
- else if((modeL == 2) && L) { // modeL = BREAK lower with one
+ if((modeL == 2) && L) { // modeL = BREAK lower with one
if(breakPerc>1){
breakPerc--;
}
@@ -404,26 +408,26 @@
}
case FIRE: {
pc.printf("Shooting\r\n");
- PRINT("FIRING");
- servo.pulsewidth(servoL); // BREAK release
+ PRINT("FIRING");
pwmM1.write(0.25); // beam motor on
servo.pulsewidth(servoBreak); // Servo to break position
- bool breaking = true; // boolean breaking?
+ 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
- controlFlag = false;
+ 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
- if((abs(enc1.getPulses())-100)%4200 > (2100*abs(breakPerc)/10)){
+ if((counts+countBreak)%4200 < 150){
servo.pulsewidth(servoFree); // Servo to free position
breaking = false;
}
}
- else{ // Not breaking
- if((abs(enc1.getPulses())+150)%4200 < 150){ // rotated 1 rev?
- pwmM1.write(0); // motor beam off
- pc.printf("Beam on startposition\r\n"); // terminal
- state = AIM; // Next stage
- }
+ else{ // 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
}
}
}