Robotarm 6DOF Codesys snelheidsregeling Sructured Text

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

Lambiek

Special Member

Ten eerste welkom op het forum.

Ik heb verder geen ervaring met Codesys, maar je benoemt TIMER0 en dan zet je bij je IF statement TIMER0.Q. Is die Q een variabele of anders? En om wat voor servo's gaat het, zijn dat modelbouw servo's?

Als je haar maar goed zit, GROETEN LAMBIEK.

Bedankt voor uw reply.

TIMER.Q is de uitgang van de ON-timer. Vanaf dat de ingang IN van de timer TRUE wordt, dan zal na afloop van de ingestelde tijd TIMER_DURATION, de uitgang TIMER.Q TRUE worden. Het betreft hier een ON-delay. Ik hoop ergens op deze manier de snelheid van positionering te regelen.

Nadien wordt de ingang terug op FALSE gezet, anders blijft deze TRUE.

Het betreft hier inderdaad modelbouw servo's. (Maximum 7,4V, 180° rotatie)

In bijlage vindt u een afbeelding kwestie van een beter beeld te hebben van het project.

Mvg

Lambiek

Special Member

Op 25 juni 2019 14:52:28 schreef Snowman159:
In bijlage vindt u een afbeelding kwestie van een beter beeld te hebben van het project.

Ziet er mooi uit zo. :)

Maar ik zie nergens dat je de servo aanstuurt met een puls tussen de 1 en 2mS, of moeten die timers dat doen? Je signaal moet +/- 20mS zijn, 50Hz dus. Als je de servo's langzamer wil laten bewegen moet die tijd langer worden.

Hier heb je nog een artikel over servo's, misschien kun je hier nog iets uithalen.

https://www.circuitsonline.net/artikelen/view/48

Als je haar maar goed zit, GROETEN LAMBIEK.
GJ_

Moderator

Op 25 juni 2019 18:13:25 schreef Lambiek:
[...]
Ziet er mooi uit zo. :)

Maar ik zie nergens dat je de servo aanstuurt met een puls tussen de 1 en 2mS

Precies mijn gedachte.

code:


IF TIMER0.Q = TRUE THEN
	GVL.SERVO6 := GVL.SERVO6 + 1;
END_IF		
TIMER0.IN := FALSE;	

Zonder een en ander in detail te bekijken, zomaar een ding om rekening mee te houden: pas op met het uitvoeren van timers in voorwaardelijke structuren. Met de instructie "TIMER0.IN := FALSE" wordt de timer nog niet gereset. De volgende PLC-cyclus is TIMER0.Q dus nog gewoon hoog. Als je een timer wil resetten, schrijf je beter:

code:

TIMER0(IN := FALSE);

Pas dan wordt de timer-code uitgevoerd en dus, de uitgang FALSE gezet.

Lambiek

Special Member

Dus hoe stuur je de servo's nu aan? Heb je daar een voorbeeld van?

Als je haar maar goed zit, GROETEN LAMBIEK.

Bedankt voor de reacties.

De servo's worden aangestuurd met een puls tussen 0,5ms en 2,5ms. Waarden tussen 1 en 2ms sturen deze niet volledig uit tussen 0 en 180 graden. Dit hangt waarschijnlijk af van type en fabrikant. Dit heb ik getest met een functiegenerator en een oscilloscoop. De neutrale positie bevindt zich op ongeveer 1,5ms.

Hier wat interessante info over de servo:

Bedrijfsstroom: 80 - 100 mA
Bedrijfsspanning: 5 - 7,4V
Blokkeerstroom: 1,8 - 2 A
Stroom in rust: 5 - 5 mA
maximale belasting: 18,3 - 21,5 kg / cm
Werkhoek: 180°
Neutrale positie: 1500 µs

Om de servo te kunnen aansturen moest ik de datasheet van de fabrikant van de PLC raadplegen. De PLC heeft 6 servo-channels. Kanaal 0A en 0B, 1A en 1B, 2A en 2B.

Voor de berekening heb ik een PDF in bijlage geplaatst. Dit is een stuk van een verslag voor een avondopleiding graduaat elektromechanica. Hierin staat alles duidelijk vermeld samen met de afbeeldingen. Anders zou dit een lange post worden. Belangrijk om te weten is dat de naamgeving en dergelijke van vele van deze variabelen vast staat en door de fabrikant is bepaald.

