Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
Revision 17:dc695e4e749b, committed 2014-11-04
- Comitter:
- phgbartels
- Date:
- Tue Nov 04 09:56:29 2014 +0000
- Parent:
- 16:9e4f61aab351
- Child:
- 18:264812048611
- Commit message:
- ALLES WERKT!!! Behalve de 'snelheid instellen'
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Mon Nov 03 23:19:13 2014 +0000 +++ b/main.cpp Tue Nov 04 09:56:29 2014 +0000 @@ -241,8 +241,6 @@ current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is pos_motor1_rad = current_pos_motor1/(1600./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa PWM1_percentage = pid(setpoint, pos_motor1_rad); - pc.printf("setpoint : %f\r\n",setpoint); - pc.printf("pos_motor_1 :%f\r\n", pos_motor1_rad); if (PWM1_percentage < -100) { PWM1_percentage = -100; @@ -276,18 +274,18 @@ return out_p + out_i + out_d; }//float pid -void translatie (float position2_setpoint_rad, float speed2_radpersecond, float marge2) +void translatie (float position2_setpoint_rad, float speed2_radpersecond) { static float setpoint = 0; if (setpoint < position2_setpoint_rad) { - setpoint = prev_setpoint +( 0.0005 * speed2_radpersecond); + setpoint += (0.005 * speed2_radpersecond); if (setpoint > position2_setpoint_rad) { setpoint = position2_setpoint_rad; } } else if (setpoint > position2_setpoint_rad) { - setpoint = prev_setpoint - (0.0005 * speed2_radpersecond); + setpoint -= (0.005 * speed2_radpersecond); if (setpoint < position2_setpoint_rad) { setpoint = position2_setpoint_rad; } @@ -297,7 +295,7 @@ } current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is - pc.printf("c: %d\r\n", motor2.getPosition());//current_pos_motor2); + pc.printf("setpoint: %f\r\n", setpoint);//current_pos_motor2); pos_motor2_rad = current_pos_motor2/(960./(2.*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaa PWM2_percentage = pid_motor2(setpoint, pos_motor2_rad); @@ -353,7 +351,7 @@ else if(current_herhalingen == 100) { current_herhalingen = 0; previous_herhalingen = 0; - state = METEN_HOOGTE; + state = ARM_KALIBRATIE; }//else break; }//case RUST: @@ -438,7 +436,7 @@ } - translatie(position2_setpoint, 10, 0.1); + translatie(position2_setpoint, 5); if (current_herhalingen >= 400) { current_herhalingen = 0; @@ -463,11 +461,12 @@ case RETURN2RUST: { GotoPosition(0,0.25); - translatie(0,10,0.1); + //translatie(0,10,0.1); doel_richting = 0; doel_hoogte = 0; if (current_herhalingen >= 200) { - translatie(0,10,0.1); + current_herhalingen = 0; + translatie(0,0.5); if (current_herhalingen >=200){ state = RUST; current_herhalingen = 0;