Versions Compared

Old Version 9

changes.mady.by.user Kenneth Brenna

Saved on

compared with

New Version Current

changes.mady.by.user Kenneth Brenna

Saved on

Key

  • This line was added.
  • This line was removed.
  • Formatting was changed.
Input: 
MoveOn : BOOL;



Description:
  1. 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.
  2. 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.  
  3. 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.
  4. 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.
  5. 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 targetUnder 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.



Info

Type: Function

Responsible: Magnus, Kenneth


Code Block
languageactionscript3
titleCode Overview
linenumberstrue
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