Programmering av styrning av tvättarm på tvättrobot

Transkript

1 Programmering av styrning av tvättarm på tvättrobot Examensarbete inom Datateknik av Per Degerfält Niklas Strömberg 29 januari 2008 Handledare: Prof. Lars Asplund, Mälardalens Högskola Jan Sandberg, Ramsta Robotics Mälardalens Högskola Institutionen för datateknik och elektronik IDE

2 DENNA SIDA HAR MED AVSIKT LÄMNATS TOM.

3 Sammanfattning Ramsta Robotics är ett företag baserat i Uppsala. Man har utvecklat en robot - Clever Cleaner, som används för att göra grovrengöring av djurstallar. Roboten styrs i dagsläget av ett enkelt styr- och reglersystem för industrin, en s.k. PLC. Den styr via ett drivkort 6 motorer, dock endast en motor i taget. Ett nytt motorstyrkort är nyligen framtaget till roboten vilket gör det möjligt att styra samtliga motorer samtidigt. Målet med arbetet var att ta fram en första version av ett nytt styrsystem som kommunicerar med de nyframtagna motorstyrkortet. Styrsystemet ska ge användaren möjligheten att välja mellan att styra respektive led manuellt, till att utnyttja invers kinematik där användaren styr var munstycket ska vara beläget och styrprogrammet omvandlar den önskade positionen till vinklar för respektive led. Funktionalitet för inspelning och uppspelning av en armrörelse ska också nnas i styrprogrammet. En ytgenererare ska också vara implementerad, med vilken operatören enkelt genom att endast ange 3 eller 4 hörnpunkter, kan få styrprogrammet att beräkna ett rörelsemönster för tvättning av den uppmarkerade ytan. Målen som var satta för detta examensarbete är uppnådda och en första version av styrprogrammet med de önskade funktionerna är framtagen. Dock behövs ytterligare arbete för att förbättra funktionaliteten då tester av styrprogrammet endast skett mot lösa motorer och ej på den faktiska roboten, där belastning på motorer, slitage och andra yttre omständigheter påverkar robotens beteende.

4 Abstract Ramsta Robotics is a company based in Uppsala. They have developed a robot - Clever Cleaner that is used to clean insides of livestock building's. The robot is currently controlled by a simple control system, a so called PLC. 6 motors is controlled with a simple controller card, but only one motor can be controlled at a time. A new motor controller card has recently been developed for the robot. The new card makes it possible to control several motors at the same time. The goal of this thesis was to develop a rst version of a control system that communicates with the new motor controller card. The control system should give the user the possibility to choose whether to control each arm separately or by using inverse kinematics, where the user is controlling the position of the nozzle and the control system calculates the desired position and then converts that to angles for each joint. Functionality for recording and replay a pre-recorded arm movement should also be implementd in the control system. A surface generator should also be implemented. The surface generator makes it possible for the user to select a surface using only 3 or 4 corner points. The control system would then use those points to create a path for the robot that will cover the selecteds surface when cleaning. The goals for this thesis are fullled and a rst version of the control system with the desired functionallity has been developed. The system needs more work to make sure it works as intended to on a robot. Mainly because tests has only been made on motors that are not yet mounted on a robot. When mounted on the robot there are things like strain and wear on the motors that will aect their functionality.

5 Förord Detta examensarbete valdes för att få en intressant och givande fortsättning på den projektkurs inom robotik som vi båda läst. Innehållet i arbetet verkade också som en passande avslutning på studierna där en blandning av de kunskaper man samlat på sig under åren skulle komma till användning. Examensarbetet ligger till största delen inom området datateknik/robotik men innehåller även en hel del matematik, då framförallt räkning med matriser och vektorer. Examensarbetet ska förhoppningsvis resultera i att det styrsystem som är utvecklat ska vara såpass bra att det efter tester på roboten och komplettering och uppdatering av vissa delar, skulle användas av Ramsta Robotics i deras Clever Cleaner. Målgruppen för denna rapport är personer med viss kunskap inom ämnet datateknik samt personal på Ramsta Robotics. I vissa kapitel har därför beskrivningar av diverse delar uteblivit då läsaren bör vara nog kunnig för att förstå vad som menas. Vi vill passa på att tacka Jörgen Lidholm, doktorand på Mälardalens högskola för den hjälp och de råd han gett oss under examensarbetets gång. Vi vill också tacka våra handledare Lars Asplund, Proessor på Mälardalens högskola, för hans hjälp. Jan Sandberg på Ramsta Robotics för möjligheten till att skriva detta examensarbete och för den tid han ställt upp med trots att vi vet hur upptagen han är. Tack också till Håkan W och Jonas N för intressanta diskussioner inom området! Västerås, Januari 2008

6 Innehåll 1 Inledning Bakgrund Syfte Mål Arbetsmetod Teori Bentligt system Cleaver cleaner Kinematik Direkt kinematik Invers kinematik Automatisk uppbyggnad av yta för tvättning Motorkonverterare Design Programmets design och uppbyggnad Operativsystem Programeringsspråk Dator Motorstyrkort Övrig utrustning Motorer Joystick Lagring Protokoll Simulator Inspelning av rörelse Uppspelning av rörelse Felövervakning

7 4 Resultat Resultat Framtida arbete Slutsatser

8 Kapitel 1 Inledning 1.1 Bakgrund Ramsta Robotics är ett företag baserat i Uppsala. Företaget grundades 1999 och har utvecklat en tvättrobot vars huvudsyfte är att sköta grovrengöring i djurstallar. I framförallt svinhus är arbetsmiljön i samband med tvättning direkt hälsovådlig, bland annat på grund av alla de kväveföreningar som löses ut när vattnet blöter upp de delar av anläggningen som skall tvättas. Detta gör att robotens arbete väsentligt förbättrar arbetsmiljön då ingen behöver vistas i svinhuset i samband med rengöringen. Aärsidén för Ramsta Robotics har varit att utveckla en enkel och prisvärd robot och idag säljs detta system i ett ertal länder i Europa, men robotar har även sålts till bland annat Kanada och Japan. Hittills har över 150 robotar sålts, de esta för att användas i svinproduktion men även fågelproduktion och annan industri har visat sig vara en marknad. Detta examensarbete är ett resultat av ett samarbete mellan Robotdalen, Mälardalens Högskola och Ramsta Robotics och syftar till att ge ett ökat tekniskt innehåll i företagets olika lösningar. 1.2 Syfte Ordet robot kan härledas från det tjeckiska ordet robota som betyder tungt, monotont eller påtvingat arbete. Det nns också ett polskt ord, robotnik, som betyder arbetare. Det var för att ersätta människor i tunga och farliga arbeten som de första in- 5

