kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar by
Diff: PROJECT_main.cpp
- Revision:
- 8:75980dc35763
- Parent:
- 7:ca1ade91bd14
- Child:
- 9:0bc7f83b761e
--- a/PROJECT_main.cpp Mon Nov 03 18:22:20 2014 +0000 +++ b/PROJECT_main.cpp Mon Nov 03 18:52:57 2014 +0000 @@ -103,8 +103,17 @@ float previouserror = 0; float pwm = 0; +float pwm1 =0; +float integral1 = 0; +float derivative1 = 0; +float controlerror1 = 0; +float previouserror1 = 0; + +int state = 1; +int count = 1; + float omrekenfactor1 = 0.0028035714; // 6.28/(32*70) -float omrekenfactor2 = 0.0015213178; // 6.28/(24*172); +float omrekenfactor2 = 0.0015213178; // 6.28/(24*172); float setpoint1 = 0; //te behalen speed van motor1 (37D) float setpoint2 = 0; //te behalen hoek van motor2 (25D) @@ -134,7 +143,7 @@ looptimerflag = true; scope.set(0, motor2.getPosition()*omrekenfactor2); scope.set(0, motor1.getPosition()*omrekenfactor1); - scope.send(); + scope.send(); } @@ -307,7 +316,7 @@ motor1dir = 0; //linksom wait(1); // anders wordt de while(1) meteen onderbroken while(1) { - if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen + if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // motor1.getSpeed()*omrekenfactor1 < 0.20, 0.20 is nog aan te passen pwm_motor1.write(0); motor1.setPosition(0); goto emgcal; @@ -323,35 +332,35 @@ wait (1); for1 = for2 = for3 = 0; -if(kalibratie==0) { - //biceps kalibratie - blink.attach(kalbi, 0.2); - for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { - if (bi_max < bi_result) { - bi_max = bi_result; + if(kalibratie==0) { + //biceps kalibratie + blink.attach(kalbi, 0.2); + for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { + if (bi_max < bi_result) { + bi_max = bi_result; + } + wait (0.01); } - wait (0.01); - } - blink.detach(); - dir1 = dir2 = dir3 = 1; - pc.printf("kalbi "); - wait (1); + blink.detach(); + dir1 = dir2 = dir3 = 1; + pc.printf("kalbi "); + wait (1); - //triceps kalibratie - blink.attach(kaltri, 0.2); - for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { - if (tri_max < tri_result) { - tri_max = tri_result; + //triceps kalibratie + blink.attach(kaltri, 0.2); + for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) { + if (tri_max < tri_result) { + tri_max = tri_result; + } + wait (0.01); } - wait (0.01); + blink.detach(); + for1 = for2 = for3 = 1; + pc.printf("kaltri "); + wait (1); + for1 = for2 = for3 = 0; + kalibratie = 1; } - blink.detach(); - for1 = for2 = for3 = 1; - pc.printf("kaltri "); - wait (1); - for1 = for2 = for3 = 0; - kalibratie = 1; -} directionchoice: log_timer.attach(looper, TSAMP_EMG); @@ -515,168 +524,225 @@ /* Vanaf hier komt de aansturing van de motor */ - switch (direction) { - case 1: - setpoint2 = (0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel - break; - case 2: - setpoint2 = (0.436332313); //25 graden vanaf nul-punt (precies midden) - break; - case 3: - setpoint2 = (0.436332313-0.197222205); // 25 graden - 11,3 graden, slag naar rechterdoel - break; - } + +// FORMAT_CODE_START + setpoint1=0; + setpoint2=0; + integral1 = integral = 0; + previouserror1 = previouserror = 0; + while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) + while(!looptimerflag) + looptimerflag = false; //clear flag + + switch (direction) { + case 1: + setpoint2 = (0.436332313+0.197222205); //25 graden + 11,3 graden, slag naar linkerdoel + break; + case 2: + setpoint2 = (0.436332313); //25 graden vanaf nul-punt (precies midden) + break; + case 3: + setpoint2 = (0.436332313-0.197222205); // 25 graden - 11,3 graden, slag naar rechterdoel + break; + } - switch (force) { - case 1: - setpoint1 = 6.8; - break; - case 2: - setpoint1 = 7.4; - break; - case 3: - setpoint1 = 8.0; - break; - } + switch(state) { + case 1: + setpoint1=0; + if(abs(motor2.getPosition()*omrekenfactor2-setpoint2)<0.01) { + state = 2; + } + break; + + case 2: + switch (force) { + case 1: + setpoint1 += 6.8*TSAMP; + break; + case 2: + setpoint1 += 7.4*TSAMP; + break; + case 3: + setpoint1 += 8.0*TSAMP; + break; + } + if(abs(motor1.getPosition()*omrekenfactor1)>2.1){ + state = 3; + } + break; + case 3: + setpoint2 = 0; + setpoint1 -= 1.0*TSAMP; + if(setpoint1 < 0){ + state = 4; + } + break; - while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) - while(!looptimerflag); - looptimerflag = false; //clear flag + case 4: + setpoint1 = setpoint2 = 0; + count++; + if(count>1000){ + count = 0; + state = 1; + goto directionchoice; + } + break; + } + + //motor regeling + + //regelaar motor1, bepaalt positie + controlerror1 = setpoint1 - motor1.getPosition()*omrekenfactor1; + integral1 = integral1 + controlerror1*TSAMP; + derivative1 = (controlerror1 - previouserror1)/TSAMP; + pwm1 = Kp1*controlerror1 + Ki1*integral1 + Kd1*derivative1; + previouserror1 = controlerror1; + + keep_in_range(&pwm1, -1,1); + pwm_motor1.write(abs(pwm1)); + if(pwm1 > 0) { + motor1dir = 1; + } else { + motor1dir = 0; + } + //regelaar motor2, bepaalt positie controlerror = setpoint2 - motor2.getPosition()*omrekenfactor2; integral = integral + controlerror*TSAMP; derivative = (controlerror - previouserror)/TSAMP; pwm = Kp2*controlerror + Ki2*integral + Kd2*derivative; - previouserror = controlerror; + previouserror = controlerror; - keep_in_range(&pwm, -1,1); - pwm_motor2.write(abs(pwm)); + keep_in_range(&pwm, -1,1); + pwm_motor2.write(abs(pwm)); if(pwm > 0) { motor2dir = 1; } else { 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.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) { + batjeset = 0; + } else { + batjeset++; + } + } else { + pwm_motor2.write(0); + batjeset = integral = derivative = previouserror = 0; + wait(1); + //goto motor1control; + } + */ + } + /* + motor1control: + while(1) { // loop voor het slaan mbv motor1 (batje snelheid) + while(!looptimerflag); + looptimerflag = false; //clear flag - //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.05 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.95) { - batjeset = 0; - } else { - batjeset++; + if (balhit == 0) { //regelaar motor1, bepaalt snelheid + controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1; + integral = integral + controlerror*TSAMP; + derivative = (controlerror - previouserror)/TSAMP; + pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative; + previouserror = controlerror; + } else { //regelaar motor1, bepaalt positie + balhit = integral = derivative = previouserror = 0; + goto resetpositionmotor1; } - } else { - pwm_motor2.write(0); - batjeset = integral = derivative = previouserror = 0; - wait(1); - goto motor1control; - } - } + + keep_in_range(&pwm, -1,1); + pwm_motor1.write(abs(pwm)); + + if(pwm > 0) { + motor1dir = 1; + } else { + motor1dir = 0; + } -motor1control: - while(1) { // loop voor het slaan mbv motor1 (batje snelheid) - while(!looptimerflag); - looptimerflag = false; //clear flag + //controleert of batje balletje heeft bereikt + //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle + if (motor1.getPosition()*omrekenfactor1 > 1.60) { + balhit = 1; + } + } + // FORMAT_CODE_END - if (balhit == 0) { //regelaar motor1, bepaalt snelheid - controlerror = setpoint1 - motor1.getSpeed()*omrekenfactor1; + resetpositionmotor1: + while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst + while(!looptimerflag); + looptimerflag = false; //clear flag + + //regelaar motor1, bepaalt positie + controlerror = -1*motor1.getPosition()*omrekenfactor1; integral = integral + controlerror*TSAMP; derivative = (controlerror - previouserror)/TSAMP; - pwm = Kp1*controlerror + Ki1*integral + Kd1*derivative; - previouserror = controlerror; - } else { //regelaar motor1, bepaalt positie - balhit = integral = derivative = previouserror = 0; - goto resetpositionmotor1; - } + pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative; + previouserror = controlerror; + + keep_in_range(&pwm, -1,1); + if(pwm > 0) { + motor1dir = 1; + } else { + motor1dir = 0; //1 = rechtsom, 0 = linksom + } + + pwm_motor1.write(abs(pwm)); - keep_in_range(&pwm, -1,1); - pwm_motor1.write(abs(pwm)); - - if(pwm > 0) { - motor1dir = 1; - } else { - motor1dir = 0; + //controleert of arm terug in positie is + if(batjeset < 200) { + if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) { + batjeset = 0; + } else { + batjeset++; + } + } else { + pwm_motor1.write(0); + batjeset = integral = derivative = previouserror = 0; + wait(1); + goto resetpositionmotor2; + } } - //controleert of batje balletje heeft bereikt - //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle - if (motor1.getPosition()*omrekenfactor1 > 1.60) { - balhit = 1; - } - } - -resetpositionmotor1: - while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst - while(!looptimerflag); - looptimerflag = false; //clear flag + resetpositionmotor2: + while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) + while(!looptimerflag); + looptimerflag = false; //clear flag - //regelaar motor1, bepaalt positie - controlerror = -1*motor1.getPosition()*omrekenfactor1; - integral = integral + controlerror*TSAMP; - derivative = (controlerror - previouserror)/TSAMP; - pwm = Kp3*controlerror + Ki3*integral + Kd3*derivative; - previouserror = controlerror; + //regelaar motor2, bepaalt positie + controlerror = -1*motor2.getPosition()*omrekenfactor2; + integral = integral + controlerror*TSAMP; + derivative = (controlerror - previouserror)/TSAMP; + pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative; + previouserror = controlerror; - keep_in_range(&pwm, -1,1); - if(pwm > 0) { - motor1dir = 1; - } else { - motor1dir = 0; //1 = rechtsom, 0 = linksom - } + keep_in_range(&pwm, -1,1); - pwm_motor1.write(abs(pwm)); - - //controleert of arm terug in positie is - if(batjeset < 200) { - if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) { - batjeset = 0; + if(pwm > 0) { + motor2dir = 1; } else { - batjeset++; + motor2dir = 0; } - } else { - pwm_motor1.write(0); - batjeset = integral = derivative = previouserror = 0; - wait(1); - goto resetpositionmotor2; - } - } -resetpositionmotor2: - while(1) { // loop voor het goed plaatsen van motor2 (batje hoek) - while(!looptimerflag); - looptimerflag = false; //clear flag - - //regelaar motor2, bepaalt positie - controlerror = -1*motor2.getPosition()*omrekenfactor2; - integral = integral + controlerror*TSAMP; - derivative = (controlerror - previouserror)/TSAMP; - pwm = Kp4*controlerror + Ki4*integral + Kd4*derivative; - previouserror = controlerror; - - keep_in_range(&pwm, -1,1); + pwm_motor2.write(abs(pwm)); - if(pwm > 0) { - motor2dir = 1; - } else { - motor2dir = 0; - } - - 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.1 || motor2.getPosition()*omrekenfactor2 < -0.1) { - 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 > 0.1 || motor2.getPosition()*omrekenfactor2 < -0.1) { + batjeset = 0; + } else { + batjeset++; + } } else { - batjeset++; + pwm_motor2.write(0); + batjeset = integral = derivative = previouserror = 0; + wait(1); + direction = force = 0; + goto motor1cal; } - } else { - pwm_motor2.write(0); - batjeset = integral = derivative = previouserror = 0; - wait(1); - direction = force = 0; - goto motor1cal; - } - } + }*/ } // end main \ No newline at end of file