Aukie Hooglugt
richting is nog niet ingesteld, kan aan setpoint liggen
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Diff: PROJECT_main.cpp
- Revision:
- 4:697d5a806cc4
- Parent:
- 3:2b2710b8b02e
- Child:
- 5:e5ca53305b87
--- a/PROJECT_main.cpp Fri Oct 31 19:38:07 2014 +0000
+++ b/PROJECT_main.cpp Mon Nov 03 10:49:21 2014 +0000
@@ -10,7 +10,6 @@
#define TSAMP_EMG 0.002 //sample frequency emg
#define KALIBRATIONTIME 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
#define FACTOR 0.6 //factor*max_waarde = threshold emg
-
//Define objects
AnalogIn emg0(PTB1); //Analog input biceps
AnalogIn emg1(PTB2); //Analog input triceps
@@ -92,8 +91,8 @@
PwmOut pwm_motor2(PTA5);
float integral = 0;
-uint8_t batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
-uint8_t balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
+float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
float controlerror = 0;
float pwm = 0;
@@ -275,30 +274,35 @@
//calibration motor 2
pwm_motor2.write(0.6); //lage PWM
- motor2dir = 1;
+ motor2dir = 0;
wait(1); // anders wordt de while(1) meteen onderbroken
- while(motor2.getSpeed()*omrekenfactor2>0) {
- if(motor2.getSpeed()*omrekenfactor2 < 0.70) { //0.70 is nog aan te passen
+ while(1) {
+ if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
pwm_motor2.write(0);
- motor2.setPosition(0);
+ motor2.setPosition(0);
+ goto motor1cal;
}
wait(0.01);
}
+motor1cal:
//calibration motor 1
pwm_motor1.write(0.55); //lage PWM
- motor1dir = 0;
+ motor1dir = 1;
wait(1); // anders wordt de while(1) meteen onderbroken
- while(motor1.getSpeed()*omrekenfactor1<0) {
- if(motor1.getSpeed()*omrekenfactor1 > -0.20 ) { // 0.20 is nog aan te passen
+ while(1) {
+ if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
pwm_motor1.write(0);
- motor1.setPosition(0);
+ motor1.setPosition(0);
+ goto emgcal;
}
wait(0.01);
}
+emgcal:
blink.detach();
blink2.detach();
dir1 = dir2 = dir3 = 1;
- for1 = for2 = for3 = 1;
+ for1 = for2 = for3 = 1;
+ pc.printf("kalmoarm ");
wait (1);
for1 = for2 = for3 = 0;
@@ -312,6 +316,7 @@
}
blink.detach();
dir1 = dir2 = dir3 = 1;
+ pc.printf("kalbi ");
wait (1);
//triceps kalibratie
@@ -324,63 +329,14 @@
}
blink.detach();
for1 = for2 = for3 = 1;
+ pc.printf("kaltri ");
wait (1);
for1 = for2 = for3 = 0;
-
- while(1) { //Loop van directiekeuze, naar slag en naar nulpositie
+directionchoice:
log_timer.attach(looper, TSAMP_EMG);
- if(direction==0) {
- ledtimer.attach(directionchoice, 1);
- }
- while(direction==0) { //Loop keuze DIRECTION
- if(bi_result>FACTOR*bi_max) {
- switch(ledburning){
- case 1:
- direction = 1;
- break;
- case 2:
- direction = 2;
- break;
- case 3:
- direction = 3;
- break;
- }
- } else {
- wait(0.01);
- }
- }
-
- ledtimer.detach();
-
- if(force==0) {
- ledtimer.attach(forcechoice, 1);
- }
- while(force==0||balhit!=0) {//Loop keuze FORCE
- if(bi_result>FACTOR*bi_max) {
- switch(ledburning){
- case 1:
- force = 1;
- break;
- case 2:
- force = 2;
- break;
- case 3:
- force = 3;
- break;
- }
- } else {
- if(tri_result>FACTOR*tri_max) {
- balhit = 3;
- } else
- wait(0.01);
- }
- }
- }
-
- ledtimer.detach();
-
+ while(1) { //Loop keuze DIRECTION
for(int i=1; i<4; i++) {
if(i==1) { //red
dir1=1;
@@ -389,6 +345,7 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(bi_result>FACTOR*bi_max) {
direction = 1;
+ pc.printf("links ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
goto forcechoice; // goes to second while(1) for the deciding the force
} else {
@@ -403,6 +360,7 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(bi_result>FACTOR*bi_max) {
direction = 2;
+ pc.printf("mid ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
goto forcechoice;
} else {
@@ -417,6 +375,7 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(bi_result>FACTOR*bi_max) {
direction = 3;
+ pc.printf("rechts ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
goto forcechoice;
} else {
@@ -426,9 +385,8 @@
}
}
}
-
- while(force==0) { //Loop keuze FORCE
- //dit kan ook een while(force==0) {} zijn, maar dan moet er een ticker gemaakt worden die de lampjes aan en uit laat gaan
+forcechoice:
+ while(1) { //Loop keuze FORCE
for(int j=1; j<4; j++) {
if(j==1) { //red
for1=1;
@@ -437,10 +395,12 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
+ pc.printf("reset ");
goto directionchoice;
} else {
if(bi_result>FACTOR*bi_max) {
force = 1;
+ pc.printf("zwak ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
goto choicesmade;
} else {
@@ -456,10 +416,12 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
+ pc.printf("reset ");
goto directionchoice;
} else {
if(bi_result>FACTOR*bi_max) {
force = 2;
+ pc.printf("normaal ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
goto choicesmade;
} else {
@@ -475,10 +437,12 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
+ pc.printf("reset ");
goto directionchoice;
} else {
if(bi_result>FACTOR*bi_max) {
force = 3;
+ pc.printf("sterk ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
goto choicesmade;
} else {
@@ -492,9 +456,10 @@
choicesmade:
blink.attach(okay, 0.2);
- while(balhit==0) { //balhit is gewoon een variabele die gebruikt wordt om uit de while te komen, naam kan verwarring veroorzaken
- if(tri_result>FACTOR*tri_max) {
+ while(1) {
+ if(tri_result>FACTOR*tri_max) {
blink.detach();
+ pc.printf("reset ");
switch (direction) {
case 1:
dir1 = 1;
@@ -512,31 +477,30 @@
for2 = for3 = 0;
break;
}
- balhit = 2; //zodat het de motorcontrol overslaat
- force = 0;
- wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
+
+ wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
+ goto forcechoice;
} else {
if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
- balhit = 1;
+ blink.detach();
+ log_timer.detach();
+ goto motorcontrol;
} else {
wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje)
}
}
}
-
- log_timer.detach();
- blink.detach();
- wait(1);
-
- if(balhit!=2){
+
+motorcontrol:
+
/* Vanaf hier komt de aansturing van de motor */
switch (direction) {
case 1:
- setpoint2 = -2*0.197222205;
+ setpoint2 = 2*0.11;
break;
case 2:
- setpoint2 = -1*0.197222205;
+ setpoint2 = 0.11;
break;
case 3:
setpoint2 = 0;
@@ -545,17 +509,17 @@
switch (force) {
case 1:
- setpoint1 = 6.8;
+ setpoint1 = 8;
break;
case 2:
- setpoint1 = 7.4;
+ setpoint1 = 8;
break;
case 3:
setpoint1 = 8;
break;
}
- while(batjeset<200) { // loop voor het goed plaatsen van motor2 (batje hoek)
+ while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
while(!looptimerflag);
looptimerflag = false; //clear flag
@@ -572,27 +536,37 @@
motor2dir = 0;
}
- //controleert of batje positie heeft bepaald
- if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
- // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
- batjeset = 0;
+ //controleert of batje positie heeft bepaald
+ if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+ if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
} else {
- batjeset++;
+ pwm_motor2.write(0);
+ batjeset = integral = 0;
+ wait(1);
+ goto motor1control;
}
}
- pwm_motor2.write(0);
- batjeset = integral = 0;
- wait(1);
-
- while(balhit==0) { // loop voor het slaan mbv motor1 (batje snelheid)
+
+motor1control:
+ while(1) { // loop voor het slaan mbv motor1 (batje snelheid)
while(!looptimerflag);
looptimerflag = false; //clear flag
- //regelaar motor1, bepaalt snelheid
- controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
- integral = integral + controlerror*TSAMP;
- pwm = Kp1*controlerror + Ki1*integral;
-
+ if (balhit == 0) { //regelaar motor1, bepaalt snelheid
+ controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1;
+ integral = integral + controlerror*TSAMP;
+ pwm = Kp1*controlerror + Ki1*integral;
+ } else { //regelaar motor1, bepaalt positie
+ pwm_motor1.write(0);
+ balhit = integral = 0;
+ wait(1); // wait voordat arm weer naar beginpositie terugkeert
+ goto resetpositionmotor1;
+ }
+
keep_in_range(&pwm, -1,1);
pwm_motor1.write(abs(pwm));
@@ -608,11 +582,9 @@
balhit = 1;
}
}
-pwm_motor1.write(0);
-balhit = integral = 0;
-wait(1); // wait voordat arm weer naar beginpositie terugkeert
-
- while(batjeset < 200) { // slagarm wordt weer in oorspronkelijke positie geplaatst
+
+resetpositionmotor1:
+ while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
while(!looptimerflag);
looptimerflag = false; //clear flag
@@ -631,17 +603,22 @@
pwm_motor1.write(abs(pwm));
//controleert of arm terug in positie is
- if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
- batjeset = 0;
+ if(batjeset < 200) {
+ if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
} else {
- batjeset++;
+ pwm_motor1.write(0);
+ batjeset = integral = 0;
+ wait(1);
+ goto resetpositionmotor2;
}
}
- pwm_motor1.write(0);
- batjeset = integral = 0;
- wait(1);
-
- while(batjeset < 200) { // loop voor het goed plaatsen van motor2 (batje hoek)
+
+resetpositionmotor2:
+ while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
while(!looptimerflag);
looptimerflag = false; //clear flag
@@ -661,18 +638,18 @@
pwm_motor2.write(abs(pwm));
//controleert of batje positie heeft bepaald
- if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
- // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
- batjeset = 0;
+ if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
+ if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
+ batjeset = 0;
+ } else {
+ batjeset++;
+ }
} else {
- batjeset++;
- }
+ pwm_motor2.write(0);
+ batjeset = integral = 0;
+ wait(1);
+ direction = force = 0;
+ goto directionchoice;
+ }
}
- pwm_motor2.write(0);
- batjeset = integral = 0;
- direction = force = 0;
- wait(1);
- }
-
- }
} // end main
\ No newline at end of file