9 dustrirobotarna utvecklades. Den första kommersiella roboten började användas 1961 på Ford Motor Company, där den kontrollerade en pressgjutningsmaskin. Under den första åren var det dock främst den amerikanska bilindustrin som använde robotar i sin produktion. Robotar utvecklas även idag med syftet, om man bortser från de ekonomiska fördelarna, att förbättra arbetsmiljön för människan. Detta är också anledningen till att Ramsta Robotics utvecklade Clever Cleaner. Att kunna låta en robot göra ett arbete som kan göra en människa sjuk eller skadad är givetvis bra. En robot kommer dessutom göra samma jobb och har heller inget emot att jobba vilken tid som helst under dygnet. Syftet med detta examensarbete är att hjälpa Ramsta Robotics utveckla deras produkt så arbetet med att sköta roboten ytterligare förenklas och eektiviseras. 1.3 Mål Målet med examensarbetet var att skapa en första version av ett styrsystem till Ramsta Robotics, Clever Cleaner. Styrsystemet ska styra armen med invers kinematik. Styrsystemet ska också innehålla en funktion där ett rörelsemönster för en yta kan beräknas endast genom att mata in ytans hörnpunkter till programmet. Alla rörelser som görs ska även kunna spelas in för att senare spelas upp i form av ett tvättprogram. 1.4 Arbetsmetod Projektet började med att vi gjorde en undersökning om hur roboten fungerade i dagsläget, då framförallt hur roboten kontrolleras och hur en tvättning går till. Detta med hjälp av Jan Sandberg på Ramsta Robotics. När vi hade den informationen påbörjades informationssökning på internet och i litteratur, om hur invers kinematik fungerar, om det skulle nnas någon metod att använda och hur man i sånt fall skulle applicera den rent praktiskt på roboten. Hänsyn till metodens komplexitet var också viktigt. Information om hur en EIA-485 bus fungerar undersöktes också då ingen tidigare erfarenhet fanns av den bus-typen. För att testa de olika metoderna för invers kinematik skrevs en simulator där 6

10 en förenklad version av roboten ritades ut med hjälp av OpenGL. Med denna var det sedan enkelt att se och analysera hur bra de olika metoderna fungerade. Eftersom vi inte hade tillgång till motorstyrkorten förrän mot slutet av projektet, ck kommunikationen testas genom att koppla ihop två datorer där den ena ck agera motorstyrkort. När motorstyrkorten var färdiga testades styrprogrammet mot motorerna så kommunikation och programmets övriga funktionalitet fungerade. Vi hoppas även kunna testa med en komplett robot inom kort. Styrprogrammet är huvudsakligen skrivet i Ada men har också vissa funktioner skrivna i C. GNAT användes som kompilator för Ada-koden i projektet, gcc för C-koden. 7

11 Kapitel 2 Teori 2.1 Bentligt system Cleaver cleaner Clever Cleaner är en helautomatisk tvättrobot med huvudsyftet att sköta grovrengöring av svinstallar. [7] Roboten kontrolleras idag av ett styrsystem där man programmerar in ett städschema för roboten med hjälp av en joystick. På joysticken nns en väljare för vilken av lederna som ska kontrolleras, roboten kan med det bentliga styrsystemet endast köra en motor i taget, men med de nya motorstyrkorten samt styrprogrammet kan alla motorer köras simultant. Roboten tvättar sedan automatiskt enligt det inspelade städschemat. Figur 2.1: Clever Cleaner 8

12 Några tekniska specikationer (Nuvarande design) Total bredd: 730 mm, med PVC-hjul 630 mm. Total längd: 2100 mm. Total höjd i hopfällt läge: 1610 mm. Armens maximala-räckvidd: 4250 mm. Eektiv arbetsräckvidd: upp till 6m. Vikt: Totalt 270 kg. Kraftförsörjning: 2 st blyackumulatorer. Elmotorer: 24 volt DC. Hitachi PLC. Punkterna ovan är specikationer på hur roboten ser ut i dagsläget, utan den hårdvara och det styrprogram som framtagits under examensarbetet. 2.2 Kinematik Figur 2.2: Robotens axlar Robotarmen ska styras med invers kinematik. Med invers kinematik menas att man bestämmer hur lederna skall vrida sig för att få slutpunkten att hamna på en given plats. Ramsta robotics tvättrobot består av 5 leder: L1 kan rotera 360 efter y-axeln L2 roterar 45 efter z-axeln 9

13 L3 är en utskjutande axel på 1.7m L4 roterar 360 efter z-axeln L5 roterar 360 efter x-axeln och har en konstant 45 vinkel mot L4 I denna modell har leden L5 förbisetts då det är den som ger den sista vinkeln för spolningen och det är något användaren själv måste se och reglera. Ramsta robotics vill kunna styra roboten med hjälp av en joystick. Tanken är att det är slutpunkten på armen som är den punkt som användaren ska kunna styra med en joystick och alla leder i armen vrider sig på ett sådant sätt att slutpunkten kommer dit. Ramsta Robotics vill även kunna markera ut en yta som roboten ska tvätta. Då beräknas ett ertal målpunkter som armens slutpunkt ska röra sig mellan. Ett viktigt kriterium är att kinematiken vrider lederna så att slutpunkten rör sig i en rak linje till målet, detta är viktigt då roboten går mot en punkt efter en vägg. För att testa kinematiken och komma fram till vilken algoritm som skulle passa bäst i detta system, testades dessa i simulatorn Direkt kinematik För att kunna hitta slutpunkten på armen så används direkt kinematik. Det går ut på att man föryttar en given punkt i ett koordinatsystem som beräknas med rotationsmatriser och translationsmatriser. Värdena i matriserna bestäms av ledernas vinklar och längder. I dessa beräkningar är origo för koordinatsystemet i början av L1. Punkten som ska bestämmas kallas för P: Man sätter upp en matris för varje leds rotation och en för varje leds längd. Sedan multiplicerar man P med dessa matriser så att P transleras ut till armens slutpunkt. Som punktlistan beskriver behövs Y-Rotationsmatrisen, Z-rotationsmatrisen samt translationsmatrisen för att nna P: Nedan beskrivs de olika matriserna x T ranslationsmatris = y z cos 0 sin Y Rotationsmatris = sin 0 cos