Hou er rekening mee dat de positionering correct gebeurt. Hier heb ik geen problemen mee. Ik kan dit perfect zonder timers, maar de arm beweegt op max snelheid en dit komt niet mooi over. Dit project werkt zoals het moet, maar een snelheidsregeling is een mooie finishing touch. :)

Tevens zal ik zoals vermeld door '(nog)amateur' in een vorige reply de syntax 'TIMER0.IN := FALSE;' wijzigen door 'TIMER0(IN := FALSE);
Dit zal ik pas vanaf vanavond kunnen testen wegens een drukke agenda.

De waarden in de code bv. voor SERVO6 --> 95 (dus de 95 = eindpositie) wordt dus omgezet naar een pulsduur voor positionering. Zie PDF. De tijdsduur(TIMER_DURATION) die ik geef aan de timers zou dus moeten bepalen om de hoeveel tijd de waarde van de servo incrementeel (hier met +1) wijzigt naar 95. Zo zou de servo langzamer moeten bewegen.

Mvg

Lambiek

Special Member

Op 26 juni 2019 12:40:41 schreef Snowman159:
De servo's worden aangestuurd met een puls tussen 0,5ms en 2,5ms. Waarden tussen 1 en 2ms sturen deze niet volledig uit tussen 0 en 180 graden. Dit hangt waarschijnlijk af van type en fabrikant.

Dat klopt, dat hangt inderdaad af van de fabrikant.

De PLC heeft 6 servo-channels. Kanaal 0A en 0B, 1A en 1B, 2A en 2B.

Aha, dat verklaart het één en ander. :)

Ik kan dit perfect zonder timers, maar de arm beweegt op max snelheid en dit komt niet mooi over. Dit project werkt zoals het moet, maar een snelheidsregeling is een mooie finishing touch. :)

Dat ben ik met je eens, als de arm bijna in positie is moet hij langzaam naar zijn juiste positie lopen.

De waarden in de code bv. voor SERVO6 --> 95 (dus de 95 = eindpositie) wordt dus omgezet naar een pulsduur voor positionering. Zie PDF. De tijdsduur(TIMER_DURATION) die ik geef aan de timers zou dus moeten bepalen om de hoeveel tijd de waarde van de servo incrementeel (hier met +1) wijzigt naar 95. Zo zou de servo langzamer moeten bewegen.

Ik vraag me af of dat goed gaat werken.

Als je in je programma een for next lus kan maken, zou je dit kunnen proberen. Je zal dat iets aan moeten passen, maar volgens mij was het zoiets. Door de waarde van step aan te passen moet de servo langzamer of sneller gaan draaien.

code:


WHILE 1 = 1                   
  FOR Positie = 1000 TO 2000 STEP 20  
    SERVO PORTA.0, Positie    
    DELAYMS 20                 
  NEXT

  FOR Positie = 2000 TO 1000 STEP -20 
    SERVO PORTA.0, Positie    
    DELAYMS 20                 
  NEXT
WEND

Voor de berekening heb ik een PDF in bijlage geplaatst. Dit is een stuk van een verslag voor een avondopleiding graduaat elektromechanica.

Is dit een school opdracht of zo?

Als je haar maar goed zit, GROETEN LAMBIEK.

Zoals Snowman159 al aangeeft, eerst maar eens die timer-instructies aanpassen, dat is echt iets dat misgaat in het programma. De timers lopen eenmalig af om vervolgens nooit gereset te worden.

Daarnaast lijkt het mij ook wel sjiek (zo niet noodzakelijk) om je timers te initialiseren bij een reset van het programma. Dus:

code:


IF GVL.xRESET THEN
.... Jouw code
ELSE
   TIMER0(IN := FALSE);
   TIMER1(IN := FALSE);
   etc. etc.
END_IF;

Zo mogelijk verplicht de compiler je om ook een waarde aan de preset time toe te kennen. In dat geval zou ik het lekker kort houden:

code:


TIMER0(IN := FALSE, PT := 0);
TIMER1(IN := FALSE, PT := 0);
etc. etc.

[Bericht gewijzigd door (nog) amateur op 26 juni 2019 14:11:19 (20%)]

Hallo

Ik heb zelf iets anders geprobeerd met positief resultaat.

Ik heb in mijn code enkel de WHILE's veranderd naar IF's. Vervolgens de TIMER_DURATION op 1ms gezet.

Ik stelde positieve resultaten vast, maar de servo's bewogen nog te traag, waardoor ik het volgende gedaan heb:

Ik heb de PLC-cyclus gewijzigd van 100ms naar 20ms.

Met als resultaat een zeer mooie soepele beweging. Met de timers was blijkbaar niets mis. Enkel de whiles zorgden voor problemen. :)

