24_7 MoveToTarget

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