MoveToTarget er hovedprogrammet for selve kjøringen av kabelroboten. Her definerer man set-punktet i 3D-rommet man ønsker at end-effectoren skal gå til.
Ut fra startposisjon og set-punkt vil programmet definere en teoretisk kabellengde, og deretter kalkulere nye posisjoner ut fra dette. Man vil så finne utgangsposisjon for hver motor ved forward kinematic, og til slutt sende ut riktig hastighet til hver motor.
Under kjøringen av funksjonsblokken implementere man også en PID-regulator kalkulerer en koeffisient, som til sluttes multipliseres med utgangshastigheten til hver motor. Denne kontrolleren får avviket i kabellengde som inngang, og justere hastigheten ut fra hvor langt den er fra målet.
Type: Function
Responsible: Kenneth
Code Overview
FUNCTION_BLOCK MoveToTarget
VAR_INPUT
MoveOn : BOOL;
END_VAR
VAR
tRatio : LREAL := 200;
mySpeed : LREAL := 15;
x : LREAL := 80.0;
y : LREAL := 30.0;
z : LREAL := 65.0;
x2 : LREAL;
y2 : LREAL;
z2 : LREAL;
tempSum : LREAL := 0;
deltaTorque : LREAL;
// Step 1:
offsets : ARRAY [0..7] OF LREAL;
i : INT := 0;
j : INT := 0;
// Step 2: theoretical cable lengths
myRopeInitials : ARRAY[0..7] OF LREAL;
averageTorque : ARRAY [0..7] OF LREAL;
// Step 3:
myAxis : ARRAY [0..7] OF AXIS_REF_SM3;
// Step 4:
myFK : ARRAY [0..1] OF ARRAY [0..2] OF LREAL;
myPos : ARRAY [0..2] OF LREAL;
// Step 5: Setpoint and combine
myTarget : ARRAY [0..2] OF LREAL := [70,52,50];
myMotorSpeeds : ARRAY [0..7] OF LREAL;
myMultipliedSpeeds : ARRAY [0..7] OF LREAL;
myTorque : ARRAY [0..7] OF LREAL;
myLastTorqueValues : ARRAY [0..7] OF ARRAY [0..49] OF LREAL;
absoluteMaxTorque : LREAL := 100;
excessTorqueWeight : LREAL := 0.01;
count : INT := 0;
END_VAR
IF MoveOn THEN
// 1. First get motors offset, runs once
(*
IF count = 0 THEN
FOR i := 0 TO 7 DO
offsets[i] := myAxis[i].fActPosition;
END_FOR
count := 1;
END_IF
*)
myAxis[0] := Axis1;
myAxis[1] := Axis2;
myAxis[2] := Axis3;
myAxis[3] := Axis4;
myAxis[4] := Axis5;
myAxis[5] := Axis6;
myAxis[6] := Axis7;
myAxis[7] := Axis8;
IF count >= 49 THEN
count := 0;
END_IF
FOR i := 0 TO 7 DO
myLastTorqueValues[i][count] := myAxis[i].fActTorque;
END_FOR
FOR i := 0 TO 7 DO
tempSum := 0;
FOR j := 0 TO 49 DO
tempSum := tempSum + myLastTorqueValues[i][j];
END_FOR
averageTorque[i] := tempSum/49;
END_FOR
count := count + 1;
offsets[0] := globals.motorOffsetsZero[0];
offsets[1] := globals.motorOffsetsZero[1];
offsets[2] := globals.motorOffsetsZero[2];
offsets[3] := globals.motorOffsetsZero[3];
offsets[4] := globals.motorOffsetsZero[4];
offsets[5] := globals.motorOffsetsZero[5];
offsets[6] := globals.motorOffsetsZero[6];
offsets[7] := globals.motorOffsetsZero[7];
// 2. Get theoretical rope positions
myRopeInitials := myInverseKinematics(
targetPosX := 5.0,
targetPosY := 5.0,
targetPosZ := 5.3);
// 3. Now calculate the position based on the adjusted cable_length
FOR i := 0 TO 3 DO
myRopeInitials[i] := (myAxis[i].fActPosition - offsets[i]) * 0.103 + (myRopeInitials[i]); (* 160 is one revolution and each revolution gives 10cm rope*) //0.1038656
END_FOR
FOR i := 4 TO 7 DO
myRopeInitials[i] := (myAxis[i].fActPosition - offsets[i]) * 0.103 + (myRopeInitials[i]); (* 160 is one revolution and each revolution gives 10cm rope*) //0.1038656
END_FOR
// 4. FK (NEED TO MEASURE WHAT the position is equal to in cm or our unit)
myFK := trilaterate(r1 := myRopeInitials[0], r2 := myRopeInitials[1], r3 := myRopeInitials[4]);
myPos := myFK[1];
// 5. Combine
myMotorSpeeds := calculate_speed(startPoint := myPos, endPoint := myTarget);
FOR i := 0 TO 7 DO
IF myMotorSpeeds[i] > 1 OR myMotorSpeeds[i] < -1 THEN
myMotorSpeeds[i] := 0.0;
END_IF
myMultipliedSpeeds[i] := myMotorSpeeds[i] * mySpeed;
END_FOR
FOR i := 1 TO 7 DO (* Removed motor 1 torque control because motor 1s torque sensor is big time fucked *)
IF ABS(averageTorque[i]) > absoluteMaxTorque THEN
deltaTorque := (ABS(averageTorque[i])-absoluteMaxTorque);
tRatio := 200;
IF myMultipliedSpeeds[i] > 0 THEN // POSITIVE - HENCE GIVE OUT MORE ROPE
myMultipliedSpeeds[i] := myMultipliedSpeeds[i] * (1 + (deltaTorque/tRatio));
ELSE // NEGATIVE
IF deltaTorque > tRatio THEN
tRatio := deltaTorque;
END_IF
myMultipliedSpeeds[i] := myMultipliedSpeeds[i] * (1 - (deltaTorque/tRatio));
END_IF
END_IF
END_FOR
globals.motorSpeeds := myMultipliedSpeeds;
END_IF