Is dit een school opdracht of zo?

Oorspronkelijk niet, maar aangezien ik mijn eindwerk volgend jaar moet doen voor de avondopleiding, dacht ik ja, kan ik evengoed nemen wat ik al heb en hier en daar eventueel nog wat leuke extra's toevoegen.

Het betreft een magazijnsysteem met 4 opslaglocaties, 1 input- en 1 outputlocatie (de outputlocatie werkt als buffer, zodat tegelijk nog input mogelijk, wat de efficiëntie ten goede komt).

De klauw neemt dan telkens een lange bout op die voorzien is een grote moerplaat.

Detectie in de input --> klauw neemt bout --> plaatst in lege locatie tot ze alle vier vol zijn.

Omgekeerd bij bestelling: opnemen uit locatie --> outputzone.

Zoals je zag op de foto zijn er 6 inductieve sensoren geplaatst.

Ik ben nog bezig met het ontwerp van een SQL-database om de magazijninhoud bij te houden en een website om online orders te kunnen plaatsen. Bij plaatsing van een bestelling controleert het warehousesysteem of er nog voorraad is en indien ja, zet deze het bestelde materiaal klaar. Kwestie van mechatronics wat te combineren met IoT. Industry 4.0 in hobbyformaat ;)

Voor mij lijkt het onderwerp van deze post opgelost.

Bedankt voor de hulp.

Mvg

Update:

Ik heb nu gewoon de timers weggelaten. Het manipuleren van de PLC-cyclus is voldoende hier. Deze staat nu op 20ms en werkt stabiel.

c code:


IF GVL.SERVO6 <> 95 THEN	
	IF GVL.SERVO6 < 95 THEN	
			GVL.SERVO6 := GVL.SERVO6 + 1;
	ELSIF GVL.SERVO6 > 95 THEN
			GVL.SERVO6 := GVL.SERVO6 - 1;
	END_IF
END_IF

Ik zal het nog eenvoudiger maken en van dergelijk stukje code een functieblok maken, waarbij de getalwaarde en de servo vervangen wordt door een parameter die bij een call telkens wordt ingevuld.

Zoals in mijn vorige reply was het allerbelangrijkste het vervangen van de whiles in if's.

mvg

Lambiek

Special Member

Op 27 juni 2019 13:24:55 schreef Snowman159:
Oorspronkelijk niet, maar aangezien ik mijn eindwerk volgend jaar moet doen voor de avondopleiding, dacht ik ja, kan ik evengoed nemen wat ik al heb en hier en daar eventueel nog wat leuke extra's toevoegen.

Leuke eindopdracht zo. :)

Op 27 juni 2019 14:07:36 schreef Snowman159:
Update:
Ik heb nu gewoon de timers weggelaten. Het manipuleren van de PLC-cyclus is voldoende hier. Deze staat nu op 20ms en werkt stabiel.

Mooi dat het werkt zo op deze manier. Nog wel even een filmpje plaatsen hé. ;)

Als je haar maar goed zit, GROETEN LAMBIEK.

Ik heb nog 1 probleem:

Alle stappen gebeuren tegelijk in plaats van na elkaar.

For loops brengen blijken geen oplossing te brengen (vb. I:=0 TO 1 DO met VAR I : INT := 0;)

Ik heb intussen meerdere manieren getest met telkens hetzelfde probleem. Nu werkt de snelheid correct, maar werken de servo's nu telkens tegelijk.

Dit levert problemen mocht de arm nog een object vasthouden. Zo zal hij draaien en tegelijk het object lossen en weggooien. Dit is nu ook niet de bedoeling. :p

c code:


PROGRAM Reset
VAR
	Positie1 : BOOL;
	Positie2 : BOOL;
	Positie3 : BOOL;
	Positie4 : BOOL;
	Positie5 : BOOL;
	Positie6 : BOOL;
	Positie7 : BOOL;
