Input: 
on : BOOL := FALSE;
off : BOOL := FALSE;

Output:

PowerDone : BOOL := FALSE;

Type: Function

Responsible: Magnus


A main function block made for controlling everything with the motors. There have been many iterations of this function block over time. At first we started out with a setup for absolute position movement, but the final logic is setup for velocity control. This change was made because to continuously move the motors towards a setpoint, that can move, it needs to run in velocity to avoid stepping. This function block consists of a lot of mandatory code, that is not very advanced, just a lot that has to be setup. Basically the code powers the motors, for then to set them in a continuous update state listening to global changes to the motor velocity.


Code Overview 
FUNCTION_BLOCK PowerMotor
VAR_INPUT
	on : BOOL := FALSE;
	off : BOOL := FALSE;
END_VAR
VAR
    speed1 : LREAL := 0;
    speed2 : LREAL := 0;
    speed3 : LREAL := 0;
    speed4 : LREAL := 0;
    speed5 : LREAL := 0;
    speed6 : LREAL := 0;
    speed7 : LREAL := 0;
    speed8 : LREAL := 0;
END_VAR	
	
VAR_STAT
    prev_speed1 : LREAL := 0;
    prev_speed2 : LREAL := 0;
    prev_speed3 : LREAL := 0;
    prev_speed4 : LREAL := 0;
    prev_speed5 : LREAL := 0;
    prev_speed6 : LREAL := 0;
    prev_speed7 : LREAL := 0;
    prev_speed8 : LREAL := 0;

    acc1 : LREAL := 0;
    acc2 : LREAL := 0;
    acc3 : LREAL := 0;
    acc4 : LREAL := 0;
    acc5 : LREAL := 0;
    acc6 : LREAL := 0;
    acc7 : LREAL := 0;
    acc8 : LREAL := 0;

    pwr1 : BOOL := FALSE;
    pwr2 : BOOL := FALSE;
    pwr3 : BOOL := FALSE;
    pwr4 : BOOL := FALSE;
    pwr5 : BOOL := FALSE;
    pwr6 : BOOL := FALSE;
    pwr7 : BOOL := FALSE;
    pwr8 : BOOL := FALSE;

    iStatus: INT := 0;
    iStatus2 : INT := 0;
    iStatus3 : INT := 0;
    iStatus4 : INT := 0;
    iStatus5 : INT := 0;
    iStatus6 : INT := 0;
    iStatus7 : INT := 0;
    iStatus8 : INT := 0;

    Power: MC_Power;
    Power2 : MC_Power;
    Power3 : MC_Power;
    Power4 : MC_Power;
    Power5 : MC_Power;
    Power6 : MC_Power;
    Power7 : MC_Power;
    Power8 : MC_Power;

    MoveVelocity : MC_MoveVelocity;
    MoveVelocity2 : MC_MoveVelocity;
    MoveVelocity3 : MC_MoveVelocity;
    MoveVelocity4 : MC_MoveVelocity;
    MoveVelocity5 : MC_MoveVelocity;
    MoveVelocity6 : MC_MoveVelocity;
    MoveVelocity7 : MC_MoveVelocity;
    MoveVelocity8 : MC_MoveVelocity;

    Reset1 : MC_Reset;
    Reset2 : MC_Reset;
    Reset3 : MC_Reset;
    Reset4 : MC_Reset;
    Reset5 : MC_Reset;
    Reset6 : MC_Reset;
    Reset7 : MC_Reset;
    Reset8 : MC_Reset;

    p:REAL:=100;

END_VAR
VAR_OUTPUT
	PowerDone : BOOL := FALSE;

END_VAR

speed1 := globals.motorSpeeds[0];
speed2 := globals.motorSpeeds[1];
speed3 := globals.motorSpeeds[2];
speed4 := globals.motorSpeeds[3];
speed5 := globals.motorSpeeds[4];
speed6 := globals.motorSpeeds[5];
speed7 := globals.motorSpeeds[6];
speed8 := globals.motorSpeeds[7];


