Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of Main-script_groep7_V3 by
Diff: main.cpp
- Revision:
- 18:264812048611
- Parent:
- 17:dc695e4e749b
--- a/main.cpp Tue Nov 04 09:56:29 2014 +0000 +++ b/main.cpp Tue Nov 04 11:28:44 2014 +0000 @@ -103,6 +103,7 @@ float PWM_richting2; float PWM_richting3; float position2_setpoint; +float snelheid; enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST, TEST}; //verschillende stadia definieren voor gebruik in CASES state_t state = RUST; @@ -237,7 +238,8 @@ current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen; } - + + pc.printf("%f\r\n", setpoint); 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); @@ -295,7 +297,6 @@ } current_pos_motor2 = motor2.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is - 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); @@ -435,8 +436,7 @@ position2_setpoint = 25; } - - translatie(position2_setpoint, 5); + translatie(position2_setpoint, 1); if (current_herhalingen >= 400) { current_herhalingen = 0; @@ -448,7 +448,14 @@ }//case INSTELLEN_RICHTING case SLAAN: { - GotoPosition(1.9 ,1000); + if (doel_hoogte == 1) { + snelheid = 2; + } else if (doel_hoogte ==2) { + snelheid = 2.25; + } else { + snelheid = 5; + } + GotoPosition(1.9, snelheid); if (current_herhalingen == 400) { current_herhalingen = 0; previous_herhalingen = 0; @@ -460,13 +467,12 @@ }//case SLAAN case RETURN2RUST: { - GotoPosition(0,0.25); - //translatie(0,10,0.1); + GotoPosition(0,0.1); doel_richting = 0; doel_hoogte = 0; if (current_herhalingen >= 200) { current_herhalingen = 0; - translatie(0,0.5); + translatie(0,1.6); if (current_herhalingen >=200){ state = RUST; current_herhalingen = 0;