END_VAR
---------------------------
//Algemene resetfunctie naar de neutrale positie.
IF GVL.xRESET THEN
	//Open van klauw (object loslaten).
	IF GVL.SERVO6 <> 95 THEN
		Positie1 := TRUE;	
		IF  Positie1 = TRUE AND GVL.SERVO6 < 95 THEN	
			GVL.SERVO6 := GVL.SERVO6 + 1;
		ELSIF Positie1 = TRUE AND GVL.SERVO6 > 95 THEN
			GVL.SERVO6 := GVL.SERVO6 - 1;
		END_IF
	ELSIF GVL.SERVO6 = 95 THEN
		Positie1 := FALSE;
		Positie2 := TRUE;
	END_IF
	
	//Aansturen van servo 2.
	IF Positie2 = TRUE AND GVL.SERVO2 <> 109 THEN		
		IF  Positie2 = TRUE AND GVL.SERVO2 < 109 THEN
			GVL.SERVO2 := GVL.SERVO2 + 1;
		ELSIF  Positie2 = TRUE AND GVL.SERVO2 > 109 THEN
			GVL.SERVO2 := GVL.SERVO2 - 1;
		END_IF	
	ELSIF GVL.SERVO2 = 109 THEN
		Positie2 := FALSE;
		Positie3 := TRUE;			
	END_IF
	
	//Aansturen van servo 1.
	IF Positie3 = TRUE AND GVL.SERVO1 <> 190 THEN		
		IF  Positie3 = TRUE AND GVL.SERVO1 < 190 THEN
			GVL.SERVO1 := GVL.SERVO1 + 1;
		ELSIF  Positie3 = TRUE AND GVL.SERVO1 > 190 THEN
			GVL.SERVO1 := GVL.SERVO1 - 1;
		END_IF	
	ELSIF GVL.SERVO1 = 190 THEN
		Positie3 := FALSE;
		Positie4 := TRUE;			
	END_IF

	
	//Aansturen van servo 3.
		IF Positie4 = TRUE AND GVL.SERVO3 <> 297 THEN		
		IF  Positie4 = TRUE AND GVL.SERVO3 < 297 THEN
			GVL.SERVO3 := GVL.SERVO3 + 1;
		ELSIF  Positie4 = TRUE AND GVL.SERVO3 > 297 THEN
			GVL.SERVO3 := GVL.SERVO3 - 1;
		END_IF	
	ELSIF GVL.SERVO3 = 297 THEN
		Positie4 := FALSE;
		Positie5 := TRUE;			
	END_IF

	
	//Aansturen van servo 4.
		IF Positie5 = TRUE AND GVL.SERVO4 <> 108 THEN		
		IF  Positie5 = TRUE AND GVL.SERVO4 < 108 THEN
			GVL.SERVO4 := GVL.SERVO4 + 1;
		ELSIF  Positie5 = TRUE AND GVL.SERVO4 > 108 THEN
			GVL.SERVO4 := GVL.SERVO4 - 1;
		END_IF	
	ELSIF GVL.SERVO4 = 108 THEN
		Positie5 := FALSE;
		Positie6 := TRUE;			
	END_IF

	
	//Aansturen van servo 5.
		IF Positie6 = TRUE AND GVL.SERVO5 <> 197 THEN		
		IF  Positie6 = TRUE AND GVL.SERVO5 < 197 THEN
			GVL.SERVO5 := GVL.SERVO5 + 1;
		ELSIF  Positie6 = TRUE AND GVL.SERVO5 > 197 THEN
			GVL.SERVO5 := GVL.SERVO5 - 1;
		END_IF	
	ELSIF GVL.SERVO5 = 197 THEN
		Positie6 := FALSE;
		Positie7 := TRUE;			
	END_IF

	
	//Aansturen van servo 6.
		IF Positie7 = TRUE AND GVL.SERVO6 <> 72 THEN		
		IF  Positie7 = TRUE AND GVL.SERVO6 < 72 THEN
			GVL.SERVO6 := GVL.SERVO6 + 1;
		ELSIF  Positie7 = TRUE AND GVL.SERVO6 > 72 THEN
			GVL.SERVO6 := GVL.SERVO6 - 1;
		END_IF	
	ELSIF GVL.SERVO6 = 72 THEN
		Positie7 := FALSE;
	END_IF

END_IF

Dit levert gewoon terug hetzelfde resultaat als mocht ik de stappen op de vorige manier schrijven:

c code:


IF GVL.SERVO6 <> 95 THEN	
	IF GVL.SERVO6 < 95 THEN	
		GVL.SERVO6 := GVL.SERVO6 + 1;
	ELSIF GVL.SERVO6 > 95 THEN
		GVL.SERVO6 := GVL.SERVO6 - 1;
        END_IF
END_IF
Lambiek

Special Member

Op 27 juni 2019 17:17:44 schreef Snowman159:
Ik heb nog 1 probleem:

Alle stappen gebeuren tegelijk in plaats van na elkaar.

kun je dat niet in verschillende loopjes regelen?

Als je haar maar goed zit, GROETEN LAMBIEK.