Versions Compared

Old Version 8

changes.mady.by.user Kenneth Brenna

Saved on

compared with

New Version 9

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. 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.



Info

Type: Function

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