acc1 := globals.motorAcc[0];
acc2 := globals.motorAcc[1];
acc3 := globals.motorAcc[2];
acc4 := globals.motorAcc[3];
acc5 := globals.motorAcc[4];
acc6 := globals.motorAcc[5];
acc7 := globals.motorAcc[6];
acc8 := globals.motorAcc[7];


pwr1 := globals.motorPwr[0];
pwr2 := globals.motorPwr[1];
pwr3 := globals.motorPwr[2];
pwr4 := globals.motorPwr[3];
pwr5 := globals.motorPwr[4];
pwr6 := globals.motorPwr[5];
pwr7 := globals.motorPwr[6];
pwr8 := globals.motorPwr[7];


IF on AND NOT(off) THEN
CASE iStatus OF

// initialization of the axis
0:
        Power(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis1);
        IF Power.Status THEN
                iStatus := iStatus + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed1 <> prev_speed1 THEN
			MoveVelocity(Execute:=FALSE, Axis:=Axis1);
		END_IF
		IF speed1 > 0 THEN
			MoveVelocity(Execute:=TRUE, Velocity:=speed1, Acceleration:=acc1, Deceleration:=100, Axis:=Axis1, Direction:=positive);
		ELSE
			MoveVelocity(Execute:=TRUE, Velocity:=-speed1, Acceleration:=acc1, Deceleration:=100, Axis:=Axis1, Direction:=negative);
		END_IF
END_CASE

CASE iStatus2 OF

// initialization of the axis
0:
        Power2(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis2);
        IF Power2.Status THEN
                iStatus2 := iStatus2 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed2 <> prev_speed2 THEN
			MoveVelocity2(Execute:=FALSE, Axis:=Axis2);
		END_IF
		IF speed2 > 0 THEN
			MoveVelocity2(Execute:=TRUE, Velocity:=speed2, Acceleration:=acc2, Deceleration:=100, Axis:=Axis2, Direction:=positive);
		ELSE
			MoveVelocity2(Execute:=TRUE, Velocity:=-speed2, Acceleration:=acc2, Deceleration:=100, Axis:=Axis2, Direction:=negative);
		END_IF
END_CASE

CASE iStatus3 OF

// initialization of the axis
0:
        Power3(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis3);
        IF Power3.Status THEN
                iStatus3 := iStatus3 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed3 <> prev_speed3 THEN
			MoveVelocity3(Execute:=FALSE, Axis:=Axis3);
		END_IF
		IF speed3 > 0 THEN
			MoveVelocity3(Execute:=TRUE, Velocity:=speed3, Acceleration:=acc3, Deceleration:=100, Axis:=Axis3, Direction:=positive);
		ELSE
			MoveVelocity3(Execute:=TRUE, Velocity:=-speed3, Acceleration:=acc3, Deceleration:=100, Axis:=Axis3, Direction:=negative);
		END_IF
END_CASE

CASE iStatus4 OF

// initialization of the axis
0:
        Power4(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis4);
        IF Power4.Status THEN
                iStatus4 := iStatus4 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed4 <> prev_speed4 THEN
			MoveVelocity4(Execute:=FALSE, Axis:=Axis4);
		END_IF
		IF speed4 > 0 THEN
			MoveVelocity4(Execute:=TRUE, Velocity:=speed4, Acceleration:=acc4, Deceleration:=100, Axis:=Axis4, Direction:=positive);
		ELSE
			MoveVelocity4(Execute:=TRUE, Velocity:=-speed4, Acceleration:=acc4, Deceleration:=100, Axis:=Axis4, Direction:=negative);
		END_IF
END_CASE

CASE iStatus5 OF

// initialization of the axis
0:
        Power5(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis5);
        IF Power5.Status THEN
                iStatus5 := iStatus5 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed5 <> prev_speed5 THEN
			MoveVelocity5(Execute:=FALSE, Axis:=Axis5);
		END_IF
		IF speed5 > 0 THEN
			MoveVelocity5(Execute:=TRUE, Velocity:=speed5, Acceleration:=acc5, Deceleration:=100, Axis:=Axis5, Direction:=positive);
		ELSE
			MoveVelocity5(Execute:=TRUE, Velocity:=-speed5, Acceleration:=acc5, Deceleration:=100, Axis:=Axis5, Direction:=negative);
		END_IF
