Input: 
MoveOn : BOOL;



Description:
  1. MoveToTarget is the main program for the actual operation of the cable robot. Here, one defines the set-point in the 3D space where the end-effector is intended to move.
  2. Based on the starting position and set-point, the program will define a theoretical cable length and then calculate new positions from this. The starting position for each motor is then determined using forward kinematics, and finally, the correct speed is sent out to each motor.
  3. During the operation of the function block, a PID controller is also implemented that calculates a coefficient, which is ultimately multiplied by the output speed of each motor. This controller receives the deviation in cable length as input and adjusts the speed based on how far it is from the target.



Type: Function

Responsible: Magnus, 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



  • No labels

Industrielle styresystemer - NTNU Ålesund - Automasjon