Beste
Ik heb via Codesys en een Pixtend PLC een robotarm (6DOF) gerealiseerd. Alles werkt zoals het moet, maar met de uitzondering dat mijn snelheidsregeling niet werkt. Ik heb reeds in ST het onderstaande stuk code geschreven om de arm terug naar zijn neutrale positie te brengen, maar met een lagere snelheid. De neutrale positie wordt vlot bereikt maar loopt spijtig genoeg enkel terug op maximum snelheid. Via TIMER_DURATION probeer ik de snelheid te regelen.
De functies van de servo's maken hier nu niet uit, aangezien ze doen wat moet. Enkel de snelheid lukt niet. Als VAR heb ik 13 timers.
c code:
IF GVL.xRESET THEN
//Open van klauw (object loslaten).
WHILE GVL.SERVO6 <> 95 DO
IF GVL.SERVO6 < 95 THEN
TIMER0(IN := TRUE, PT := TIMER_DURATION);
IF TIMER0.Q = TRUE THEN
GVL.SERVO6 := GVL.SERVO6 + 1;
END_IF
TIMER0.IN := FALSE;
ELSIF GVL.SERVO6 > 95 THEN
TIMER1(IN := TRUE, PT := TIMER_DURATION);
IF TIMER1.Q = TRUE THEN
GVL.SERVO6 := GVL.SERVO6 - 1;
END_IF
TIMER1.IN := FALSE;
END_IF
END_WHILE
//Aansturen van servo 2.
WHILE GVL.SERVO6 = 95 AND GVL.SERVO2 <> 109 DO
IF GVL.SERVO2 < 109 THEN
TIMER2(IN := TRUE, PT := TIMER_DURATION);
IF TIMER2.Q = TRUE THEN
GVL.SERVO2 := GVL.SERVO2 + 1;
END_IF
TIMER2.IN := FALSE;
ELSIF GVL.SERVO2 > 109 THEN
TIMER3(IN := TRUE, PT := TIMER_DURATION);
IF TIMER3.Q = TRUE THEN
GVL.SERVO2 := GVL.SERVO2 - 1;
END_IF
TIMER3.IN := FALSE;
END_IF
END_WHILE
//Aansturen van servo 1.
WHILE GVL.SERVO2 = 109 AND GVL.SERVO1 <> 190 DO
IF GVL.SERVO1 < 190 THEN
TIMER4(IN := TRUE, PT := TIMER_DURATION);
IF TIMER4.Q = TRUE THEN
GVL.SERVO1 := GVL.SERVO1 + 1;
END_IF
TIMER4.IN := FALSE;
ELSIF GVL.SERVO1 > 190 THEN
TIMER5(IN := TRUE, PT := TIMER_DURATION);
IF TIMER5.Q = TRUE THEN
GVL.SERVO1 := GVL.SERVO1 - 1;
END_IF
TIMER5.IN := FALSE;
END_IF
END_WHILE
//Aansturen van servo 3.
WHILE GVL.SERVO1 = 190 AND GVL.SERVO3 <> 297 DO
IF GVL.SERVO3 < 297 THEN
TIMER6(IN := TRUE, PT := TIMER_DURATION);
IF TIMER6.Q = TRUE THEN
GVL.SERVO3 := GVL.SERVO3 + 1;
END_IF
TIMER6.IN := FALSE;
ELSIF GVL.SERVO3 > 297 THEN
TIMER7(IN := TRUE, PT := TIMER_DURATION);
IF TIMER7.Q = TRUE THEN
GVL.SERVO3 := GVL.SERVO3 - 1;
END_IF
TIMER7.IN := FALSE;
END_IF
END_WHILE
//Aansturen van servo 4.
WHILE GVL.SERVO3 = 297 AND GVL.SERVO4 <> 108 DO
IF GVL.SERVO1 < 108 THEN
TIMER8(IN := TRUE, PT := TIMER_DURATION);
IF TIMER8.Q = TRUE THEN
GVL.SERVO4 := GVL.SERVO4 + 1;
END_IF
TIMER8.IN := FALSE;
ELSIF GVL.SERVO4 > 108 THEN
TIMER9(IN := TRUE, PT := TIMER_DURATION);
IF TIMER9.Q = TRUE THEN
GVL.SERVO4 := GVL.SERVO4 - 1;
END_IF
TIMER9.IN := FALSE;
END_IF
END_WHILE
//Aansturen van servo 5.
WHILE GVL.SERVO4 = 108 AND GVL.SERVO5 <> 197 DO
IF GVL.SERVO5 < 197 THEN
TIMER10(IN := TRUE, PT := TIMER_DURATION);
IF TIMER10.Q = TRUE THEN
GVL.SERVO5 := GVL.SERVO5 + 1;
END_IF
TIMER10.IN := FALSE;
ELSIF GVL.SERVO5 > 197 THEN
TIMER11(IN := TRUE, PT := TIMER_DURATION);
IF TIMER11.Q = TRUE THEN
GVL.SERVO5 := GVL.SERVO5 - 1;
END_IF
TIMER11.IN := FALSE;
END_IF
END_WHILE
//Aansturen van servo 6.
WHILE GVL.SERVO5 = 197 AND GVL.SERVO6 <> 72 DO
IF GVL.SERVO6 < 72 THEN
TIMER12(IN := TRUE, PT := TIMER_DURATION);
IF TIMER12.Q = TRUE THEN
GVL.SERVO6 := GVL.SERVO6 + 1;
END_IF
TIMER12.IN := FALSE;
ELSIF GVL.SERVO6 > 72 THEN
TIMER13(IN := TRUE, PT := TIMER_DURATION);
IF TIMER13.Q = TRUE THEN
GVL.SERVO6 := GVL.SERVO6 - 1;
END_IF
TIMER13.IN := FALSE;
END_IF
END_WHILE
END_IF
Ik heb het internet de laatste twee weken al hopeloos binnenstebuiten gekeerd. Ik dacht eventueel ook aan een chopperregeling en om deze zelf te maken en dan te activeren via een 5V digital out spanning van de PLC wanneer nodig.
Hopelijk kan iemand mij helpen. Het probleem zit ergens in de code, maar ik zie echt niet wat het is.
Alvast bedankt.
Met vriendelijke groeten
