24_2 CODESYS

Innhold i main koden

Variabler

PROGRAM PLC_PRG
VAR
	//MQTT variables
	sTopic										: STRING(255) := 'ObjectCoordinates';
	fbSubscribeMQTT_2							: WagoAppCloud.FbSubscribeMQTT_2(eConnection := eConnectionId.Connection1);
	eQos										: eQualityOfService;
	xBusy										: BOOL;
	xError										: BOOL;
	oStatus										: FbResult;
	xDataReceived								: BOOL;
	xDataTruncated								: BOOL;
	sReceivedTopic								: STRING(255);
	Recived										: ARRAY[0..100] OF BYTE;
	xData										: STRING(255);
	Size										: DWORD;
	MQTTenable									: BOOL;
	
	timerStart									: BOOL;	
	timerSearch									: TON;
	delay										: TON;
	startDelay									: BOOL;	
	//Splitting MQTT string 
	BadRoe 										: ARRAY[0..200] OF STRING(255);
	suction										: INT;
	i											: INT;
	xArray										: ARRAY[0..200] OF INT;
	yArray										: ARRAY[0..200] OF INT;
	j											: INT;
	nullArray									: ARRAY[0..200] OF INT;
	calculatedStepperPosX						: ARRAY[0..200] OF DINT;
	calculatedStepperPosY						: ARRAY[0..200] OF DINT;
	
	//Servo variables  servo1 right, servo2 left
	once										: BOOL;
	MoveServos									: FbServoMotor;
	PowerServo1									: MC_Power;
	PowerServo2 								: MC_Power;
	startPosServo1								: LREAL;
	startPosServo2								: LREAL;
	savePos										: BOOL;
	posServo1									: LREAL;
	posServo2									: LREAL;
	Reset										: BOOL;
	Execute										: BOOL;
	servo1Status								: BOOL;
	servo2Status								: BOOL;
	
	
	//Sevo-motor end brakes variables
	EndBrakes									: FbEndBrakes;
	
	//Steppermotor variables
	stepperControlYaxis							: FbStepperMotor;
	stepperControlXaxis							: FbStepperMotor;
	desiredPosYCheck							: DINT;
	desiredPosXCheck							: DINT;
	resetStepper								: BOOL;	
	stepStartY									: BOOL;
	stepStartX									: BOOL;
	stepperResetReq								: BOOL;
	stepperError								: BOOL;
	stepperBusy									: BOOL;
	stepPosReached								: BOOL;
	SearchX										: DINT;
	SearchY										: DINT;
	stepperStatus								: WagoSysErrorBase.FbResult;
	
	//Skuff variabler
	drawer1										: ARRAY [0..2] OF LREAL:= [320, 460, 380];
	drawer2										: ARRAY [0..2] OF LREAL:= [650, 805, 770];
	Skuff3										: ARRAY [0..2] OF LREAL:= [275, 410, 380];
	
	//outputs
	ventilActivated								: BOOL;		
	ledbarON									: BOOL;
	elmagnetActivated							: BOOL;
END_VAR

Main program

//Naudstopp funksjon som slår av alt visst naudstop er trykt inn
IF GVL.naudstopp THEN
	GVL.iStatus:= 0;
	GVL.stepDesiredPosX:= GVL.stepperXPos;
	GVL.stepDesiredPosY:= GVL.stepperYPos;
	GVL.openDrawer1:= FALSE;
	GVL.startRoeBot:= FALSE;
	GVL.ElektroMagnet:= FALSE;
	GVL.MagnetVentil:= FALSE;
END_IF

//MQTT subscribe to Topic
WagoSysPlainMem.MemCopy(ADR(xData), ADR(Recived), Size);

fbSubscribeMQTT_2(xSubscribe:=MQTTenable, sTopic:=sTopic, EQos:=eQos, aPayloadData:=Recived);
xBusy:=fbSubscribeMQTT_2.xBusy;
oStatus:=fbSubscribeMQTT_2.oStatus;
xDataReceived:=fbSubscribeMQTT_2.xDataReceived;
Size:=fbSubscribeMQTT_2.dwRxNBytes;
xDataTruncated:=fbSubscribeMQTT_2.xDataTruncated;
sReceivedTopic:=fbSubscribeMQTT_2.sReceivedTopic;				


