Skapa en Robotics simulator med bearbetning (6 / 7 steg)
Steg 6: kinematik
För detta steg kommer vi lägga till en andra fliken på skissen om omvänd kinematik och rörelser beräknas. För denna tutorial återanvändas jag några av koden från min fyrfotadjur robot. I grund och botten konverterar funktionen IK() tre koordinater till tre vinklar. SetTime() genererar ett värde från 0 till 4. WritePos() kräver både funktioner och genererar en sinusfunktion som ser ut som en horisontell åtta, för mjuka rörelser av roboten.
Det enda vi behöver ändra på fliken huvudsakliga skiss är att anropa funktionen writePos(). Om du tittar på koden i den andra fliken, kan den lätt köras på en arduino utan ändringar. Detta är vad jag gjorde med min fyrfotadjur simulator. Jag testade koden och senare kopierade hela saken till min Arduino skiss.
Huvudfliken
PShape bas, axel, upArm, loArm, slutet;
float rotX, rotY;
flyta posX = 1, posY = 50, posZ = 50;
float alfa, beta, gamma;
void setup() {
storlek (1200, 800, OPENGL);
bas = loadShape("r5.obj");
Axel = loadShape("r1.obj");
upArm = loadShape("r2.obj");
loArm = loadShape("r3.obj");
slut = 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);
Översätt (0, 4, 0);
rotateY(gamma);
Shape(Shoulder);
Översätt (0, 25, 0);
rotateY(PI);
rotateX(alpha);
Shape(upArm);
Översätt (0, 0, 50);
rotateY(PI);
rotateX(beta);
Shape(loArm);
Översätt (0, 0, -50);
rotateY(PI);
Shape(End);
}
void mouseDragged() {
rotY-= (mouseX - pmouseX) * 0,01;
rotX-= (mouseY - pmouseY) * 0,01;
}
Omvänd kinematik fliken
flyta F = 50;
flottör T = 70.
flyta millisOld, gTime, gSpeed = 4;
void IK() {
float X = posX;
float Y = bukett;
float Z = posZ;
float L = sqrt(Y*Y+X*X);
flyta dia = sqrt(Z*Z+L*L);
alpha = PI/2-(ARCTAN2 (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 = ARCTAN2 (Y, X);
}
void setTime() {
gTime += ((float) millis () / 1000 - millisOld)*(gSpeed/4);
om (gTime > = 4) gTime = 0;
millisOld = (float) millis () / 1000;
}
void writePos() {
IK();
setTime();
posX = synd (gTime * PI/2) * 20;
posZ = sin(gTime*PI) * 10;
}