Stap 6: kinematica
Voor deze stap zullen we een tweede tabblad toegevoegd aan de schets waar de inverse kinematica en bewegingen worden berekend. Voor deze tutorial hergebruikt ik een deel van de code van mijn viervoeter robot. Kortom, de IK() functie omgezet in drie coördinaten drie hoeken. SetTime() genereert een tijdwaarde van 0 tot en met 4. WritePos() beide functies aanroept en genereert een sinus-functie die eruit als een horizontale acht ziet, waardoor voor soepele bewegingen van de robot.
Het enige wat dat we moeten veranderen in het tabblad belangrijkste schets is het aanroepen van de functie writePos(). Als je kijkt naar de code in het tweede tabblad, kan het gemakkelijk worden uitgevoerd op een arduino zonder aanpassingen. Dit is wat ik deed met mijn viervoeter simulator. Ik testte de code en later het hele ding naar mijn Arduino schets gekopieerd.
Hoofdtabblad
PShape base, shoulder, upArm, loArm, end; float rotX, rotY; float posX=1, posY=50, posZ=50; float alpha, beta, gamma; void setup(){ size(1200, 800, OPENGL); base = loadShape("r5.obj"); shoulder = loadShape("r1.obj"); upArm = loadShape("r2.obj"); loArm = loadShape("r3.obj"); end = loadShape("r4.obj"); shoulder.disableStyle(); upArm.disableStyle(); loArm.disableStyle(); } void draw(){ writePos(); background(32); smooth(); lights(); fill(#FFE308); noStroke(); translate(width/2,height/2); rotateX(rotX); rotateY(-rotY); scale(-4); translate(0,-40,0); shape(base); translate(0, 4, 0); rotateY(gamma); shape(shoulder); translate(0, 25, 0); rotateY(PI); rotateX(alpha); shape(upArm); translate(0, 0, 50); rotateY(PI); rotateX(beta); shape(loArm); translate(0, 0, -50); rotateY(PI); shape(end); } void mouseDragged(){ rotY -= (mouseX - pmouseX) * 0.01; rotX -= (mouseY - pmouseY) * 0.01; } Inverse Kinematics-tabblad
float F = 50; float T = 70; float millisOld, gTime, gSpeed = 4; void IK(){ float X = posX; float Y = posY; float Z = posZ; float L = sqrt(Y*Y+X*X); float dia = sqrt(Z*Z+L*L); alpha = PI/2-(atan2(L, Z)+acos((T*T-F*F-dia*dia)/(-2*F*dia))); beta = -PI+acos((dia*dia-T*T-F*F)/(-2*F*T)); gamma = atan2(Y, X); } void setTime(){ gTime += ((float)millis()/1000 - millisOld)*(gSpeed/4); if(gTime >= 4) gTime = 0; millisOld = (float)millis()/1000; } void writePos(){ IK(); setTime(); posX = sin(gTime*PI/2)*20; posZ = sin(gTime*PI)*10; }