//Timers
timerSearch(IN:=timerStart, PT:= T#5S);
delay(IN:=startDelay, pt:= T#1S);

GVL.numbObject:= STRING_TO_INT(BadRoe[0]);

//Split string
BadRoe:= FSplitString(sInput:=xData, sSplitChar:=',',numbObject:=GVL.numbObject); 


//Run signal for steppermotor in x axis
stepStartX:= ((GVL.stepDesiredPosX <> GVL.stepperXPos) AND NOT (desiredPosXCheck <> GVL.stepDesiredPosX));


//Run signal for steppermotor in y axis
stepStartY:= ((GVL.stepDesiredPosY <> GVL.stepperYPos) AND NOT (desiredPosYCheck <> GVL.stepDesiredPosY));


//Reset faults on servo-drives
servo1.bErrorAckn:= Reset;
servo2.bErrorAckn:= Reset;


//Servo-motor Endbreaks
Execute:=EndBrakes.Execute;
EndBrakes(EB_TL:=GVL.EB_TL, EB_TR:=GVL.EB_TR, EB_BL:=GVL.EB_BL, EB_BR:=GVL.EB_BR, actPos:=servo1.fActposition, setPos:=GVL.servoDesiredPos);
GVL.servoDesiredPos:=EndBrakes.pos;

//Save current position when at bottom endbreaks
IF (GVl.EB_BL OR GVL.EB_BR) AND NOT savePos THEN
	startPosServo1:= servo1.fActPosition;
	startPosServo2:= servo2.fActPosition;
	savePos:= TRUE;
ELSIF NOT (GVL.EB_BL OR GVL.EB_BR) THEN
	savePos:= FALSE;
END_IF

posServo1:=  startPosServo1 + GVL.servoDesiredPos; 	
posServo2:=  startPosServo2 + GVL.servoDesiredPos; 


// init stepper controll
stepperControlYaxis(powerON:=TRUE, endbreakPosDir:= GVL.EB_AF, endbreakNegDir:= GVL.EB_AB, setPos:= GVL.stepDesiredPosY, reset:= resetStepper, start:= stepStartY, iPort:= StepperYaxis);
stepperControlXaxis(powerON:=TRUE, endbreakPosDir:= GVL.EB_AR, endbreakNegDir:= GVL.EB_AL, setPos:= GVL.stepDesiredPosX, reset:= resetStepper, start:= stepStartX, iPort:= StepperXaxis);
GVL.stepperYPos:= stepperControlYaxis.actualPos;
GVL.stepperXPos:= stepperControlXaxis.actualPos;

// Enable power servos
PowerServo1(Enable:=TRUE, bRegulatorON:=TRUE, bDriveStart:=TRUE, Axis:=servo1);
PowerServo2(Enable:=TRUE, bRegulatorON:=TRUE, bDriveStart:=TRUE, Axis:=servo2);
GVL.servo1ActualPos:= servo1.fActPosition;
GVL.servo2ActualPos:= servo2.fActPosition;

servo1Status:= PowerServo1.Status;
servo2Status:= PowerServo2.Status;

desiredPosXCheck := GVL.stepDesiredPosX;
desiredPosYCheck := GVL.stepDesiredPosY;


CASE GVL.iStatus OF
0: // INIT BOT
	IF GVL.startRoeBot THEN
		GVL.LedBar:= TRUE;
		GVL.iStatus:= 10;
	END_IF
	
10: //Check which drawer
	IF GVL.openDrawer1 THEN
		GVL.iStatus:= 100;
	ELSIF GVL.openDrawer2 THEN
		GVL.iStatus:= 200;
	ELSIF GVL.openDrawer3 THEN
		GVL.iStatus:= 300;
	END_IF
	
100: //Move to open drawer
	GVL.servoDesiredPos:= drawer1[0];
	MoveServos(posServo1:= posServo1, posServo2:= posServo2,servo1Status:= servo1Status, servo2Status:= servo2Status, execute:= Endbrakes.Execute);
	IF ((posServo1 <= servo1.fActPosition + 0.1) AND (posServo1 >= servo1.fActPosition - 0.1)) AND ((posServo2 <= servo2.fActPosition + 0.1)  AND (posServo2 >= servo2.fActPosition - 0.1)) THEN
		GVL.iStatus:= 101;
	END_IF
	
101: //Move stepper motor and activate electro magnet
	GVL.stepDesiredPosY:= 15000;
	IF GVL.stepperYPos = GVL.stepDesiredPosY THEN
		GVL.ElektroMagnet:= TRUE;
		GVL.stepDesiredPosY:= 4000;
		GVL.iStatus:= 102;
	END_IF
	
102: //Move stepper motor to open drawer posistion
	IF GVL.stepperYpos = 4000 THEN
		GVL.ElektroMagnet:= FALSE;
		GVL.stepDesiredPosY:= 200;
		GVL.iStatus:= 103;
	END_IF
	
103: //Move stepper motor back 
	IF GVL.stepperYpos <= 200  THEN
		startDelay:= TRUE;
		GVL.iStatus:= 104;
	END_IF	

104: //Move servo motors for scanning posistion
	GVL.servoDesiredPos:= drawer1[1];
	MoveServos(posServo1:= posServo1, posServo2:= posServo2,servo1Status:= servo1Status, servo2Status:= servo2Status, execute:= Endbrakes.Execute);
	IF delay.Q AND((posServo1 <= servo1.fActPosition + 0.1) AND (posServo1 >= servo1.fActPosition - 0.1)) AND ((posServo2 <= servo2.fActPosition + 0.1)  AND (posServo2 >= servo2.fActPosition - 0.1)) THEN
		startDelay:=FALSE;
		GVL.iStatus:= 105;
	END_IF

105: //Move stepper to scan position
	GVL.stepDesiredPosY:= 12000;
	IF GVL.stepperYPos= 12000 THEN
		timerStart := TRUE;
		MQTTenable:=TRUE;
		xArray := nullArray;
		yArray := nullArray;
	END_IF
	
	IF timerSearch.Q THEN
		i := 1;
		GVL.istatus:= 106;
	END_IF
	
106: //Search for Bad Roe and store values from MQTT in arrays and calculate new stepperpos
	timerStart := FALSE;
	FOR i := 1 TO GVL.numbObject DO
		xArray[i] := STRING_TO_INT(BadRoe[((i*2)-1)]);
		yArray[i] := STRING_TO_INT(BadRoe[(i*2)]);
		IF i = GVL.numbObject THEN
			GVL.istatus:= 107;
		END_IF
	END_FOR	
	
107: //Calculate the change in stepper position
	GVL.stepDesiredPosX:= REAL_TO_DINT(0 + ((205 - xArray[1]) / (-0.138)));
	GVL.stepDesiredPosY:= REAL_TO_DINT(12000 + ((205 - yArray[1]) / (0.108)));
	IF (GVL.stepDesiredPosX = GVL.stepperXPos) AND (GVL.stepDesiredPosY = GVL.stepperYPos) THEN
		GVL.iStatus:= 108;
	END_IF
	

108: //run to new stepper psoition	
	MQTTenable:=FALSE;
	startDelay:=TRUE;
	GVL.servoDesiredPos:= drawer1[2];
	MoveServos(posServo1:= posServo1, posServo2:= posServo2,servo1Status:= servo1Status, servo2Status:= servo2Status, execute:= Endbrakes.Execute);
	IF delay.Q AND ((posServo1 <= servo1.fActPosition + 0.1) AND (posServo1 >= servo1.fActPosition - 0.1)) AND ((posServo2 <= servo2.fActPosition + 0.1)  AND (posServo2 >= servo2.fActPosition - 0.1)) THEN
		startDelay:=FALSE;
		GVL.MagnetVentil:=TRUE;
		GVL.iStatus:= 109;
	END_IF
	
109: // Move servos to scanning position
	startDelay:=TRUE;
	IF delay.Q THEN
		GVL.MagnetVentil:=FALSE;
		GVL.servoDesiredPos:= drawer1[1];
		MoveServos(posServo1:= posServo1, posServo2:= posServo2,servo1Status:= servo1Status, servo2Status:= servo2Status, execute:= Endbrakes.Execute);
	END_IF
	IF delay.Q AND((posServo1 <= servo1.fActPosition + 0.1) AND (posServo1 >= servo1.fActPosition - 0.1)) AND ((posServo2 <= servo2.fActPosition + 0.1)  AND (posServo2 >= servo2.fActPosition - 0.1)) THEN
		startDelay:=FALSE;
		GVL.stepDesiredPosY:= 0;
		GVL.stepDesiredPosX:= 0;
	END_IF
	IF GVL.stepDesiredPosY = GVL.stepperYPos AND  GVL.stepDesiredPosX = GVL.stepperXPos THEN
		GVL.iStatus:= 110;
	END_IF 
110:
	startDelay:=TRUE;
	GVl.servoDesiredPos:= drawer1[0];
	MoveServos(posServo1:= posServo1, posServo2:= posServo2,servo1Status:= servo1Status, servo2Status:= servo2Status, execute:= Endbrakes.Execute);
	IF delay.Q AND((posServo1 <= servo1.fActPosition + 0.1) AND (posServo1 >= servo1.fActPosition - 0.1)) AND ((posServo2 <= servo2.fActPosition + 0.1)  AND (posServo2 >= servo2.fActPosition - 0.1)) THEN
		startDelay:=FALSE;
		GVL.iStatus:= 111;
	END_IF

111:
	GVL.stepDesiredPosY:= 15000;
	IF GVL.stepperYPos = GVL.stepDesiredPosY THEN
		GVL.stepDesiredPosY:= 0;
		GVL.iStatus:= 112;
	END_IF
	
112:
	IF GVL.stepperYPos = GVL.stepDesiredPosY THEN
		startDelay:=TRUE;
		GVl.servoDesiredPos:= 0;
		IF delay.Q AND((posServo1 <= servo1.fActPosition + 0.1) AND (posServo1 >= servo1.fActPosition - 0.1)) AND ((posServo2 <= servo2.fActPosition + 0.1)  AND (posServo2 >= servo2.fActPosition - 0.1)) THEN
			startDelay:=FALSE;
			GVL.iStatus:= 0;
		END_IF
	END_IF

END_CASE


//Run the program in manuell mode for testing servos and stepper using HMI
IF GVL.manuell THEN
	GVL.servoDesiredPos:= GVL.manServoDesiredPos;
	GVL.stepDesiredPosY:= GVL.manStepDesiredPosY;
	GVL.stepDesiredPosX:= GVL.manStepDesiredPosX;
	GVL.stepperYPos:= stepperControlYaxis.actualPos;
	GVL.stepperXPos:= stepperControlXaxis.actualPos;
	MoveServos(posServo1:= posServo1, posServo2:= posServo2,servo1Status:= servo1Status, servo2Status:= servo2Status, execute:= Endbrakes.Execute);
	GVL.iStatus:= 0;	
END_IF

Space shortcuts

Page tree