14 Z Rotationsmatris = cos sin 0 0 sin cos För att bestämma positionen för P måste den multipliceras med följade matriser: 1. Rotationsmatrisen för L1 rotation: Q1 2. Translationsmatrisen för L1 längd: Q2 3. Rotationsmatrisen för L2 rotation: Q3 4. Translationsmatrisen för L2 längd: Q4 5. Translationsmatrisen för L3 längd: Q5 6. Rotationsmatrisen för L4 rotation: Q6 7. Translationsmatrisen för L4 längd: Q7 Om man multiplicerar ihop alla matriser så får man en slutgiltig matris som beskriver P position cos 2 sin sin 2 cos M = Q1 Q2 Q3 Q4 Q5 Q6 Q7 = cos 1 0 sin sin 1 0 cos l cos 4 sin sin 4 cos l l l = M I denna modell så börjar P alltid i origo O : (0; 0; 0) vilket medför att det är bara kolumn 4 i matrisen M som påverkar P : s föryttning. P : s position kan därför beskrivas enligt följade: 11

15 P x = ( cos 3 cos 2 sin 3 cos 3 sin 2 c2) l2 cos 3 sin 2 l3 cos 3 sin 2 l2; P y = ( sin 2 sin 3 + cos 2 cos 3) l2 + cos 2 l3 + cos 2 l2 + l1; P z = (sin 1 cos 2 sin 3 + sin 1 sin 2 c2) l4 + sin 1 sin 2 l3 + sin 1 sin 2 l2; 12

16 2.2.2 Invers kinematik Med invers kinematik är en målpunkt G:(x,y,z) känd och problemet ligger i att räkna ut vinklarna för lederna och i detta fall även längden på L3 för att P ska nå G. För Ramsta Robotics Clever Cleaner där slutpunkten P ska styras med en joystick, kan man se det som att joysticken yttar punkten G i rummet och med hjälp av invers kinematik räkna ut hur varje led ska förytta sig för att P ska nå sitt mål. Ett krav i rörelsemönstret för armen är att P måste röra sig efter en rak linje. Detta är viktigt då munstycket annars kan gå in i en vägg eller golvet. Det nns två vanliga lösningar för invers kinematik. Den ena är en analytisk lösning. Problem med dessa är att en lösning ofta kan vara omöjliga att få fram. Samt att den kan ge oändligt många lösningar eller ingen lösning. En annan metod är att använda en numerisk lösning där Jacobis metoden kan användas. Dessa kan vara beräkningstunga men man hittar ofta en bra lösning. CCD Vad som sökes är en metod som är snabb men ger ändå bra löningar, en sådan metod är algoritmen Cyclic-Coordinate Descent (CCD) [1]. Den metoden är bra för att den är enkel att implementera samt för att den inte är beräkningstung. CCD ska även vara en modell som passar bra i realtidssystem. CCD algoritmen börjar med sista leden och jobbar sig bakåt, för varje led roteras leden så att vektorn som går från basen av leden till P ligger parallellt med vektorn mellan basen av leden och G. Se bild 2.3. Att beräkna CCD algoritmen Figur 2.3: Cyclic-Coordinate Descent oine så att de okända vinklarna framkommer och sedan applicera vinklarna på motorerna skulle kunna resultera i att armens slutpunkt P inte rör sig efter en rak linje mot målpunkten G utan P skulle med stor sannolikhet röra sig i en kurvig bana. Detta fenomen kommer att bli mer synligt då avståndet mellan P 13

17 och G ökar. Att istället ha en metod som ger små förändringar i lederna och en liten rörelse i P för varje beräkning, och dessutom se till att rörelsen ligger i en vektor mot G. Skulle ge ett resultat som skulle kunna uppfylla kriteriumet att få P att röra sig i en rak linje. En variant av CCD metoden testades. Datorn beräknade då oine hur P:s avstånd till G förändrades när man roterade eller translerade en led i taget så att P gjorde en föryttning. I ett försök i simulatorn beräknades vinkeln för leden så att rotationen skulle ge en föryttning på 1 enhet (1 enhet = 1mm). = arccos(1 dist 2 2 L 2 ) Innan rotationen applicerades på leden beräknades, med hjälp av direkt kinematik, om rotationen skulle resultera i att avståndet mellan P och G minskade. Om avståndet inte minskade så inverterades och ett nytt test gjordes. Om avståndet minskade så applicerades rotationen på leden. Sedan gjordes samma beräkning för nästa led. När led L3 som är en utskjutande led skulle beräknas, så gjordes två tester där leden sköts in/ut en enhet och sedan gjordes samma beräkningar igen. Efter en del tester i simulator framgick det att rörelserna tenderade att bli väldigt hackiga. För att rörelsen skulle bli mjukare lades ett rörelseschema, se bild 2.4, in som beskrev de rörelser som skulle testas med direkt kinematik. I schemat fanns det olika rörelserna som lederna skulle göra, och två leder kunde dessutom röra sig samtidigt. Schemat gjordes med en 2-dimensions array som initierades med rörelserna i början. För varje föryttning gjordes alla rörelser i schemat och med direkt kinematik beräknades den nya slutpunkten P. Sedan beräknades avståndet mellan P och G och resultatet sparades i en lista. Sedan sorterades listan och det bästa resultatet applicerades på lederna. Denna lösning med invers kinematik fungerade ganska tillfredställande i simulatorn, men överlag är det en naiv och inte direkt eektiv lösning då direkt kinematiksberäkningen skulle behöva göras 32 ggr för varje föryttning och denna föryttning ska beräknas minst 10ggr/sekund. Tester för att kontrollera om datorn skulle klara av att göra dessa beräkningar så ofta som nödvändigt utfördes. I testet loopades ett kodstycke som utförde direkt kinematiksberäkningen 100 gånger. För varje gång ändrades vinkeln för lederna så testet skulle vara mer veklighetstroget. Koden kördes sedan i datorn och testet gav att det tog 0,0022 sek att göra 100 beräkningar. Det betyder 14

18 Figur 2.4: Rörelseschema att det skulle ta 0,704 ms att göra 32 beräkningar, och att datorn skulle utan problem klara av tidskravet på 10ggr/sekund. Ett annat problem med denna lösning är att slutpunkten P inte rör sig i en perfekt linje mot målet G varför illustreras med bild 2.5 Figur 2.5: Problem vid användning av CCD Här syns två lösningar för att komma närmare G där P antingen är vid p1 eller p2 som bild 2.5 visar. I detta fall så är det p1 som skulle vara den rätta lösningen för att P ska förytta sig i en rak linje. Men avståndet d2 blir kortare än avståndet d1 och av den anledningen så kommer den rörelsern att appliceras på lederna. Detta ger resultatet att P inte rör sig i en perfekt linje mot G. 15

