// ********************************************************************** // Roboticky stolni fotbal // Diplomova prace, CVUT FEL // Vojtech Myslivec, vojtech@myslivec.net // Kveten 2015 // ********************************************************************** INTERFACE // deklarace pouzitych programovych baliku USEPACKAGE CAM; // globani promenne, pouzivane v celem programu, nejsou definovany jako konstanty, protoze mohou byt programove meneny VAR_GLOBAL // parametry stolu, mice a os s hraci tableXsize : INT := 1020; // delka stolu v mm tableYsize : INT := 705; // sirka stolu v mm attackArea : INT := 50; // maximalni dosah hracu od osy vpred v mm ballSize : INT := 35; // prumer micku v mm safetyBorder : INT := 5; // ochranne pasmo u kraju v mm, kam uz osa nezajizdi gearRatio : REAL := 0.0185282; // prevodni pomer mezi rotacni a translacni osou playersDistance : INT := 240; // vzdalenost mezi hraci na ose v mm referenceToPlayer : INT := 40; // vzdalenost hrace od nuly osy v mm playerLength : INT := 70; // delka nohy hrace v mm rotationGearRatio : REAL := 3.055; // prevodni pomer mezi motorem a usou rotacniho pohybu axis1Xposition : INT := 906; // pozice x osy 1 v mm // parametry kamery cameraXResolution : INT := 1280; // rozliseni kamery v ose x v px cameraYResolution : INT := 1022; // rozliseni kamery v ose y v px basicSearchRegion : INT := 300; // strana vyhledavaciho ctverce v mm // pozice kamery vuci hraci plose tableXStartOffset : INT := 0; // offset v obraze na zacatku osy X v px tableYStartOffset : INT := 56; // offset v obraze na zacatku osy Y v px tableXEndOffset : INT := 0; // offset v obraze na konci osy X v px tableYEndOffset : INT := 98; // offset v obraze na konci osy Y v px // parametry programu speedThreshold : INT := 3; // maximalni rychlost mice pro prechod na utocny mod v mm/snimek defenseThreshold : INT := 10; // minimalni rychlost pohybu ve smeru osy x pro prechod na obranny mod v mm/snimek accuracyLimit : INT := 4; // minimalni delka vektoru rychlosti v mm, pro ktery je detekovan pohyb allowedDifference : INT := 5; // povolena hystereze regulatoru polohy v mm, ochrana proti kmitani velocityPreset : INT := 700; // referencni rychlost posunu osy v mm/snimek returnTimerPreset : TIME := T#1s; // cas, za ktery se vrati osa do puvodni polohy v pripade necinnosti disableTimerPreset : TIME := T#10s; // cas, za ktery se vypne napajeni motoru v pripade necinnosti // globalni promenne programu returnTimer : TON; // casovac pro navrat osy do vychozi polohy v pripade necinnosti disableTimer : TON; // casovac pro vypnuti napajeni motoru v pripade necinnosti ballX : INT := 0; // pozice mice v ose x v mm ballY : INT := 0; // pozice mice v ose y v mm ballXprevious : INT := 0; // predchozi pozice mice v ose x v mm ballYprevious : INT := 0; // predchozi pozice mice v ose y v mm ballXnext : INT := 0; // odhadovana nasledujici pozice mice v ose x v mm ballYnext : INT := 0; // odhadovana nasledujici pozice mice v ose y v mm vectorX : INT := 0; // slozka x vektoru rychlosti mice v mm vectorY : INT := 0; // slozka y vektoru rychlosti mice v mm speed : REAL := 0; // rychlost pohybu mice, norma vektoru rychlosti mice mode : INT := 0; // mod programu, 0 = necinnost, 1 = navrat, 2 = obrana, 3 = utok defense : INT := 0; // bod na ose, ke kteremu smeruje letici mic v mm od kraje stolu velocity : INT := 0; // aktualne nastavena rychlost posunu osy tableXsizePx : INT; // delka stolu v ose x v px, prepocitano podle parametru stolu a kamery tableYsizePx : INT; // delka stolu v ose y v px, prepocitano podle parametru stolu a kamery TX : REAL; // transformacni konstanta pro prevod z px do mm pro osu x TY : REAL; // transformacni konstanta pro prevod z px do mm pro osu y END_VAR // deklarace pouzitych programu PROGRAM initialization; PROGRAM main_routine; PROGRAM go_to_stop; END_INTERFACE IMPLEMENTATION // zapnuti napajeni motoru, pokud jeste zapnuto neni FUNCTION_BLOCK enableAxes VAR retDINT : DINT; // navratova hodnota pro volane podprogramy END_VAR // pokud neni zapnuta osa pro horizontalni pohyb, bude zapnuta IF ("FOTBALEK-D435".Axis_1_Translation.actorMonitoring.driveState <> ACTIVE) THEN retDINT := _enableAxis(axis := Axis_1_Translation, enableMode := ALL, servoControlMode := ACTIVE, servoCommandToActualMode := INACTIVE, nextCommand := IMMEDIATELY, commandId := _getCommandId(), forceControlMode := INACTIVE, STWBitSet := 0, movingMode := DO_NOT_CHANGE); END_IF; // pokud neni zapnuta osa pro rotacni pohyb, bude zapnuta IF ("FOTBALEK-D435".Axis_1_Rotation.actorMonitoring.driveState <> ACTIVE) THEN retDINT := _enableAxis(axis := Axis_1_Rotation, enableMode := ALL, servoControlMode := ACTIVE, servoCommandToActualMode := INACTIVE, nextCommand := IMMEDIATELY, commandId := _getCommandId(), forceControlMode := INACTIVE, STWBitSet := 0, movingMode := DO_NOT_CHANGE); END_IF; END_FUNCTION_BLOCK // vypnuti napajeni motoru, pokud jeste vypnuto neni FUNCTION_BLOCK disableAxes VAR retDINT : DINT; // navratova hodnota pro volane podprogramy END_VAR // pokud je zapnuta osa pro horizontalni pohyb, bude vypnuta IF ("FOTBALEK-D435".Axis_1_Translation.actorMonitoring.driveState = ACTIVE) THEN retDINT := _disableAxis(axis:=_to.Axis_1_Translation, disableMode:=ALL, servoControlMode:=INACTIVE, servoCommandToActualMode:=ACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId()); END_IF; // pokud je zapnuta osa pro rotacni pohyb, bude vypnuta IF ("FOTBALEK-D435".Axis_1_Rotation.actorMonitoring.driveState = ACTIVE) THEN retDINT := _disableAxis(axis:=_to.Axis_1_Rotation, disableMode:=ALL, servoControlMode:=INACTIVE, servoCommandToActualMode:=ACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId()); END_IF; END_FUNCTION_BLOCK // rotace osy s nastavenym limitem momentu proti zaseknuti mice FUNCTION_BLOCK rotateAxisWithLimit VAR_INPUT velocity : INT; // nastavena rychlost otoceni acceleration : DINT; // nastavene zrychleni pro rozbehu a dobehu position : INT; // nastavena relativni pozice otoceni END_VAR VAR_OUTPUT done : DINT; // promenna indikujici uspesne dokonceni pohybu (0) nebo urazenou vzdalenost ve stupnich END_VAR VAR retDINT : DINT; // navratova hodnota pro volane podprogramy startPosition : INT; // pocatecni pozice pred rozbehem endPosition : INT; // koncova pozice v pridapde zaseknuti END_VAR done := 0; // ulozeni pocatecni pozice startPosition := LREAL_TO_INT(Axis_1_Rotation.positioningState.actualPosition); // start otaceni na zadanou pozici retDINT := _pos(axis:=_to.Axis_1_Rotation, direction:=BY_VALUE, positioningMode:=RELATIVE, position:=position, velocityType:=DIRECT, velocity:=velocity, positiveAccelType:=DIRECT, positiveAccel:=acceleration, negativeAccelType:=DIRECT, negativeAccel:=acceleration, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=AT_MOTION_START, commandId:=_getCommandId()); // sledovani dokonceni pohybu nebo prekrocini limitu WHILE (_to.Axis_1_Rotation.motionstatedata.motioncommand <> motion_done) DO // pokud je prekrocen moment motoru IF ("FOTBALEK-D435".Axis_1_Rotation.actualTorque.value > 0.35) THEN // zastaveni motoru retDINT := _stop(axis:=_to.Axis_1_Rotation, stopMode:=STOP_WITHOUT_ABORT, stopSpecification:=ALL_AXIS_MOTION, movingMode:=CURRENT_MODE, stopId:=_getCommandId(), mergeMode:=IMMEDIATELY, nextCommand:=WHEN_BUFFER_READY, commandId:=_getCommandId()); // povoleni osy po zastaveni retDINT := _enableAxis(axis:=_to.Axis_1_Rotation, movingMode:=POSITION_CONTROLLED, enableMode:=ALL, servoControlMode:=ACTIVE, servoCommandToActualMode:=INACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId(), forcecontrolMode:=INACTIVE); // rotace o nula stupnu, nutne pro pokracovani programu, jinak se program zastavi retDINT := _pos(axis:=_to.Axis_1_Rotation, direction:=BY_VALUE, positioningMode:=RELATIVE, position:=0, velocityType:=DIRECT, velocity:=velocity, positiveAccelType:=DIRECT, positiveAccel:=acceleration, negativeAccelType:=DIRECT, negativeAccel:=acceleration, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // ulozeni koncove pozice endPosition := LREAL_TO_INT(Axis_1_Rotation.positioningState.actualPosition); // vypocet urazene vzdalenosti do zaseknuti done := startPosition - endPosition; END_IF; END_WHILE; END_FUNCTION_BLOCK // inicializace osy v rotacnim i translacnim smeru FUNCTION_BLOCK axesInitialization VAR retDINT : DINT; // navratova hodnota pro volane podprogramy enableAxes1 : enableAxes; // funkcni blok zapnuti napajeni disableAxes1 : disableAxes; // funkcni blok vypnuti napajeni END_VAR // zapnuti oranzoveho majacku pro signalizaci inicializace beacon_orange := 1; // zapnuti motoru/napajeni pro rotaci a translaci retDINT := _enableAxis(axis:=_to.Axis_1_Rotation, movingMode:=POSITION_CONTROLLED, enableMode:=ALL, servoControlMode:=ACTIVE, servoCommandToActualMode:=INACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId(), forcecontrolMode:=INACTIVE); retDINT := _enableAxis(axis:=_to.Axis_1_Translation, movingMode:=POSITION_CONTROLLED, enableMode:=ALL, servoControlMode:=ACTIVE, servoCommandToActualMode:=INACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId(), forcecontrolMode:=INACTIVE); // pomaly pohyb osy smerem ke kraji retDINT := _move(axis:=_to.Axis_1_Translation, direction:=POSITIVE, velocityType:=DIRECT, velocity:=40, moveTimeOutType:=WITHOUT_TIME_LIMIT, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_ACCELERATION_DONE, commandId:=_getCommandId(), movingMode:=SPEED_CONTROLLED); // monitoring momentu na motoru, po dosazeni okraje vypne posun retDINT := _enableTorqueLimiting(axis:=_to.Axis_1_Translation, torqueLimit:=30, torqueLimitType:=USER_DEFAULT, torqueLimitUnit:=TORQUE, nextCommand:=WHEN_TORQUELIMIT_REACHED, commandId:=_getCommandId()); // vypnuti posunu osy smerem ke kraji retDINT := _stop(axis:=_to.Axis_1_Translation, stopMode:=STOP_WITHOUT_ABORT, stopSpecification:=ALL_AXIS_MOTION, movingMode:=CURRENT_MODE, stopId:=_getCommandId(), mergeMode:=IMMEDIATELY, nextCommand:=WHEN_BUFFER_READY, commandId:=_getCommandId()); // znovuzapnuti osy retDINT := _enableAxis(axis:=_to.Axis_1_Translation, movingMode:=POSITION_CONTROLLED, enableMode:=ALL, servoControlMode:=ACTIVE, servoCommandToActualMode:=INACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId(), forcecontrolMode:=INACTIVE); // nastaveni aktualni pozice jako nula osy retDINT := _homing(axis:=_to.Axis_1_Translation, homingMode:=DIRECT_HOMING, homePositionType:=DIRECT, homePosition:=0, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_AXIS_HOMED, commandId:=_getCommandId()); // presun hracu do stredu hriste retDINT := _pos(axis:=_to.Axis_1_Translation, direction:=SHORTEST_WAY, positioningMode:=RELATIVE, position:=-185, velocityType:=DIRECT, velocity:=50, positiveAccelType:=DIRECT, positiveAccel:=1000, negativeAccelType:=DIRECT, negativeAccel:=1000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // zastaveni pohybu retDINT := _stop(axis:=_to.Axis_1_Translation, stopMode:=STOP_WITHOUT_ABORT, stopSpecification:=ALL_AXIS_MOTION, movingMode:=CURRENT_MODE, stopId:=_getCommandId(), mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // povel pro kameru, zapnuti podprogramu pro inicializaci osy v rotaci axis1RotationInitizalizeEnable := 1; // vyckavani na zapnuti podprogramu retDINT := _waitTime(timeValue:=T#200ms); // pokud jeste neni osa inicializovana, je proveden proces inicializace IF (axis1InitializationResult <> 1) THEN // nastaveni synchronizace mezi rotacni a translacni osou, aby nedoslo behem otaceni k deinicializaci translacni osy retDINT := _setMaster(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, master:=_to.Axis_1_Rotation, transientBehavior:=WITH_NEXT_SYNCHRONIZING); retDINT := _enableVelocityGearing(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, direction:=NEGATIVE, gearingRatioType:=DIRECT, gearingRatio:=gearRatio, positiveAccelType:=DIRECT, positiveAccel:=1000, negativeAccelType:=DIRECT, negativeAccel:=1000, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_AXIS_SYNCHRONIZED, commandId:=_getCommandId()); // dokud neni sledovany hrac ve spravne pozici WHILE(axis1InitializationResult <> 1) DO // pootoceni hraci smerem dozadu retDINT := _pos(axis:=_to.Axis_1_Rotation, direction:=NEGATIVE, positioningMode:=RELATIVE, position:=10, velocityType:=DIRECT, velocity:=40, positiveAccelType:=DIRECT, positiveAccel:=1000, negativeAccelType:=DIRECT, negativeAccel:=1000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); retDINT := _stop(axis:=_to.Axis_1_Rotation, stopMode:=STOP_WITHOUT_ABORT, stopSpecification:=ALL_AXIS_MOTION, movingMode:=CURRENT_MODE, stopId:=_getCommandId(), mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); END_WHILE; // desynchronizace os retDINT := _disableVelocityGearing(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // korekce nulove polohy osy v horizontalnim smeru osy retDINT := _redefinePosition(axis:=_to.Axis_1_Translation, redefineSpecification:=ACTUAL_VALUE, redefineMode:=ABSOLUTE, position:=-185, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId()); END_IF; // vypnuti podpprogramu pro inicializace osy v rotaci, kvuli uspore vypocetni narocnosti axis1RotationInitizalizeEnable := 0; // pootoceni hracu do vychozi pozice, ktera je mirne odlisna od inicializacni retDINT := _pos(axis:=_to.Axis_1_Rotation, direction:=POSITIVE, positioningMode:=RELATIVE, position:=50, velocityType:=DIRECT, velocity:=100, positiveAccelType:=DIRECT, positiveAccel:=1000, negativeAccelType:=DIRECT, negativeAccel:=1000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // vypnuti napajeni motoru a os disableAxes1(); // vypnuti oranzoveho majacku beacon_orange := 0; END_FUNCTION_BLOCK // inicializace promennych FUNCTION_BLOCK initVariables // rozmer hraci plochy v px v osach x a y tableXsizePx := cameraXResolution - tableXStartOffset - tableXEndOffset; tableYsizePx := cameraYResolution - tableYStartOffset - tableYEndOffset; // transformacni konstanty mezi souradnicemi v px a mm pro osy x a y TX := INT_TO_REAL(tableXsize)/INT_TO_REAL(tableXsizePx); TY := INT_TO_REAL(tableYsize)/INT_TO_REAL(tableYsizePx); // nastaveni rychlosti presunu osy velocity := velocityPreset; END_FUNCTION_BLOCK FUNCTION_BLOCK setSearchRegion VAR // lokalni promenne pro uchovani rozmeru oblasti vyhledavani searchRegionXStartTemp : INT; // zacatek v ose x searchRegionYStartTemp : INT; // zacatek v ose y searchRegionXEndTemp : INT; // delka v ose x searchRegionYEndTemp : INT; // delka v ose y END_VAR // pokud mice nenalezen, maximalizuj oblast vyhledavani IF (vectorX = 0 AND vectorY = 0 AND ballXnext = 0 AND ballXnext = 0) THEN // nastaveni oblasti na celou hraci plochu searchRegionXStartTemp := tableXStartOffset; searchRegionYStartTemp := tableYStartOffset; searchRegionXEndTemp := cameraXResolution - tableXStartOffset - tableXEndOffset; searchRegionYEndTemp := cameraYResolution - tableYStartOffset - tableYEndOffset; ELSE // nastaveni oblasti podle pozice mice - ctverec okolo orientovany smerem k vlastni brane searchRegionXStartTemp := REAL_TO_INT(ballXnext/TX) - ballSize + tableXStartOffset; searchRegionYStartTemp := REAL_TO_INT(ballYnext/TY) - basicSearchRegion/2 + tableYStartOffset; searchRegionXEndTemp := basicSearchRegion + 100; searchRegionYEndTemp := basicSearchRegion; // omezeni oblasti pokud se mic nachazi u kraje hriste IF (searchRegionXStartTemp < tableXStartOffset) THEN searchRegionXStartTemp := tableXStartOffset; END_IF; IF (searchRegionYStartTemp < tableYStartOffset) THEN searchRegionYStartTemp := tableYStartOffset; END_IF; IF (searchRegionXStartTemp + searchRegionXEndTemp > cameraXResolution - tableXEndOffset) THEN searchRegionXEndTemp := cameraXResolution - tableXEndOffset - searchRegionXStartTemp; END_IF; IF (searchRegionYStartTemp + searchRegionYEndTemp > cameraYResolution - tableYEndOffset) THEN searchRegionYEndTemp := cameraYResolution - tableYEndOffset - searchRegionYStartTemp; END_IF; END_IF; // odeslani rozmeru vyhledavaci oblasti po siti profinet searchRegionXStart := searchRegionXStartTemp; searchRegionYStart := searchRegionYStartTemp; searchRegionXEnd := searchRegionXEndTemp; searchRegionYEnd := searchRegionYEndTemp; END_FUNCTION_BLOCK // uprava zmerene pozice micku dle konkretniho umisteni kamery, ofsetu, atd. FUNCTION_BLOCK coordinateSystemTransfer // prepocet souradnic x a y z kamery v px do souradnic v mm vuci hraci plose ballX := REAL_TO_INT((WORD_TO_INT(positionX) - tableXStartOffset)*TX); ballY := REAL_TO_INT((WORD_TO_INT(positionY) - tableYStartOffset)*TY); // pokud neni mic nalezen IF (ballX < 0) THEN ballX := 0; END_IF; IF (ballY < 0) THEN ballY := 0; END_IF; END_FUNCTION_BLOCK // vypocet vektoru pohybu mice a odhad budouci pozice FUNCTION_BLOCK countVectorAndEstimateNextPosition VAR Xaccumulator : INT := 0; // promenne pro uchovani vektoru pri vypoctu odrazu Yaccumulator : INT := 0; sign : INT; // znamenko slozky vektoru i : INT; // promenna pro for cyklus END_VAR // pocatecni inicializace udaju vectorX := 0; vectorY := 0; speed := 0; ballXnext := ballX; ballYnext := ballY; // pokud je znama soucasna i predchozi pozice mice IF (ballX <> 0 AND ballY <> 0 AND ballXprevious <> 0 AND ballYprevious <> 0) THEN vectorX := -(ballY - ballYprevious); // vypocet slozek x a y vektoru pohybu mice vectorY := ballX - ballXprevious; speed := SQRT(vectorX*vectorX + vectorY*vectorY); // vypocet delky vektoru // pokud je vektor dostatecne dlouhy = presny IF (SQRT(vectorX*vectorX + vectorY*vectorY) > accuracyLimit) THEN ballXnext := ballX + vectorX; // vypocet odhadovane budouci pozice ballYnext := ballY + vectorY; // pokud dojde v dalsim kroce k odrazu od steny hriste IF (TRUE AND (ballXnext < 0 OR ballY < 0 OR ballX > tableXsize OR ballY > tableYsize)) THEN Xaccumulator := vectorX; Yaccumulator := vectorY; ballXnext := ballX; ballYnext := ballY; // aplikace zakona odrazu "krok za krokem" FOR i:=1 TO MIN(MAX(Xaccumulator, Yaccumulator), 1000) BY 1 DO IF (ballXnext <= 0 OR ballXnext >= tableXsize) THEN Xaccumulator := -Xaccumulator; vectorX := -vectorX; END_IF; IF (ballYnext <= 0 OR ballYnext >= tableYsize) THEN Yaccumulator := -Yaccumulator; vectorY := -vectorY; END_IF; IF (ABS(Xaccumulator) > 0) THEN sign := (Xaccumulator/ABS(Xaccumulator)); ballXnext := ballXnext + sign; Xaccumulator := Xaccumulator - sign; END_IF; IF (ABS(Yaccumulator) > 0) THEN sign := (Yaccumulator/ABS(Yaccumulator)); ballYnext := ballYnext + sign; Yaccumulator := Yaccumulator - sign; END_IF; END_FOR; END_IF; ELSE vectorX := 0; vectorY := 0; speed := 0; END_IF; END_IF; ballXprevious := ballX; // ulozeni predchozi pozice pro pristi krok ballYprevious := ballY; END_FUNCTION_BLOCK // vyber herniho modu osy - obrana, utok ... FUNCTION_BLOCK selectMode VAR_INPUT axisXposition : INT; // pozice osy, pro kterou se pocita END_VAR IF (ballX <= axisXposition) THEN // jestlize je mic pred tyci IF (vectorY > defenseThreshold ) THEN // jestlize se mic blizi smerem k tyci dostatecnou rychlosti mode := 2; // obrana RETURN; ELSIF ((speed < speedThreshold) // jestlize se pobybuje pomalu a je v dosahu AND (ballX > axisXposition - attackArea)) THEN mode := 3; // utok RETURN; // opusteni funkce, kvuli zrychleni ELSE mode := 0; // nedelej nic END_IF; ELSE mode := 1; RETURN; // opusteni funkce, kvuli zrychleni END_IF; // casovac na vraceni do vychozi pozice po neaktivite returnTimer(); IF (returnTimer.q) THEN // pokud jiz osa ve vychozi pozici neni IF (ABS(Axis_1_Translation.positioningState.actualPosition + 185) > allowedDifference) THEN mode := 1; // navrat na puvodni pozici END_IF; END_IF; END_FUNCTION_BLOCK // vypocet obranne akce osy FUNCTION_BLOCK countDefense VAR_INPUT axisXposition : INT; // pozice osy, pro kterou se pocita END_VAR VAR temp : INT; // docasna promenna pro vypocty END_VAR // vypocet bodu protnuti drahy mice s bodem na ose temp := -(vectorX*ballXprevious + vectorY*ballYprevious); IF (vectorY <> 0) THEN temp := REAL_TO_INT(-(vectorX*axisXposition + temp)/vectorY); ELSE // mic se nepriblizuje, zatim neni treba nic delat temp := 0; END_IF; IF (temp > 0 AND temp < tableYsize) THEN IF (ballX < axisXposition) THEN IF (vectorY > 0) THEN defense := temp; // vraceni bodu na ose, ktery je treba branit END_IF; END_IF; END_IF; END_FUNCTION_BLOCK // presun hracu na zadanou pozici FUNCTION_BLOCK movePlayers VAR_INPUT position : INT; // referencni pozice END_VAR VAR_OUTPUT moved : BOOL := FALSE; // promenna indikujici uspesny presun END_VAR VAR retDINT : DINT; // navratova hodnota pro volane podprogramy newReference : INT; // promenna pro ulozeni reference po jeji uprave fixRotation : BOOL := FALSE; // indikator nutnosti opravy vule rotacni osy END_VAR // pokud je bliz k prvnimu hraci IF (ABS(-Axis_1_Translation.positioningState.actualPosition + referenceToPlayer - position) < ABS(-Axis_1_Translation.positioningState.actualPosition + referenceToPlayer + playersDistance - position)) THEN // pokud lze dosahnout prvnim hracem IF (position >= tableYsize - playersDistance - safetyBorder - referenceToPlayer) THEN newReference := position - referenceToPlayer - playersDistance; ELSE newReference := position - referenceToPlayer; END_IF; ELSE // pokud je bliz ke druhemu hraci // pokud lze dosahnout prvnim druhym hracem IF (position =< referenceToPlayer + playersDistance + safetyBorder) THEN newReference := position - referenceToPlayer; ELSE newReference := position - referenceToPlayer - playersDistance; END_IF; END_IF; // pokud je referenci bod smysluplny a lze se do neho presunout (ochrana poskozeni HW) IF (newReference > 0 + safetyBorder AND (newReference < tableYsize/2 - safetyBorder OR position < tableYsize/2)) THEN // pokud je pohyb ve smeru, kdy vule v rotacni ose cini problemy, korekce vule IF (ABS(Axis_1_Translation.positioningState.actualPosition - newReference) > allowedDifference) THEN IF (Axis_1_Translation.positioningState.actualPosition + newReference > 0) THEN // drobny posun v rotacni ose dopredu retDINT := _pos(axis:=_to.Axis_1_Rotation, direction:=NEGATIVE, positioningMode:=RELATIVE, position:=40, velocityType:=DIRECT, velocity:=7000, positiveAccelType:=DIRECT, positiveAccel:=100000, negativeAccelType:=DIRECT, negativeAccel:=100000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); fixRotation := TRUE; ELSE fixRotation := FALSE; END_IF; // presun osy na novou pozici retDINT := _pos(axis:=_to.Axis_1_Translation, direction:=NEGATIVE, positioningMode:=ABSOLUTE, position:=-newReference, velocityType:=DIRECT, velocity:=velocity, positiveAccelType:=DIRECT, positiveAccel:=5000, negativeAccelType:=DIRECT, negativeAccel:=5000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // opet korekce vule, tentokrat pri dojezdu IF (fixRotation) THEN // drobny posun v rotacni ose zpet retDINT := _pos(axis:=_to.Axis_1_Rotation, direction:=POSITIVE, positioningMode:=RELATIVE, position:=40, velocityType:=DIRECT, velocity:=7000, positiveAccelType:=DIRECT, positiveAccel:=100000, negativeAccelType:=DIRECT, negativeAccel:=100000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); END_IF; // ochrany casovy interval proti vibracim retDINT := _waitTime(timeValue:=T#100ms); // reset casovace neaktivity osy disableTimer(IN := FALSE); disableTimer(pt := disableTimerPreset, IN := TRUE); END_IF; // indikator uspesneho presunu osy moved := TRUE; END_IF; END_FUNCTION_BLOCK // vykop mice FUNCTION_BLOCK shoot VAR_INPUT shift : INT := 0; // vstupni parametr udavaji predchozi jiz provedeny posun v rotacni ose END_VAR VAR retDINT : DINT; // navratova hodnota pro volane podprogramy retDINT2 : DINT; // navratova hodnota pro volane podprogramy rotateAxisWithLimit1 : rotateAxisWithLimit; // funkcni blok rotace osy s limitem momentu END_VAR // zapnuti synchronizace pro vykopu mezi rotacni a translacni osou, aby nedochazelo k posunu do strany, translacni osa sleduje rotacni pohyb retDINT := _setMaster(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, master:=_to.Axis_1_Rotation, transientBehavior:=WITH_NEXT_SYNCHRONIZING); retDINT := _enableVelocityGearing(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, direction:=NEGATIVE, gearingRatioType:=DIRECT, gearingRatio:=gearRatio, positiveAccelType:=DIRECT, positiveAccel:=100000, negativeAccelType:=DIRECT, negativeAccel:=100000, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_AXIS_SYNCHRONIZED, commandId:=_getCommandId()); // vykop mice s limitem na moment rotateAxisWithLimit1(velocity := 8000, acceleration := 100000, position := 400, done => retDINT); // pokud uspesne vykopnuto IF (retDINT = 0) THEN // navrat zpet, opet s limitem, zde se casto zasekaval mic rotateAxisWithLimit1(velocity := 5000, acceleration := 1000, position := -(400 - shift), done => retDINT); ELSE // pokud se mic zasekl, navrat zpet o urazenou vzdalenost a pak presun do vychozi pozice WHILE (retDINT <> 0) DO rotateAxisWithLimit1(velocity := 5000, acceleration := 1000, position := DINT_TO_INT(retDINT), done => retDINT); retDINT2 := _waitTime(timeValue:=T#200ms); // casove zpozdeni nutne, jinak nefunguje rotateAxisWithLimit1(velocity := 5000, acceleration := 1000, position := shift, done => retDINT); retDINT2 := _waitTime(timeValue:=T#200ms); // casove zpozdeni nutne, jinak nefunguje END_WHILE; END_IF; // desynchronizace pohybu os retDINT := _disableVelocityGearing(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // reset casovace neaktivity osy disableTimer(IN := FALSE); disableTimer(pt := disableTimerPreset, IN := TRUE); END_FUNCTION_BLOCK // postrceni mice smerem k druhemu hraci osy a nasledny vykop FUNCTION_BLOCK poke VAR_INPUT distance : INT := 0; // vzdalenost mice od osy direction : INT := 0; // smer nahravky - vpravo, vlevo END_VAR VAR retDINT : DINT; // navratova hodnota pro volane podprogramy angle : INT; // uhel, o ktery je treba natocit hrace pro nahravce do strany rotateAxisWithLimit1 : rotateAxisWithLimit; // funkcni blok rotace s limitem momentu END_VAR // zapnuti synchronizace pro vykopu mezi rotacni a translacni osou, aby nedochazelo k posunu do strany, translacni osa sleduje rotacni pohyb a zaroven se muze sama pohybovat retDINT := _setMaster(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, master:=_to.Axis_1_Rotation, transientBehavior:=WITH_NEXT_SYNCHRONIZING); retDINT := _enableVelocityGearing(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, direction:=NEGATIVE, gearingRatioType:=DIRECT, gearingRatio:=gearRatio, positiveAccelType:=DIRECT, positiveAccel:=100000, negativeAccelType:=DIRECT, negativeAccel:=100000, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_AXIS_SYNCHRONIZED, commandId:=_getCommandId()); // vypocet uhlu, o ktery je potreba se natocit, aby byl mic spravne nahran angle := REAL_TO_INT(ASIN(INT_TO_REAL(distance)/INT_TO_REAL(playerLength))*rotationGearRatio*180/3.1415 + 1*rotationGearRatio); // natoceni hracem na uroven mice rotateAxisWithLimit1(velocity := 3000, acceleration := 100000, position := angle, done => retDINT); // postrceni mice do strany retDINT := _pos(axis:=_to.Axis_1_Translation, direction:=BY_VALUE, positioningMode:=RELATIVE, position:=-ballSize*direction, velocityType:=DIRECT, velocity:=velocity, positiveAccelType:=DIRECT, positiveAccel:=3000, negativeAccelType:=DIRECT, negativeAccel:=3000, blendingMode:=INACTIVE, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // natoceni hrace do pozice pro vykop rotateAxisWithLimit1(velocity := 7000, acceleration := 100000, position := -(angle+200), done => retDINT); // desynchronizace pohybu os retDINT := _disableVelocityGearing(followingObject:=_to.Axis_1_Translation_SYNCHRONOUS_OPERATION, mergeMode:=IMMEDIATELY, nextCommand:=WHEN_MOTION_DONE, commandId:=_getCommandId()); // reset casovace neaktivity osy disableTimer(IN := FALSE); disableTimer(pt := disableTimerPreset, IN := TRUE); END_FUNCTION_BLOCK // provedeni obranne akce FUNCTION_BLOCK defenseAction VAR movePlayers1 : movePlayers; // funkcni blok posunu hracu END_VAR // nastaveni rychlosti presunu osy a podle rychlosti mice velocity := MIN(velocityPreset, REAL_TO_INT(speed*25)); // presun osy movePlayers1(position := defense); velocity := velocityPreset; // casovac pro navrat osy returnTimer(IN := FALSE); returnTimer(pt:= returnTimerPreset, IN := TRUE); END_FUNCTION_BLOCK // funkcep ro navrat osy do vychozi pozice FUNCTION_BLOCK returnAction VAR movePlayers1 : movePlayers; // funkce posunu hracu END_VAR // navrat soy movePlayers1(position := 225); END_FUNCTION_BLOCK // funkce zakladniho vykopu pred sebe FUNCTION_BLOCK attackAction VAR_INPUT target : INT := 0; // misto vykopu shift : INT := 0; // jiz provedeny posun v rotaci END_VAR VAR_OUTPUT done : INT := 0; // indikator dokonceni akce END_VAR VAR movePlayers1 : movePlayers; // funkce pro presun hracu shoot1 : shoot; // funkce pro vykop newReference : INT := -1; // promenna upravene reference readyToShoot : BOOL; // indikator pripravenosti k vykopu coordinateSystemTransfer1 : coordinateSystemTransfer; // funkce nacteni udaju z kamery END_VAR // pokud je mic ve hre IF (ballX > 0) THEN // urceni mista vykopu IF (target = 0) THEN newReference := ballYnext;// + REAL_TO_INT((INT_TO_REAL(ballYnext - tableYsize/2)/INT_TO_REAL(tableYsize/2)*30)); ELSE newReference := target;// + REAL_TO_INT((INT_TO_REAL(ballYnext - tableYsize/2)/INT_TO_REAL(tableYsize/2)*30)); END_IF; // ochrana pred dojezdem na okraj stolu IF (newReference =< safetyBorder OR newReference >= tableYsize/2 - safetyBorder) THEN newReference := ballYnext; END_IF; // presun hracu movePlayers1(position := newReference, moved => readyToShoot); // aktualizace casovace pro navrat osy returnTimer(IN := FALSE); returnTimer(pt:= returnTimerPreset, IN := TRUE); // pokud probehl presun na misto vykopu IF (readyToShoot) THEN // cekani na vhodnou pozici mice a vykop coordinateSystemTransfer1(); WHILE (NOT returnTimer.q) DO IF (ABS(Axis_1_Translation.positioningState.actualPosition + ballY - referenceToPlayer + 10) > allowedDifference AND ABS(Axis_1_Translation.positioningState.actualPosition + ballY - referenceToPlayer - playersDistance - 10) > allowedDifference) THEN coordinateSystemTransfer1(); returnTimer(); ELSE shoot1(shift := shift); done := 1; EXIT; END_IF; END_WHILE; // aktualizace casovace pro navrat osy returnTimer(IN := FALSE); returnTimer(pt:=returnTimerPreset, IN := TRUE); END_IF; END_IF; END_FUNCTION_BLOCK // funkce pro vykop mice s nahravkou vlastnimu hraci FUNCTION_BLOCK attack2Action VAR_INPUT target : INT := 0; // misto vykopu END_VAR VAR movePlayers1 : movePlayers; // funkce posunu osy poke1 : poke; // funkce nahravky newReference : INT := -1; // promenna upravene reference readyToShoot : BOOL; // indikator pripravenosti k vykopu attackAction1 : attackAction; // funkce vykopu done : INT := 1; // indikator dokonceni akce rotateAxisWithLimit1 : rotateAxisWithLimit; // funkce rotace osou s limitem momentu retDINT : DINT; // navratova hodnota pro volane podprogramy retDINT2 : DINT; // navratova hodnota pro volane podprogramy END_VAR // pokud je mic ve hre IF (ballX > 0) THEN // urceni strany pro nahravku - vpravo, vlevo IF (ballYnext > safetyBorder*2 OR ballYnext < tableYsize/2 - safetyBorder*2) THEN IF (ballYnext > tableYsize/2) THEN newReference := ballYnext + ballSize; // presun na pozici movePlayers1(position := newReference, moved => readyToShoot); IF (readyToShoot) THEN // nahravka a vykop poke1(direction := -1, distance := axis1Xposition - ballXnext); attackAction1(target := target, shift := 200, done => done); END_IF; ELSE newReference := ballYnext - ballSize; // presun na pozici movePlayers1(position := newReference, moved => readyToShoot); IF (readyToShoot) THEN // nahravka a vykop poke1(direction := 1, distance := axis1Xposition - ballXnext); attackAction1(target := target - playersDistance, shift := 200, done => done); END_IF; END_IF; IF (done = 0) THEN rotateAxisWithLimit1(velocity := 3000, acceleration := 100000, position := 200, done => retDINT); // pokud se mic zasekl, navrat zpet o urazenou vzdalenost a pak presun do vychozi pozice WHILE (retDINT <> 0) DO rotateAxisWithLimit1(velocity := 5000, acceleration := 1000, position := DINT_TO_INT(retDINT), done => retDINT); retDINT2 := _waitTime(timeValue:=T#200ms); // casove zpozdeni nutne, jinak nefunguje rotateAxisWithLimit1(velocity := 5000, acceleration := 1000, position := 200, done => retDINT); retDINT2 := _waitTime(timeValue:=T#200ms); // casove zpozdeni nutne, jinak nefunguje END_WHILE; END_IF; END_IF; END_IF; END_FUNCTION_BLOCK // funkce pro vyber vhodne strategie vykopu a jeho samtoneho provedeni FUNCTION_BLOCK selectAndDoAttack // deklarace pouzitych funkcnich bloku VAR attackAction1 : attackAction; attack2Action1 : attack2Action; END_VAR // podle pozice mice zvoli vhodnou taktiku vykopu // aktualne kvuli bezpecnosti pouze jednoduchy vykop IF (TRUE OR ABS(ballYnext - tableYsize/2) < 100) THEN attackAction1(); // presun na pozici a vykop mice ELSE attack2Action1(target := 350); // presun na pozici a vykop mice s nahravkou END_IF; END_FUNCTION_BLOCK PROGRAM initialization // deklarace pouzitych funkcnich bloku VAR axesInitialization1 : axesInitialization; initVariables1 : initVariables; setSearchRegion1 : setSearchRegion; END_VAR // inicializace promennych initVariables1(); // nastaveni oblasti hledani mice na celou hraci plochu setSearchRegion1(); // inicializace osy, horizontalne i rotacne axesInitialization1(); // priznak dokoncene inicializace osy axisInitialized := TRUE; END_PROGRAM PROGRAM main_routine // deklarace pouzitych funkcnich bloku VAR setSearchRegion1 : setSearchRegion; enableAxes1 : enableAxes; disableAxes1 : disableAxes; coordinateSystemTransfer1 : coordinateSystemTransfer; countVectorAndEstimateNextPosition1 : countVectorAndEstimateNextPosition; selectMode1 : selectMode; countDefense1 : countDefense; defenseAction1 : defenseAction; returnAction1 : returnAction; selectAndDoAttack1 : selectAndDoAttack; END_VAR // nekonecna ridici smycka, pokud v ServoSynchronousTask WHILE (TRUE) DO IF (axisInitialized) THEN // prevod zmerenych udaju o pozici mice v px na skutecnou pozici v mm coordinateSystemTransfer1(); // vypocet vektoru rychlosti mice a odhad budouci pozice countVectorAndEstimateNextPosition1(); // vyber modu osy - cekani, navrat, obrana, utok selectMode1(axisXposition := axis1Xposition); // chovani osy podle vybraneho modu IF (mode = 1) THEN // mod navrat do vychozi pozice enableAxes1(); // zapnuti motoru, pokud nejsou returnAction1(); // navrat do vychozi pozice ELSIF (mode = 2) THEN // mod obrana countDefense1(axisXposition := axis1Xposition); // vypocet pruseciku s osou enableAxes1(); // zapnuti motoru, pokud nejsou defenseAction1(); // provedeni obranneho pohybu ELSIF (mode = 3) THEN // mod utok enableAxes1(); // zapnuti motoru, pokud nejsou selectAndDoAttack1(); // vyber typu utoku a jeho provedeni END_IF; // nastaveni oblasti vyhledavani pro dalsi cyklus kamery setSearchRegion1(); // casovac necinnosti motoru pro vypnuti napajeni disableTimer(); // update casovace IF (disableTimer.Q) THEN // pokud je dosazeno stanoveneho casu disableAxes1(); // vypnuti motoru END_IF; END_IF; END_WHILE; END_PROGRAM PROGRAM go_to_stop VAR retDINT : DINT; // navratova hodnota pro volane podprogramy END_VAR retDINT := _disableAxis(axis:=_to.Axis_1_Rotation, disableMode:=ALL, servoControlMode:=INACTIVE, servoCommandToActualMode:=ACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId()); retDINT := _disableAxis(axis:=_to.Axis_1_Translation, disableMode:=ALL, servoControlMode:=INACTIVE, servoCommandToActualMode:=ACTIVE, nextCommand:=WHEN_COMMAND_DONE, commandId:=_getCommandId()); END_PROGRAM END_IMPLEMENTATION