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
|