19 En lösning på Invers kinematiken med vektorgeometri söktes istället. Efter samtal med Lars Asplund om problemet så testades det att med hjälp av derivatan på ledernas rörelser, söka efter vilka rotationer och translationer som skulle appliceras på lederna för att styra P mot G i en rak linje. Genom att derivera formlerna för direkt kinematik ges vilken riktning som en viss led påverkar slutpunkten P. Att derivera formeln P x = ( cos 1 cos 2 sin 3 cos 1 sin 2 c2) l2 cos 1 sin 2 l3 cos 1 sin 2 l2 med avseende på 1 skulle ge vilken påverkan led 1 rotation ger P i x-led. Så att derivera hela formeln för direkt kinematik med avseende på alla vinklar och sedan representera dessa som vektorer, kan man beräkna hur mycket varje led skall rotera för att P ska röra sig korrekt. Exempel på IK lösning Här är ett exempel med två leder med längden 200 l.e. som endast kan rotera runt Z-axeln samt en målpunkt G illustreras lösningen. 1 = 54 = = 66 = 294 Figur 2.6: Exempel med två leder 16

20 Först måste matrisen för direkt kinematik beräknas cos 2 sin sin M = 2 cos cos 2 sin sin 2 cos Även här så börjar P i origo [0,0,0] så när P multipliceras med M så är det endast kolumn 4 som påverkar P :s position. P kan alltså beskrivas enligt: P x = 200 sin 2 cos sin 1 cos sin 1 P y = 200 sin 1 sin cos 1 cos cos 1 P z = 0 Sedan beräknas derivatan för P för att få fram hur ledernas påverkar P : s position. Derivatan för P med avseende på 1 dp x = 200 sin 1 sin cos 1 cos cos 1 1 dp y = 200 cos 1 sin sin 1 cos sin 1 1 dp z 1 = 0 Derivatan för P med avseende på 2 blir: dp x 2 = 200 cos 1 cos sin 1 sin sin 1 dp y 2 = 200 cos 1 sin sin 1 cos cos 1 dp z 2 = 0 Ledernas nuvarande vinklar läggs till i derivatan, och derivatans lösningar representeras som vektorer. Hädanefter så presenteras bara x och y då z inte har 17

21 någon påverkan. V 1 x = 200 sin 306 sin cos 306 cos cos 306 = 17:6 V 1 y = 200 cos 306 sin sin 306 cos sin 306 = 335:0 V 2 x = 200 cos 306 cos sin 306 sin sin 306 = 261:8 V 2 y = 200 cos 306 sin sin 306 cos cos 306 = 290:8 Beräknar! r som är riktningsvektorn mot G.! r normaliseras och multipliceras med med den längd som ska vara den minsta föryttning som P kan göra. I detta fall 20.!!! G P r =!! G P 20 P x = 200 cos 306 sin sin 306 cos sin 306 = 335:0 P y = 200 sin 306 sin cos 306 cos cos 306 = 17:6!! (368:0; 131:0) r =! (368:0; 131:0)! (335:0; 17:6)! (335:0; 17:6) 20 = (5:6; 19:2)! Vektorerna V! 1 V 2! r Ritas in i bild 2.7.! V 1= ( 17:6; 335:0)! V 2= (261:8; 290:8)! r = (5:6; 19:2)! Vektorerna V! 1 V 2 visar riktningen som P har i förhållande till ledernas rotation. Att söka hur mycket varje led ska rotera för att P ska röra sig i! r riktning, kan göras med hjälp av ett ekvationssystem. " # " # " # 17:6 261:8 335:0 290:8 1 2 = 5:6 19:2 18

22 Figur 2.7: Exempel med två leder lösningen till ekvationssystemet med determinanter enlig Cramers regel 1 = det 1 det D = 2 = det 2 det D = 5:6 290:8 19:2 261:8 17:6 290:8 335:0 261:8 = 0: :6 19:2 335:0 5:6 17:6 290:8 335:0 261:8 = 0:0238 Längden på V1 V2 är samma som längden på radien för den axeln, detta ger att svaret på ekvationen är i radianer. För att konvertera radianer till grader multipliceras talen med Detta ger att led 1 ska vrida sig 2:069 och att led 2 ska vrida sig 1:367 för att P ska röra sig en längdenhet mot G i riktningen av! r. 2.3 Automatisk uppbyggnad av yta för tvättning För att förenkla inspelningsprocessen så var ett mål med arbetet att Clever Cleaner ska kunna spola av en yta automatiskt efter det att ytans hörnpunkter har angivits. Ytorna kan representeras av 3 eller 4 punkter som bildar ytan som ska tvättas. Punkterna markeras genom att användaren styr armens slutpunkt P till någon punkt i rummet där den sedan markeras som en hörnpunkt. Yta med tre punkter Om tre punkter representerar ytan så skapas en riktningsvektor Br! mellan p 0 och p 1, som beskriver längs vilken riktning som munstycket ska spola av. En! riktningsvektor Hr mellan p 1 och p 2 som markerar ena kanten och en riktningsvektor V! r mellan p 0 och p 2 som markerar andra kanten. Sedan beräknas ett ertal målpunkter G efter kanterna. Dessa målpunkter sätts upp på så sätt att slutpunkten P på armen kommer att svepa över ytan i ett fördenierat sicksack mönster vilket gör att hela ytan kommer att bli avspolad. Beräkningar för 19

23 Figur 2.8: Yta med 3 punkter Figur 2.9: Tre punkters yta med kantpunkter att hitta alla punkter G, görs med vektogeometri genom att först hitta en riktingsvektor för! r som är vinkelrät mot basvektorn! Br denna beräknas fram med hjälp av två vektorprodukter. Först vektorprodukten mellan basvektorn! Br och kantvektorn! V r. Den vektorn som bildas av vektorprodukten är vinkelrät mot både! Br och! V r dvs. den pekar ut från ytan. Vektorprodukten mellan den nya vektorn och! Br som sedan normaliseras ger den sökta riktingsvektorn.!!! Br V r! Br r =!!! Br V r Br Riktningsvektorn! r används för att hitta avståndet mellan punkterna G på respektive kantvektor. Avståndet beräknas med skalärprodukten mellan! r och den kant där avstånden söks.! dh =! r Hr d dv =!! r V r d 20

