Industrielle styresystemer - Wiki
You are viewing an old version of this page. View the current version.
Compare with Current View Page History
Version 1 Current »
targetPosX : LREAL;targetPosY : LREAL;targetPosZ : LREAL;
Output:
myInverseKinematicsOffsets: ARRAY [0..7] OF LREAL
Type: Function
Responsible: Magnus, Simen
New improved version of 24_7 Inverse Kinematics (Function)!
FUNCTION myInverseKinematicsOffsets : ARRAY [0..7] OF LREAL (* Calculates inverse kinematics for all 8 motors, returns 8 rope lengths *) VAR_INPUT targetPosX : LREAL; targetPosY : LREAL; targetPosZ : LREAL; END_VAR VAR i : INT := 0; motor_positions : ARRAY [0..7] OF ARRAY [0..2] OF INT; deltaX, deltaY, deltaZ : LREAL; offsets : ARRAY [0..7] OF ARRAY [0..2] OF LREAL; END_VAR motor_positions[0][0] := 0; motor_positions[0][1] := 0; motor_positions[0][2] := 0; motor_positions[1][0] := 138; motor_positions[1][1] := 0; motor_positions[1][2] := 0; motor_positions[2][0] := 138; motor_positions[2][1] := 102; motor_positions[2][2] := 0; motor_positions[3][0] := 0; motor_positions[3][1] := 102; motor_positions[3][2] := 0; motor_positions[4][0] := 0; motor_positions[4][1] := 0; motor_positions[4][2] := 95; motor_positions[5][0] := 138; motor_positions[5][1] := 0; motor_positions[5][2] := 95; motor_positions[6][0] := 138; motor_positions[6][1] := 102; motor_positions[6][2] := 95; motor_positions[7][0] := 0; motor_positions[7][1] := 102; motor_positions[7][2] := 95; offsets[0][0] := -2.1213; offsets[0][1] := -2.1213; offsets[0][2] := -5.0; offsets[1][0] := 2.1213; offsets[1][1] := -2.1213; offsets[1][2] := -5.0; offsets[2][0] := 2.1213; offsets[2][1] := 2.1213; offsets[2][2] := -5.0; offsets[3][0] := -2.1213; offsets[3][1] := 2.1213; offsets[3][2] := -5.0; offsets[4][0] := 0.0; offsets[4][1] := -3.0; offsets[4][2] := 5.0; offsets[5][0] := 3.0; offsets[5][1] := 0.0; offsets[5][2] := 5.0; offsets[6][0] := 0.0; offsets[6][1] := 3.0; offsets[6][2] := 5.0; offsets[7][0] := -3.0; offsets[7][1] := 0.0; offsets[7][2] := 5.0; i := 0; FOR i := 0 TO 7 DO deltaX := ABS(motor_positions[i][0] - targetPosX + offsets[i][0]); deltaY := ABS(motor_positions[i][1] - targetPosY + offsets[i][1]); deltaZ := ABS(motor_positions[i][2] - targetPosZ + offsets[i][2]); myInverseKinematicsOffsets[i] := SQRT(deltaX*deltaX + deltaY*deltaY + deltaZ*deltaZ); // Funker ikke med POW eller ** END_FOR