kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar by
Diff: PROJECT_main.cpp
- Revision:
- 3:2b2710b8b02e
- Parent:
- 2:10dd6a88dfd7
- Child:
- 4:697d5a806cc4
--- a/PROJECT_main.cpp Fri Oct 31 16:17:18 2014 +0000 +++ b/PROJECT_main.cpp Fri Oct 31 19:38:07 2014 +0000 @@ -327,10 +327,60 @@ wait (1); for1 = for2 = for3 = 0; -directionchoice: + + while(1) { //Loop van directiekeuze, naar slag en naar nulpositie 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; @@ -376,8 +426,9 @@ } } } -forcechoice: - while(1) { //Loop keuze FORCE + + 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 for(int j=1; j<4; j++) { if(j==1) { //red for1=1; @@ -441,8 +492,8 @@ choicesmade: blink.attach(okay, 0.2); - while(1) { - if(tri_result>FACTOR*tri_max) { + 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) { blink.detach(); switch (direction) { case 1: @@ -461,22 +512,23 @@ for2 = for3 = 0; break; } - - wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze - goto forcechoice; + balhit = 2; //zodat het de motorcontrol overslaat + force = 0; + wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze } else { if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) { - blink.detach(); - log_timer.detach(); - goto motorcontrol; + balhit = 1; } else { wait(0.01); // not sure of de wait noodzakelijk is (nu toegevoegd zodat het niet teveel strain levert op bordje) } } } - -motorcontrol: - + + log_timer.detach(); + blink.detach(); + wait(1); + + if(balhit!=2){ /* Vanaf hier komt de aansturing van de motor */ switch (direction) { @@ -503,7 +555,7 @@ break; } - while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) + while(batjeset<200) { // loop voor het goed plaatsen van motor2 (batje hoek) while(!looptimerflag); looptimerflag = false; //clear flag @@ -520,37 +572,27 @@ motor2dir = 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++; - } + //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; } else { - pwm_motor2.write(0); - batjeset = integral = 0; - wait(1); - goto motor1control; + batjeset++; } } - -motor1control: - while(1) { // loop voor het slaan mbv motor1 (batje snelheid) + pwm_motor2.write(0); + batjeset = integral = 0; + wait(1); + + while(balhit==0) { // loop voor het slaan mbv motor1 (batje snelheid) while(!looptimerflag); looptimerflag = false; //clear flag - 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; - } - + //regelaar motor1, bepaalt snelheid + controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1; + integral = integral + controlerror*TSAMP; + pwm = Kp1*controlerror + Ki1*integral; + keep_in_range(&pwm, -1,1); pwm_motor1.write(abs(pwm)); @@ -566,9 +608,11 @@ balhit = 1; } } - -resetpositionmotor1: - while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst +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 while(!looptimerflag); looptimerflag = false; //clear flag @@ -587,22 +631,17 @@ pwm_motor1.write(abs(pwm)); //controleert of arm terug in positie is - if(batjeset < 200) { - if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) { - batjeset = 0; - } else { - batjeset++; - } + if (motor1.getPosition()*omrekenfactor1 > 0.03 || motor1.getPosition()*omrekenfactor1 < -0.03) { + batjeset = 0; } else { - pwm_motor1.write(0); - batjeset = integral = 0; - wait(1); - goto resetpositionmotor2; + batjeset++; } } - -resetpositionmotor2: - while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) + pwm_motor1.write(0); + batjeset = integral = 0; + wait(1); + + while(batjeset < 200) { // loop voor het goed plaatsen van motor2 (batje hoek) while(!looptimerflag); looptimerflag = false; //clear flag @@ -622,18 +661,18 @@ pwm_motor2.write(abs(pwm)); //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 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) { - batjeset = 0; - } else { - batjeset++; - } + 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; } else { - pwm_motor2.write(0); - batjeset = integral = 0; - direction = force = 0; - wait(1); - goto directionchoice; - } + batjeset++; + } } + pwm_motor2.write(0); + batjeset = integral = 0; + direction = force = 0; + wait(1); + } + + } } // end main \ No newline at end of file