24 Yta med fyra punkter En yta kan också representeras med 4 punkter. Med det följer två direkta problem. Den fjärde punkten ligger inte i samma plan som de andra tre. Punkterna måste sättas ut i rätt ordning. Vektorerna som bildas mellan punkterna 0-1, 1-2, 2-3 och 3-0 får inte korsas. Figur 2.10: Två problem med en yta skapad med fyra punkter Det första problemet löses genom att bilda ett plan med de 3 första punkterna och sedan projicera ner den fjärde punkten på planet. För att lösa det andra problemet så används en metod för att beräkna avståndet mellan linjer i rummet. Först skapas en linje L 1 mellan hörnpunkten P 0 och P 1, och en linje L 2 mellan P 2 och P 3. Mellan två linjer i ett rum nns alltid minst en vektor! d som är vinkelrät mot linjerna, se bild 2.11 Den vektorns längd ger det kortaste avståndet mellan linjernal 1 och L 1. För att nna! d så Figur 2.11: Vektorn! d ger avståndet mellan linjerna används egenskapen att skalärprodukten mellan två ortogonala vektorer är noll. 21

25 Ett ekvationssystem sätts upp som söker rätt på den punkt på vardera linje där skalärprodukten är noll. Om avståndet mellan linjerna är nära noll så korsas linjerna. Men två linjer som ligger i samma plan kommer alltid att korsas om de inte ligger helt parallellt i förhållande till varandra. En kontroll måste utföras om korsningen ligger innanför ytan som ska spolas. Detta görs enkelt genom att jämnföra avståndet d 1 mellan punkterna som bildar linjen och avståndet d 2 mellan start punkten och korsnings punkten, se bild Om avståndet till korsningen är mindre än avståndet mellan punkterna så korsas linjerna i ytan och denna punkt uppsättning är inte tillåten. Figur 2.12: Undersöker om korsningen ligger innanför ytans gränser Samma undersökning sker sedan mellan linjerna som bildas av hörnpunkterna P 1 -P 2 och P 3 -P 0. Om det visar sig att två linjer korsas innanför ytan så byter datorn plats på två punkter så att problemet löser sig. Detta gör uppsättning av hörnpunkter mycket lättare för användaren av Clever Cleaner. Uppsättningen för alla målpunkter G efter kanterna på en yta med fyra hörnpunkter görs nästan på samma sätt som en yta med tre hörnpunkter, med den skillnaden att när ena kanten tar slut under uppbyggnaden så byts kanten ut mot en ny kantvektor. Exempel ytan som bestås av punkterna P 0, P 1, P 2 och P 3 så bildas fört bas vektorn P! 0 P 1, sen två kanter! P 1 P 2 skulle ta slut ersätt den kanten med vektorn!! P 1 P 2 och! P 2 P 3. P 0 P 3. Om kanten 22

26 Figur 2.13: G för en yta med fyra hörnpunkter 2.4 Motorkonverterare Värdet som Inversa kinematiken (IK) ger, är den vinkel eller längd som varje led ska förytta sig för att armens slutpunkt P ska röra sig i riktningen mot målpunkten G. Signalen som ska skickas till motorerna är en PWM signal med ett värde mellan +40 till -40. Så det går inte att direkt skicka vinkeln för leden till motorerna utan den måste först konverteras till en hastighet mellan 40. Eftersom alla leder får en vinkel som ska läggas på ledens nuvarande vinkel från IK och det förväntas att alla armar ska ha tagit sig till den nya vinkeln under samma tid, så kan man dra en direkt parallell att värdet som IK ger ut är samma sak som hastigheterna sinsemellan lederna. Dvs. om led 0 får värdet +0.3 och led 1 får värdet 0.6 betyder det att dessa leder ska vrida sig det värdet under samma tid. Man kan säga att led 1 ska röra sig dubbelt så fort som led 0. För att göra en konverterare från grader till PWM signal så behövs en algoritm som konverterar upp den största vinkelförändringen till 40, vilket är PWM-max, och låter de andra vinklarna följa med på ett sådant sätt att förhållandena mellan vinklarna inte förändras. Först söks den vinkel som ska göra största förändringen. V max och så skapas en faktor f av den. f används sedan för att hitta alla motorers PWM värde. f = 1 V max P W M i = f IK i 40 I detta fall så kommer armen att röra sig i max hastighet efter som P W M max kommer att vara 40 hela tiden. Konstanten 40 byts sedan ut mot en variabel mellan 0-40 som styrs av utslaget från joysticken, vilket gör att man får en varierande hastighet för P. 23

27 Kapitel 3 Design 3.1 Programmets design och uppbyggnad Figur 3.1: Schema över hur styrprogrammet är uppbyggt. Mycket tid har lagts vid designen av styrprogrammet för att få den så bra som möjligt innan själva programmeringen påbörjades. Många problem löstes redan vid framtagandet av designen och har underlättat det fortsatta arbetet. Designen och valen av paketen har skett för att göra uppbyggnaden så logisk 24

28 som möjligt och att paketen endast ska vara synliga för andra paket i de fall där det är nödvändigt. Förutom paketen som nns med i designen nns också två denitionsler, Ramsta denes och Motor denes där globala variabler som behöver vara synliga från många paket har placerats. De olika paketens funktionalitet beskrivs mer ingående senare i rapporten. 3.2 Operativsystem Operativsystemet som körs på styrdatorn är i grunden en Linuxkärna med Realtids patchar 1 som ändrar hur operativsystemet beter sig.[3] Patcharna kon- gurerar systemet så att så gott som hela kärnan kan bli avbruten. Systemets klocka patchas också till att bli betydligt mer högupplöst.[4] Genom att använda en Linuxkärna som grund får man tillgång till drivrutiner för grak, TCP/IP och fönstersystem på ett enkelt sätt. Styrprogrammet i detta projekt använder sig till exempel av drivrutiner för joystick samt serieport vilket gör att det passar utmärkt till att köra styrprogrammet på. 3.3 Programeringsspråk Styrprogrammet är i huvudsak skrivet i Ada men också C kod nns för mer hårdvarunära programmering. Den OpenGL simulator som använts för att testa olika IK lösningar är även den skriven i C. Ada är ett modernt programmeringsspråk som är skapat framförallt för att bygga stora långlivade inbyggda system där pålitlighet och eektivitet är viktigt. Ada används därför i dagsläget ofta inom yg och tågindustrin där kraven på säkerhet är höga. Ada är ett starkt typat programmeringsspråk som lämpar sig mycket bra för utveckling av realtidssystem. Det är också enkelt att skapa trådar, även kallade tasks i ada, och att tilldela dessa prioriteter och periodtider. Ada erbjuder också skydd av delade objekt genom att använda typen protected, förhindras automatiskt med hjälp av mutual exclusion, att era tasks kan läsa/skriva samtidigt till objektet. 1 CONFIG PREEMPT RT patch set 25

