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
Revision 45:520d86583ff6, committed 2015-10-22
- Comitter:
- RemcoDas
- Date:
- Thu Oct 22 19:59:08 2015 +0000
- Parent:
- 44:97f5622db2c4
- Child:
- 46:ed8ada80ba17
- Commit message:
- Aim aangepast, EMG vasthouden versimpeld
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Oct 22 12:14:24 2015 +0000
+++ b/main.cpp Thu Oct 22 19:59:08 2015 +0000
@@ -136,7 +136,7 @@
}
void checkSide(AnalogIn& ks, int dir){
- if((dirM2.read() == dir) && (pwmM2.read()>0)){
+ if((dirM2.read() == dir) && (pwmM2.read()>0)){ // direction motor clashes with killswitch and motor is on
if(ks.read()>0.8){
pwmM2.write(0); // Aim motor freeze
pc.printf("BOEM! CRASH! KASTUK!\r\n"); // Terminal
@@ -145,7 +145,7 @@
}
}
-void checkAim(){
+void checkAim(){ // check if killswitch clashes with direction motor
checkSide(ksLeft, 1);
checkSide(ksRight, 0);
}
@@ -169,37 +169,39 @@
L = true;
}
- if((modeL>2) && (modeR >2 && R && L)) { // 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
- if(aimState!=OFF){ // only if aim motor is rotating
- 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; // ""
- }
+ both = true; // Return true
+ pwmM2.write(0); // Aim motor freeze
+ aimState = OFF; // next state
}
- else if((modeR == 2) && (aimState != CCW && (modeL == 1))) { // modeL ==2 AND rotation is not CCW AND modeR has been 1
- if(R){
+ else if((modeR == 2)) { // modeR ==2
+ if(aimState != CCW){
aimState = CCW; // Rotate CCW
pc.printf("Rotate -, CCW\r\n");
PRINT("Rotate CCW");
- motorAim(0);
+ motorAim(0);
}
}
- else if((modeL == 2) && (aimState != CW && (modeR == 1))) { // modeR == 2 AND rotation is not CW AND modeL has been 1
- if(L){
+ else if((modeL == 2)) { // modeL == 2
+ if(aimState != CW){
aimState = CW; // Rotate CW
pc.printf("Rotate +, CW\r\n");
PRINT("Rotate CW");
motorAim(1);
}
- }
+ }
+ // modeR AND CCW OR modeL and CW
+ else if((modeR == 1 && aimState == CCW) || (modeL == 1 && aimState == CW)) { // R=1 en CW
+ pwmM2.write(0); // Aim motor freeze
+ aimState = OFF;
+ pc.printf("Freeze\r\n");
+ PRINT("Freeze");
+ }
return both;
}
int main(){
@@ -258,51 +260,23 @@
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.1); // 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");
wait(1);
- PRINT("Flex right half to swing beam");
+ PRINT("Push button to continue");
while(state==CALIBRATE_BEAM){
- if(emgFlag){
+ if(controlFlag){
pc.printf(""); // lege regel printen, anders doet setknop het niet
- emgFlag = false;
-
- int modeL = defMode(emgL, potL, true); // determine modeL
- int modeR = defMode(emgR, potR, false); // determine modeR
-
- if(modeR < 2) { // modeR == 1
- R = true;
- }
- if(modeL < 2) { // modeL == 1
- L = true;
- }
- if (btn || (modeL == 3 && L) || (modeR == 3 && R)){ // If setbutton is on or one mode is 3, and has been 1
+ 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");
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("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
- pc.printf("Beam move\r\n");
- }
- }
+ }
}
}
lcd.cls();