END_CASE

CASE iStatus6 OF

// initialization of the axis
0:
        Power6(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis6);
        IF Power6.Status THEN
                iStatus6 := iStatus6 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed6 <> prev_speed6 THEN
			MoveVelocity6(Execute:=FALSE, Axis:=Axis6);
		END_IF
		IF speed6 > 0 THEN
			MoveVelocity6(Execute:=TRUE, Velocity:=speed6, Acceleration:=acc6, Deceleration:=100, Axis:=Axis6, Direction:=positive);
		ELSE
			MoveVelocity6(Execute:=TRUE, Velocity:=-speed6, Acceleration:=acc6, Deceleration:=100, Axis:=Axis6, Direction:=negative);
		END_IF
END_CASE

CASE iStatus7 OF

// initialization of the axis
0:
        Power7(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis7);
        IF Power7.Status THEN
                iStatus7 := iStatus7 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed7 <> prev_speed7 THEN
			MoveVelocity7(Execute:=FALSE, Axis:=Axis7);
		END_IF
		IF speed7 > 0 THEN
			MoveVelocity7(Execute:=TRUE, Velocity:=speed7, Acceleration:=acc7, Deceleration:=100, Axis:=Axis7, Direction:=positive);
		ELSE
			MoveVelocity7(Execute:=TRUE, Velocity:=-speed7, Acceleration:=acc7, Deceleration:=100, Axis:=Axis7, Direction:=negative);
		END_IF
END_CASE

CASE iStatus8 OF

// initialization of the axis
0:
        Power8(Enable:=TRUE, bRegulatorOn:=TRUE, bDriveStart:=TRUE, Axis:=Axis8);
        IF Power8.Status THEN
				PowerDone := TRUE;
                iStatus8 := iStatus8 + 1;
        END_IF

// Move the axis to position p by use of the MC_MoveAbsolute function block
1:
		IF speed8 <> prev_speed8 THEN
			MoveVelocity8(Execute:=FALSE, Axis:=Axis8);
		END_IF
		IF speed8 > 0 THEN
			MoveVelocity8(Execute:=TRUE, Velocity:=speed8, Acceleration:=acc8, Deceleration:=100, Axis:=Axis8, Direction:=positive);
		ELSE
			MoveVelocity8(Execute:=TRUE, Velocity:=-speed8, Acceleration:=acc8, Deceleration:=100, Axis:=Axis8, Direction:=negative);
		END_IF
END_CASE


END_IF



IF off AND NOT(on) THEN
	 Power(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis1);
	 Power2(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis2);
	 Power3(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis3);
	 Power4(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis4);
	 Power5(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis5);
	 Power6(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis6);
	 Power7(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis7);
	 Power8(Enable:=TRUE, bRegulatorOn:=FALSE, bDriveStart:=FALSE, Axis:=Axis8);
	 
	Reset1(Axis:=Axis1, Execute := TRUE);
	Reset2(Axis:=Axis2, Execute := TRUE);
	Reset3(Axis:=Axis3, Execute := TRUE);
	Reset4(Axis:=Axis4, Execute := TRUE);
	Reset5(Axis:=Axis5, Execute := TRUE);
	Reset6(Axis:=Axis6, Execute := TRUE);
	Reset7(Axis:=Axis7, Execute := TRUE);
	Reset8(Axis:=Axis8, Execute := TRUE);
	
	iStatus := 0;
    iStatus2 := 0;
    iStatus3 := 0;
    iStatus4 := 0;
    iStatus5 := 0;
    iStatus6 := 0;
    iStatus7 := 0;
    iStatus8 := 0;
	
	
END_IF



prev_speed1 := speed1;
prev_speed2 := speed2;
prev_speed3 := speed3;
prev_speed4 := speed4;
prev_speed5 := speed5;
prev_speed6 := speed6;
prev_speed7 := speed7;
prev_speed8 := speed8;


  • No labels

Industrielle styresystemer - NTNU Ålesund - Automasjon