29 3.4 Dator Datorn som används är en 3,5 EM730 Embedded SBC från AEWIN Technologies 2. Figur 3.2: Den dator som används för att köra styrprogrammet. På bilden är ett trådlöst nätverkskort monterat. Specikationer Processor - Intel Celeron M ULV 600MHz Zero cache processor. Minne MB DDR SO-DIMM, stödjer upp till 1GB. Serieportar - Tre EIA-232 och en EIA-232/485 4/2-tråds serieportar. USB anslutningar - Stödjer fyra USB 2.0/1.1 portar. Det nns också kortplatser för bland annat trådlöst nätverkskort, vilket kan komma att användas för att överföra tvättscheman och annan data från roboten. Systemet drivs av en enkel matningsspänning på 5V och kan operera i temperaturer mellan Systemet har en EIA tråds anslutning som används för att kommunicera med motorkorten. Ändringar som bör göras innan montering på robot Datorn har i sitt grundutförande endast passiv kylning men bör kompletteras med en äkt, alternativt en bättre kyläns då klimatet med säkerhet kommer variera där systemet kommer monteras. Detta för att förlänga komponenternas livslängd

30 3.5 Motorstyrkort Motorkortet är framtaget av SMTeknik och har till uppgift att styra motorn enligt de instruktioner som kommer från styrsystemet.[smteknik] Kortet ska därefter svara med relevanta data om aktuell strömförbrukning, temperatur och motorposition. Varje motordrivkort kan genom styrsystemet ges ett unikt id som då lagras i EEPROM 3. Kortet har en anslutning för 24V matning samt en EIA-485 anslutning för kommunikation. På motorkortet sitter en processor (ATMega8) som hanterar kommunikationen, mäter aktuell motorström och temperatur samt skickar styrsignaler till motordrivkretsen. Signalen till motordrivkretsen innehåller en riktning och en PWM-signal. Drivkretsen svarar sedan för nivåanpassning till FET som styr motorn. Kretsen har även inbyggd skyddslogik för att säkerställa att endast rätt FET är öppen vid varje tillfälle. I motorn sitter en dubbel halleektgivare som ger signal om riktning och en puls per motorvarv. Motostyrkortet gjuts tillsammans med motorn in i polyuretangummi innan det monteras på roboten för att få ett bra skydd mot yttre påverkan av vatten och annat som kan skada komponenterna. Figur 3.3: Motorstyrkortet från SMTeknik. 3 EEPROM - Electrically Erasable Programmable Read-Only Memory är en typ av minne som behåller sin data även om det inte nns någon spänning till kretsen 27

31 3.6 Övrig utrustning Nedan redovisas för ytterligare hårdvara som använts under projektets gång Motorer Motorerna på roboten är 24volts El-motorer och styrs via en H-brygga på motorstyrkortet. På motorerna sitter också en växellåda med utväxlingen 42:1. På motoraxeln sitter en tachometer som möjliggör för motorstyrkortet att avläsa riktning och motorvarvtal Joystick För att kunna styra roboten med det nya styrsystemet har två joysticks använts för att göra styrningen så enkel som möjligt. Två stycken vanliga 3-axliga konsument-joysticks inkopplade via USB har använts under testerna Lagring Ett USB-minne har använts som boot-disk. Operativsystemet läses vid uppstart in till datorns RAM tillsammans med styrprogrammet. USB-minnet används också av styrprogrammet för att lagra de tvättprogram som användaren väljer att spela in. Det minne som har använts under testerna är av storleken 512 MiB, där mindre än 20% upptas av OS samt styrprogrammet. Figur 3.4: Testsetup med dator samt 3 motorer med motorstyrkort. 28

32 3.7 Protokoll Då styrdatorn sköter all kommunikation med styrkorten behövs det ett protokoll för att hantera den. Uppbyggnad av paket och styrkommandon denerades av konstruktören 4 till motorstyrkorten och var därmed det som lade grunden till hur protokollet skulle utformas. Send Byte Innehåll 0 Start 1 Id 2 Order 3 Data 4 Checksumma 5 Slut De order som kan skickas i dagsläget Recieve Order Innehåll Sätt id 10 Nollställ position 20 Utstyrning av PWM 30 Skicka status 40 Byte Innehåll 0 Start 1 Id 2 Position - MSB 3 Position - LSB 4 Ström (10*A) 5 Temperatur (Grader Celcius) 6 Checksumma 7 Slut Programmet har i huvudsak tre paket som hanterar kommunikationen i programmet. Dessa är, Message handler, Communication handler samt Serial ctrl. Message handler Message handler innehåller en array av motorobjekt, av typen protected. Arrayen är av samma storlek som det nns motorer och är till för att lagra det 4 SMTeknik 29

33 senaste datat från varje motor. I paketet nns en tråd vars syfte är att läsa av communication handlers mottagarbuert, för att kontrollera om det nns någon nytt meddelande i den. Om det gör det så läser den av meddelandet och lägger in de nya värdena till den motor meddelandet kom ifrån och uppdaterar även ut-index i mottagarbuerten. Tråden hanterar också eventuella felmeddelanden som skickas i de fall en omsändning har inträat. Message handler innehåller även funktioner för att skapa och placera meddelanden i sändarbuerten. Vid händelse av ett fel kommer tråden att stoppas så att inga nya meddelanden kan skickas till motorerna, i och med det kommer också ett meddelande skickas till alla motorer med ordern att stanna. Om styrprogrammet är i inspelningsläge kommer message handler att skicka alla PWM värden, som ska sändas till motorerna, skickas till recorder också. Communication handler Communication handler sköter sändning och mottagning av paketen till och från motorstyrkortet. Paketet innehåller därför en sänd- och mottagarbuert. Buerterna är arrayer som består av objekt av typen protected, i och med det kommer läsningar och skrivningar skyddas med semaphorer. En tråd sköter sändning och mottagning av meddelanden. Tråden kan sättas i två lägen, sändning eller mottagning. I läget sändning står den alltid, om det inte är så att ett meddelande precis har skickats. I sändning kan två saker inträa. Det första är att den kontrollerar sändbuerten om ut-index ej är detsamma som in-index. Ut-index specicerar på vilken plats ett meddelandet som ska skickas ligger och in-index på vilken plats ett nytt meddelande ska läggas. Om index skiljer sig betyder det att det nns ett nytt meddelande att skicka. Meddelandet skickas en byte i taget via skrivfunktionen som nns i paketet Serial Ctrl. Om meddelandet skickas utan några fel kommer tråden hamna i mottagarläget. I mottagarläget kontrollerar tråden om det kommer något svar över seriebussen. Om det nns någon data så kommer funktionen som läser av meddelandet kontrollera så att det är ett giltigt meddelande som kommer fram. Kontrollen börjar med att se om första byten är en startbyte. Är det en startbyte läses ytterligare sex byte. Sista byten kontrolleras att det är en slutbyte. Checksumman kontrolleras sedan mot det mottagna meddelandet att det är korrekt. Checksumman räknas fram genom att addera värdet på byte 1 till 5 modulo 128 på ett meddelande från motorstyrkortet och värdet på byte 1 till 3 modulo 30

