Industrielle styresystemer - Wiki
You are viewing an old version of this page. View the current version.
Compare with Current View Page History
« Previous Version 8 Next »
MoveOn : BOOL;Description:
Type: Function
Responsible: Kenneth
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