kappa
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Fork of PROJECT_incl_regelaar by
Diff: PROJECT_main.cpp
- Revision:
- 25:1ec0da9b26a5
- Parent:
- 24:1b812b393264
- Child:
- 26:8d91c4c54bb6
--- a/PROJECT_main.cpp Tue Nov 04 10:39:45 2014 +0000 +++ b/PROJECT_main.cpp Tue Nov 04 11:28:21 2014 +0000 @@ -543,20 +543,19 @@ scope.set(4, state); switch(state) { - case 1: { - setpoint1=0; - //setpoint2 += 1*TSAMP; + case 1: switch (direction) { case 1: pwm_motor2.write(0.8); motor2dir = 1; count++; - if (count>140) { + if (count>170) { state=2; count = 0; pwm_motor2.write(0); } break; + case 2: pwm_motor2.write(0.8); motor2dir = 1; @@ -589,27 +588,27 @@ case 3: { switch (force) { case 1: - pwm_motor1.write(0.8); - motor2dir = 1; + pwm_motor1.write(1); + motor1dir = 1; break; case 2: - pwm_motor1.write(0.8); - motor2dir = 1; + pwm_motor1.write(1); //net niet haalbaar + motor1dir = 1; break; case 3: - pwm_motor1.write(0.8); - motor2dir = 1; + pwm_motor1.write(1); //niet haalbaar + motor1dir = 1; break; } - if(fabs(motor1.getPosition()*omrekenfactor1)>2.36) { - setpoint1 = 2.36; + if(fabs(motor1.getPosition()*omrekenfactor1)>7.0) { + pwm_motor1.write(0); state = 4; } break; } case 4: { count++; - if(count>1000) { + if(count>4000) { count = 0; state = 1; goto motor2cal; @@ -617,163 +616,5 @@ 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; - scope.set(5, pwm1); - - keep_in_range(&pwm1, -1,1); - pwm_motor1.write(fabs(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; - - keep_in_range(&pwm, -1,1); - pwm_motor2.write(fabs(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 - - 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; - } - - keep_in_range(&pwm, -1,1); - pwm_motor1.write(abs(pwm)); - - if(pwm > 0) { - motor1dir = 1; - } else { - motor1dir = 0; - } - - //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 - - 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 = 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)); - - //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; - } - } - - 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); - - 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; - } else { - batjeset++; - } - } 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