34 128 på ett meddelande från styrdatorn. Det kompletta meddelandet läggs i sånt fall in i mottagarbuerten där indexet uppdateras. Nu kan även ut-index i sändarbuerten uppdateras då en komplett meddelandecykel har ägt rum och inga omsändningar kommer vara nödvändiga. Tråden sätts också till sändningsläget igen. I sändläget kan också ett statusmeddelande, som har till syfte att polla motorerna för att se deras status, skickas till alla motorer. Statusmeddelandet kommer dock inte skickas om det inte är så att motorstyrkortens id är satta och kontrollerade först. Statusmeddelandet kommer alltså alltid sändas till motorerna så länge det inte nns något meddelande i sändarbuerten, då meddelanden i sändarbuerten har högre prioritet. Anledningen till att statusmeddelandet nns är för att säkerställa att det alltid nns relevant data om motorerna tillgänglig. Eftersom det alltid är styrprogrammet som initierar kommunikationen så skulle ingen aktuell data från motorstyrkorten nnas så länge inget nytt meddelande skickas från styrprogrammet. Detta är inte önskvärt då exempelvis en motor kan vara igång utan att styrprogrammet har någon vetskap om det. Omsändning av meddelandet sker om inte någon svar har mottagits inom 12 ms. En felräknare kommer att räknas upp för varje gång en motor inte svarar. Varje motor har en egen felräknare vilken paketet Error övervakar. Felräknaren minskas för varje korrekt svar, men ej mindre än noll. Tiden innan dess att en omsändning sker är vald för att ligga på en god marginal för både tiden det tar innan ett svar i bästa fall kan mottas, samt det tidskravet som nns på sampling av motorerna på 10 ggr/sekund. Vid mottagning av ett meddelande i motorstyrkortet kommer ett svar skickas, efter det att hela meddelandet mottagits, med en fördröjning på minst 50s. Fördröjningen är nödvändig eftersom en växling från sändare till mottagare, först måste ske på styrdatorn. Växlingen har uppmätts till 20s. Överföringshastigheten som används för kommunikationen är 38400bps vilket betyder att tiden för att skicka en bit är s Detta betyder att ett meddelande till motorstyrkortet skickas på = 1560s Meddelande från motorstyrkortet skickas på = 2080s 31

35 Vid mottagning av ett meddelande nns även en felhantering som säkerställer att det inte ligger någon gammal data i UART buerten. Felhanteringen fungerar så att om det visar sig att det nns data i buerten och det inte är startbyte så kommer funktionen läsa av buerten meddelandestorleken+1 gånger. Detta tömmer buerten från skräpdata, alternativt kan bidra till att ett korrekt meddelande kan avläsas. I övriga fall så är buerten tömd tills dess att nästa svar anländer. Serial Ctrl Serial Ctrl är ett paket som sköter öppning och stängning samt skrivning och läsning av serieporten. Paketet är ett interface i ada för att komma åt funktionerna denierade i serial interface, vilka är skrivna i c. 32

36 3.8 Simulator En simulator av roboten med alla dess leder gjordes i OpenGL skrivet i C. Eftersom det inte fanns en robot att testa styrprogrammet på så var skapandet av en simulator nödvändig. Detta för att på ett enkelt sätt kunna provköra kinematiken. Algoritmer för invers kinematik skrevs först i C och applicerades sedan direkt mot simulatorn. På det sättet var det tydligt om teorin för algoritmen fungerade. När algoritmen fungerade tillfredställande i simulatorn så skrevs koden om till att fungera i Ada. För att testa att algoritmen även fungerade i Ada så gjordes en server i C simulatorn och i Ada så skrevs en klient i paketet message handler. Med detta så kunde Ada koden köras och skicka data via sockets till simulatorn. Detta gjorde så att paket som IK, Surface, och motor converter kunde testas mot OpenGL simulatorn. Figur 3.5: Simulator gjord i OpenGL 33

37 3.9 Inspelning av rörelse Inspelning av robotarmens rörelse sker i paketet recorder. En inspelning aktiveras av operatören. Recorder skapar alternativt skriver över en ny programl till vilken motorernas PWM värde sparas. Recorder innehåller en array som är ett protected object vilken är till för att spara PWM-värden som skickats till motorerna. Om systemet är i inspelningsläget kommer message handler uppdatera arrayen varje gång den mottagit en ny PWM som ska skickas till motorerna. I recorder nns en task som exekveras 10ggr/sekund[Sandberg], för varje exekvering sparas värdet på PWM för varje motor, som då ligger i PWM-arrayen till l. Programlen lagras på USB-Minnet. Storleken av PWM är en byte. En inspelning kan tidsmässigt variera mycket men på grund av att det med 6 stycken motorer där PWM lagras 10 ggr/sekund endast ger en lstorlek på = :94M B, för ett program där inspelningen varar i 24 timmar, så spelar längden på inspelningen ingen roll. Vanligtvis är en inspelning betydligt kortare än så. Fördelen med detta är dock att man kan lagra många olika program, för olika typer av tvättningar med mera, utan att det nns någon som helst risk till att fylla minnet Uppspelning av rörelse Uppspelning av robotarmens rörelse sker i paketet player. En uppspelning startas av operatören och består i första versionen av styrprogrammet endast av en förenklad uppspelning där inte val av tvättprogram med mera inte är implementerat. I player ligger en task där läsning från en programl sker i samma frekvens som skrivning till len skett med, i det här fallet 10ggr/sekund. Player läser PWM för alla inkopplade motorer från len och skickar sedan iväg dessa till motorerna och stannar sedan när den når slutet av programlen Felövervakning Ett paket för felhantering är nödvändig för att detektera fel i kommunikationen samt kontrollera så att motorernas strömförbrukning eller temperaturer inte hamnar på för höga nivåer. Error innehåller en tråd som avläser motorvärdena som ligger lagrade i motorarrayen i message handler. Om något fel upptäcks kommer en agga sättas i ett skyddat objekt i error vilket main ständigt läser av och sedan hanterar det om ett fel har uppstått. 34

