271 lines
7.9 KiB
C
Executable file
271 lines
7.9 KiB
C
Executable file
#include "logic.h"
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::OnBallOwned()
|
|
{
|
|
bool wasKeeper = this->IsKeeper();
|
|
|
|
this->SetKeeper(false);
|
|
|
|
if(!WIRELESS_ACTIVE)
|
|
{
|
|
Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
|
|
|
|
ourAktuator->Kick();
|
|
this->SetKeeper(wasKeeper);
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::OnBallLost()
|
|
{
|
|
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::OnBecomeKeeper()
|
|
{
|
|
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
|
|
if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) > DISTANCE_TOLERANCE)
|
|
{
|
|
status = STATUS_KEEPER_RETURN_HOME;
|
|
|
|
ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetOrientation(), DEFAULT_SPEED, 0);
|
|
}
|
|
else
|
|
{
|
|
UpdateKeeperMovement();
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::OnBecomeAttacker()
|
|
{
|
|
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
|
|
ourNavigator->Stop();
|
|
|
|
UpdateAttackerMovement();
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::Update()
|
|
{
|
|
// We want to use a navigator
|
|
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
|
|
Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
|
|
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
Obstacle_Tracker* ourObstacleTracker = parent->GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN);
|
|
|
|
if(!this->HasBall() && ourBallTracker->HasBall())
|
|
{
|
|
this->hasBall = true;
|
|
this->OnBallOwned();
|
|
}
|
|
else if(this->HasBall() && !ourBallTracker->HasBall())
|
|
{
|
|
this->hasBall = false;
|
|
this->OnBallLost();
|
|
}
|
|
|
|
switch(status)
|
|
{
|
|
case STATUS_ATTACKER_SEARCHING_AT_HOME:
|
|
case STATUS_ATTACKER_SEARCHING_AT_ENEMY:
|
|
if(ourBallTracker->KnowsBallDirection())
|
|
{
|
|
ourNavigator->Stop();
|
|
}
|
|
break;
|
|
case STATUS_KEEPER_HUNT_BALL:
|
|
if(ourBallTracker->KnowsBallDirection())
|
|
{
|
|
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
else
|
|
{
|
|
ourNavigator->Stop();
|
|
}
|
|
break;
|
|
}
|
|
|
|
if(!ourNavigator->IsMoving())
|
|
{
|
|
if(this->IsKeeper())
|
|
{
|
|
UpdateKeeperMovement();
|
|
}
|
|
else
|
|
{
|
|
UpdateAttackerMovement();
|
|
}
|
|
}
|
|
|
|
if(ourNavigator->IsMoving())
|
|
{
|
|
if(ourObstacleTracker->IsObstacleOnPath())
|
|
{
|
|
avoidsObstacle = true;
|
|
for(uint8 i = 0; i < 5; i++)
|
|
{
|
|
if(ourObstacleTracker->IsObstacleOnPath(easyAngle(ourNavigator->GetDirection() - ((i - 2) * PI / 4.0f))))
|
|
{
|
|
}
|
|
ourNavigator->GetDirection();
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::UpdateKeeperMovement()
|
|
{
|
|
Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
|
|
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
|
|
if(ourBallTracker->KnowsBallDirection())
|
|
{
|
|
if(ourBallTracker->GetBallDirection() > KEEPER_LEFT_ANGLE &&
|
|
ourBallTracker->GetBallDirection() <= PI)
|
|
{
|
|
if(distance2d(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
|
|
{
|
|
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
|
|
if(angleDifference > PI)
|
|
{
|
|
angleDifference = (2 * PI) - angleDifference;
|
|
}
|
|
|
|
if(angleDifference > ORIENTATION_TOLERANCE)
|
|
{
|
|
status = STATUS_KEEPER_TURN_TO_BALL;
|
|
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_HUNT_BALL;
|
|
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_DRIVE_TO_DEFEND;
|
|
ourNavigator->DriveTo(DEFENCE_L_LOC_X, DEFENCE_L_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
else if(ourBallTracker->GetBallDirection() < KEEPER_RIGHT_ANGLE &&
|
|
ourBallTracker->GetBallDirection() >= PI)
|
|
{
|
|
if(distance2d(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
|
|
{
|
|
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
|
|
if(angleDifference > PI)
|
|
{
|
|
angleDifference = (2 * PI) - angleDifference;
|
|
}
|
|
|
|
if(angleDifference > ORIENTATION_TOLERANCE)
|
|
{
|
|
status = STATUS_KEEPER_TURN_TO_BALL;
|
|
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_HUNT_BALL;
|
|
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_DRIVE_TO_DEFEND;
|
|
ourNavigator->DriveTo(DEFENCE_R_LOC_X, DEFENCE_R_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(distance2d(HOME_LOC_X, HOME_LOC_Y, ourPositionTracker->GetPositionX(), ourPositionTracker->GetPositionY()) < DISTANCE_TOLERANCE)
|
|
{
|
|
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
|
|
if(angleDifference > PI)
|
|
{
|
|
angleDifference = (2 * PI) - angleDifference;
|
|
}
|
|
|
|
if(angleDifference > ORIENTATION_TOLERANCE)
|
|
{
|
|
status = STATUS_KEEPER_TURN_TO_BALL;
|
|
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_HUNT_BALL;
|
|
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_DRIVE_TO_DEFEND;
|
|
ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(status == STATUS_KEEPER_TURN_LEFT)
|
|
{
|
|
status = STATUS_KEEPER_TURN_RIGHT;
|
|
ourNavigator->RotateTo(315, DEFAULT_ROTATION_SPEED);
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_KEEPER_TURN_LEFT;
|
|
ourNavigator->RotateTo(45, DEFAULT_ROTATION_SPEED);
|
|
}
|
|
}
|
|
}
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Logic::UpdateAttackerMovement()
|
|
{
|
|
Ball_Tracker* ourBallTracker = parent->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
|
|
Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
|
Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
|
|
|
if(ourBallTracker->KnowsBallDirection())
|
|
{
|
|
float angleDifference = fabs(ourBallTracker->GetBallDirection() - ourPositionTracker->GetOrientation());
|
|
|
|
if(angleDifference > PI)
|
|
{
|
|
angleDifference = (2 * PI) - angleDifference;
|
|
}
|
|
|
|
if(angleDifference > ORIENTATION_TOLERANCE)
|
|
{
|
|
status = STATUS_ATTACKER_TURN_TO_BALL;
|
|
ourNavigator->RotateTo(ourBallTracker->GetBallDirection(), DEFAULT_ROTATION_SPEED);
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_ATTACKER_DRIVE_TO_BALL;
|
|
ourNavigator->Drive(ourBallTracker->GetBallDirection(), EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
if(ourPositionTracker->GetPositionX() > 0)
|
|
{
|
|
status = STATUS_ATTACKER_SEARCHING_AT_HOME;
|
|
ourNavigator->DriveTo(HOME_LOC_X, HOME_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
else
|
|
{
|
|
status = STATUS_ATTACKER_SEARCHING_AT_ENEMY;
|
|
ourNavigator->DriveTo(ENEMY_LOC_X, ENEMY_LOC_Y, EMPTY_FLOAT, DEFAULT_SPEED, 0);
|
|
}
|
|
}
|
|
}
|