38 Det som kontrolleras är alltså Error Temperatur Strömförbrukning Varje motor innehåller en variabel Error där värdet beror på hur många felsändningar den aktuella motorn har genererat. Error adderas med ett vid en eventuell omsändning och subtraheras med ett om en sändning har varit lyckad. Detta gör att man kan anse att en motor är defekt om error uppgår till ett visst värde. Hur stort detta värde bör vara måste testas fram beroende på hur systemet beter sig på roboten. Att en felsändning sker är inte helt ovanligt och kan exempelvis bero på yttre störningar, kabeltyp m.m, vilket gör att meddelandet blir felaktigt. Då inga tester utförts på roboten, nns därför inget gränsvärde framtaget i dagsläget. Skulle man löst det så att error inte hade subtraherats när en sändning lyckats skulle varje motor anses som defekt efter en viss tid. Strömförbrukningen på varje motor är också något som kontrolleras, men är i detta fall speciell. På roboten används motorernas strömförbrukning både till att veta när en led benner sig i ett ändläge eller vid eventuell kollision, då en ökad strömförbrukning är en eekt av det. Men också till att upptäcka om något är fel med motorn. Om problemet orsakas av att leden står i ändläget bör felet hanteras på ett annat sätt, då det ju egentligen inte är ett fel. För att skilja på dessa är en lösning att med hjälp av motorpositionen kontrollera om leden benner sig i närheten av ändläget, då man kan anta att om motorerna då får en ökad strömförbrukning, betyder det att leden med största sannolikhet står i ändläget. På det sättet kan felen separeras och hanteras korrekt. Blir temperaturen på korten för hög är det något som inte står rätt till, alternativt är temperaturen där roboten arbetar för hög. I båda fall måste felet hanteras för att inte riskera att motorstyrkort eller motorer blir defekta på grund av överhettning. 35

39 Kapitel 4 Resultat 4.1 Resultat Clever Cleaner kan med det framtagna styrsystemet kontrolleras med manuell styrning, där armen kontrolleras genom att styra varje motor för sig. Skillnaden mot det gamla styrsystemet är att man vid manuell styrning kan styra alla motorer samtidigt. Motorerna styrs med hjälp av de nyframtagna motorstyrkorten. Clever Cleaner kan också styras med hjälp av invers kinematik vilket beskrivs mer utförligt nedan. Styrsystemet körs på ett inbyggt system där en patchad version av Linux används som realtids-operativsystem, och som kommunicerar med motorstyrkorten via en EIA-485 serieport. Styrprogrammet kommunicerar hela tiden med motorerna för att se till att de senaste värdena från motorerna alltid nns tillgängligt. Anledningen till detta är förutom att beräkningarna av inversa kinematiken blir mer korrekt ju mer aktuella värden som används, också att så snabbt som möjligt upptäcka om temperatur eller strömförbrukning är utanför de rekommenderade värdena. Styrsystemet har en enkel felhantering för att hantera temperatur och strömförbrukning, men också hur kommunikationen mot motorstyrkortet fungerar. Ytgenererare En funktion för att efter inmatning av tre eller fyra punkter skapa ett rörelsemönster för att tvätta ytan punkterna spänner upp nns och är testad med gott resultat i simulatorn. 36

40 Inspelning och Uppspelning En enkel version av inspelning av armrörelsen nns implementerad i styrprogrammet. Inspelningen spelar in alla PWM som skickas till motorerna med en frekvens av 10 gånger/sekund. För tillfället kan endast ett program spelas in åt gången. Applicera kinematik lösningen på Clever Cleaner Eftersom Clever Cleaner har 4 leder som ska styras med hjälp av invers kinematik och led 3 påverkar slutpunkten P i samma plan som led 2 och led 4, så skulle en direkt applicering av ovanstående lösning ge oändligt många lösningar av ekvationen. En enkel och fungerande lösning på problemet är att räkna ut led 3 :s rörelse före och sedan med hänsyn till denna rörelse räkna ut led 1 led 2 och led 4 rörelser med metoden som presenterades ovan. Att räkna ut led 3 rörelse görs enklast med led 3 rörelsevektor! v 3 som fås av derivatan för P med avseende på l 3 och vektorn! r mellan P och målpunkten G. Genom att normalisera dessa och sedan ta skalärprodukten ges ett tal mellan 0 och 1. Där 0 betyder att led 3 rörelse inte skulle göra någon påverkan till att P skulle komma närmare G. Om det skulle bli 1 betyder det istället att led 3 rörelse ger en direkt föryttning för P mot G. Sedan multipliceras talet med en längdenhet. Figur 4.1: Beräkna L3:s rörelse Som bilden till vänster i gur 12.8 visar så blir skalärprodukten 1 då riktningsvektorerna har samma riktning. På bilden till höger blir skalärprodukten 0 då riktningsvektorerna istället har en 90 vinkel mellan sig.!! G P l 3 = jg P j dp 3 l När denna beräkning är klar så beräknas rotationerna för resterande leder. Först beräknas derivatan fram. Derivatan är beräknad i förhand och är därefter en konstant formel i koden. Derivatan för P med avseende på l 3 : 37

41 dp x 1 = (sin 1 cos 2 sin 4 + sin 1 sin 2 cos 4 ) len3 + sin 1 sin 2 armp os(2) + sin 1 sin 2 len1 dp y 1 = 0 dp z 1 = (cos 1 cos 2 sin 4 + cos 1 sin 2 cos 4 ) len3 + cos 1 sin 2 armp os(2) + cos 1 sin 2 len1 Derivatan för P med avseende på 2 : dp x = (cos 2 1 sin 2 sin 4 cos 1 cos 2 cos 4 ) len3 cos 1 cos 2 armp os(2) cos 1 cos 2 len1 dp y 2 = ( cos 2 sin 4 sin 2 cos 4 ) len3 sin 2 armp os(2) sin 2 len1 dp z 2 = ( sin 1 sin 2 sin 4 + sin 1 cos 2 cos 4 ) len3 + sin 1 cos 2 armp os(2) + sin 1 cos 2 len1 Derivatan för P med avseende på 3 : dp x 2 = cos 1 sin 2 dp y 2 = cos 2 dp z 2 = sin 1 sin 2 ; Derivatan för P med avseende på 4 : dp x 2 = ( cos 1 cos 2 cos 4 + cos 1 sin 2 sin 4 ) len3 dp y 2 = ( sin 2 cos 4 cos 2 sin 4 ) len3; dp z 2 = (sin 1 cos 2 cos 4 sin 1 sin 2 sin 4 ) len3 Riktningsvektorn! r mellan P och G beräknas, normaliseras och multipliceras med en längenhet. Men här måste också rörelsen som led3 gjorde dras av från! r. Annars skulle P förytta sig mer än en längdenhet åt gången, vilket skulle resultera i att armen skulle röra sig snabbare i vissa situationer.!!! r = G P!! G P l Sedan så ska ekvation systemet för armrörelserna för led 1, led 2, och led 4 sättas upp och beräknas. 38