diff options
Diffstat (limited to 'source/Concept/Framework/modules')
11 files changed, 511 insertions, 39 deletions
diff --git a/source/Concept/Framework/modules/executor/navigator.c b/source/Concept/Framework/modules/executor/navigator.c index 7c4618a..8320eca 100755 --- a/source/Concept/Framework/modules/executor/navigator.c +++ b/source/Concept/Framework/modules/executor/navigator.c @@ -59,7 +59,7 @@ void Navigator::Drive(float newDirection, float newAngle, float newSpeed, float //-----------------------------------------------------------------------------
void Navigator::DriveTo(float newX, float newY, float newAngle, float newSpeed, float rotationSpeed)
{
- if(newX < 0 || newY < 0) return;
+ if(newX == EMPTY_FLOAT || newY == EMPTY_FLOAT) return;
Position_Tracker* locationeer = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
@@ -125,7 +125,7 @@ bool Navigator::TargetReached() bool targetReached = false;
- if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < 1.0f))
+ if(!HasTarget() || (distance2d(targetX, targetY, locationeer->GetPositionX(), locationeer->GetPositionY()) < DISTANCE_TOLERANCE))
{
targetReached = true;
}
@@ -230,7 +230,7 @@ void Navigator::CalculateEngines() vBack = ((float)vBack * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
vRight = ((float)vRight * (float)((float)this->robotSpeed / (float)speedCorrection)) + this->rotationSpeed;
- //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vLeft,10,2);
+ (parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,2);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vBack,10,3);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(vRight,10,4);
diff --git a/source/Concept/Framework/modules/executor/navigator.h b/source/Concept/Framework/modules/executor/navigator.h index 53fa74b..989bc04 100755 --- a/source/Concept/Framework/modules/executor/navigator.h +++ b/source/Concept/Framework/modules/executor/navigator.h @@ -67,6 +67,21 @@ public: return (targetX != EMPTY_FLOAT && targetY != EMPTY_FLOAT);
}
+ float GetTargetX()
+ {
+ return targetX;
+ }
+
+ float GetTargetY()
+ {
+ return targetY;
+ }
+
+ float GetDirection()
+ {
+ return direction;
+ }
+
bool HasTargetAngle()
{
return (targetAngle != EMPTY_FLOAT);
diff --git a/source/Concept/Framework/modules/input/mouse_sensor.h b/source/Concept/Framework/modules/input/mouse_sensor.h index b3d1b1a..1f2922a 100755 --- a/source/Concept/Framework/modules/input/mouse_sensor.h +++ b/source/Concept/Framework/modules/input/mouse_sensor.h @@ -214,6 +214,16 @@ public: {
return positionY;
}
+
+ void AdjustPositionX(float newPosX)
+ {
+ this->positionX = newPosX;
+ }
+
+ void AdjustPositionY(float newPosY)
+ {
+ this->positionY = newPosY;
+ }
};
#endif diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.c b/source/Concept/Framework/modules/interpreter/ball_tracker.c index 16fdfb4..a882abb 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.c +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.c @@ -3,6 +3,8 @@ //-----------------------------------------------------------------------------
void Ball_Tracker::Update()
{
+ Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+
uint8 sensorCount = (IO_SENSOR_IR_330_DEG - IO_SENSOR_IR_0_DEG) + 1;
uint16 intensity[sensorCount];
uint8 greatestIntensity = 0;
@@ -26,15 +28,12 @@ void Ball_Tracker::Update() {
if(intensity[i] > BALL_HELD_INTENSITY) // Ball derzeit sehr nah dran
{
- ballHeld = true;
- ballHeldCounter = 100;
+ if(ballHeldCounter < 10) ballHeldCounter++;
}
- else if(ballHeldCounter > 0) // Oder vor kurzem erst sehr nah dran
+ else if(ballHeldCounter > 0) {
- ballHeld = true;
ballHeldCounter--;
}
- else ballHeld = false; // ansonsten hat er den Ball nicht
}
}
@@ -132,13 +131,15 @@ void Ball_Tracker::Update() direction = (intensity[greatestIntensity] * mainDirection +
intensity[secondIntensity] * secondDirection) /
(intensity[greatestIntensity] + intensity[secondIntensity]);
-
- direction = easyAngle(direction);
}
else
{
direction = mainDirection;
}
+
+ direction += ourPositionTracker->GetOrientation();
+
+ direction = easyAngle(direction);
}
else
{
diff --git a/source/Concept/Framework/modules/interpreter/ball_tracker.h b/source/Concept/Framework/modules/interpreter/ball_tracker.h index cb90ff2..756a09f 100755 --- a/source/Concept/Framework/modules/interpreter/ball_tracker.h +++ b/source/Concept/Framework/modules/interpreter/ball_tracker.h @@ -12,7 +12,6 @@ public: this->moduleId = 0;
this->direction = EMPTY_FLOAT;
this->ballHeldCounter = 0;
- this->ballHeld = false;
}
Ball_Tracker(uint32 trackerId)
@@ -21,13 +20,11 @@ public: this->moduleId = trackerId;
this->direction = EMPTY_FLOAT;
this->ballHeldCounter = 0;
- this->ballHeld = false;
}
protected:
float direction;
uint8 ballHeldCounter;
- bool ballHeld;
public:
void Update();
@@ -42,9 +39,9 @@ public: return (direction != EMPTY_FLOAT);
}
- bool RobotHasBall()
+ bool HasBall()
{
- return ballHeld;
+ return (ballHeldCounter >= 3);
}
};
diff --git a/source/Concept/Framework/modules/interpreter/command_handler.c b/source/Concept/Framework/modules/interpreter/command_handler.c index 7d8937b..e2a9ba2 100644 --- a/source/Concept/Framework/modules/interpreter/command_handler.c +++ b/source/Concept/Framework/modules/interpreter/command_handler.c @@ -3,6 +3,7 @@ //-----------------------------------------------------------------------------
void Command_Handler::Update()
{
+ Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
Keyboard* ourKeyboard = parent->GetModule<Keyboard>(IO_KEYBOARD_MAIN);
uint8 curInput = ourKeyboard->GetInput();
@@ -13,11 +14,20 @@ void Command_Handler::Update() if(curInput == 10)
{
ExecuteCommand();
+
+ for(uint8 i = 0; i < this->currentCommandLength; i++)
+ {
+ this->buffer[i] = NULL;
+ ourDisplay->Print(" ", i + 1, 1);
+ }
+
+ this->currentCommandLength = 0;
}
else if(curInput == 11)
{
if(this->currentCommandLength > 0)
{
+ ourDisplay->Print(" ", this->currentCommandLength, 1);
this->currentCommandLength--;
this->buffer[currentCommandLength] = NULL;
}
@@ -28,6 +38,7 @@ void Command_Handler::Update() {
this->buffer[this->currentCommandLength] = curInput;
this->currentCommandLength++;
+ ourDisplay->Print(curInput, this->currentCommandLength, 1);
}
}
@@ -38,8 +49,163 @@ void Command_Handler::Update() //-----------------------------------------------------------------------------
void Command_Handler::ExecuteCommand()
{
- if(this->buffer[0] == 5)
+ if(this->buffer[0] == 1)
+ {
+ if(this->buffer[1] == 1)
+ {
+ this->displayDistanceSensors = true;
+ this->displayPositionTracker = false;
+ this->displayBallTracker = false;
+ this->displayMouseSensors = false;
+ }
+ else if(this->buffer[1] == 2)
+ {
+ this->displayDistanceSensors = false;
+ this->displayPositionTracker = true;
+ this->displayBallTracker = false;
+ this->displayMouseSensors = false;
+ }
+ else if(this->buffer[1] == 3)
+ {
+ this->displayDistanceSensors = false;
+ this->displayPositionTracker = false;
+ this->displayBallTracker = true;
+ this->displayMouseSensors = false;
+ }
+ else if(this->buffer[1] == 4)
+ {
+ this->displayDistanceSensors = false;
+ this->displayPositionTracker = false;
+ this->displayBallTracker = false;
+ this->displayMouseSensors = true;
+ }
+ else if(this->buffer[1] == 0)
+ {
+ this->displayDistanceSensors = false;
+ this->displayPositionTracker = false;
+ this->displayBallTracker = false;
+ this->displayMouseSensors = false;
+ }
+ }
+ else if(this->buffer[0] == 2)
+ {
+ Logic* ourLogic = parent->GetModule<Logic>(IO_LOGIC_MAIN);
+ if(this->buffer[1] == 1)
+ {
+ ourLogic->SetKeeper(true);
+ }
+ else if(this->buffer[1] == 2)
+ {
+ ourLogic->SetKeeper(false);
+ }
+ }
+ else if(this->buffer[0] == 3)
+ {
+ if(this->buffer[1] == 1)
+ {
+ Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+ ourNavigator->RotateTo(180, 200);
+ }
+ else if(this->buffer[1] == 2)
+ {
+ Aktuator* ourAktuator = parent->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
+ ourAktuator->Kick();
+ }
+ else if(this->buffer[1] == 3)
+ {
+ Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+ ourNavigator->RotateTo(float(buffer[2]) * 100.0f + float(buffer[3]) * 10.0f + float(buffer[4]), 200);
+ }
+ else if(this->buffer[1] == 4)
+ {
+ Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+ Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+
+ this->ticksPerCmOffset += 2.5f;
+ ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ }
+ else if(this->buffer[1] == 5)
+ {
+ Mouse_Sensor* ourLeftMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+ Mouse_Sensor* ourRightMouse = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+
+ this->ticksPerCmOffset -= 2.5f;
+ ourLeftMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ ourLeftMouse->AdjustPositionY(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ ourRightMouse->AdjustPositionX(-3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ ourRightMouse->AdjustPositionY(3.88f * (TICKS_PER_CM + this->ticksPerCmOffset));
+ }
+ }
+ else if(this->buffer[0] == 4)
+ {
+ Navigator* ourNavigator = parent->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
+ int16 speed = 200;
+ switch(this->buffer[1])
+ {
+ case 1:
+ ourNavigator->Drive(225, 0, speed, 0);
+ break;
+ case 2:
+ ourNavigator->Drive(180, 0, speed, 0);
+ break;
+ case 3:
+ ourNavigator->Drive(135, 0, speed, 0);
+ break;
+ case 4:
+ ourNavigator->Drive(270, 0, speed, 0);
+ break;
+ case 5:
+ ourNavigator->Rotate(DEFAULT_ROTATION_SPEED);
+ break;
+ case 6:
+ ourNavigator->Drive(90, 0, speed, 0);
+ break;
+ case 7:
+ ourNavigator->Drive(315, 0, speed, 0);
+ break;
+ case 8:
+ ourNavigator->Drive(0, 0, speed, 0);
+ break;
+ case 9:
+ ourNavigator->Drive(45, 0, speed, 0);
+ break;
+ case 0:
+ ourNavigator->Stop();
+ break;
+ }
+ }
+ else if(this->buffer[0] == 5)
+ {
+ Position_Tracker* ourPositionTracker = parent->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
+ if(this->buffer[1] == 1)
+ {
+ ourPositionTracker->SetPosition(float(), ourPositionTracker->GetPositionY(), ourPositionTracker->GetOrientation());
+ }
+ else if(this->buffer[1] == 0)
+ {
+ ourPositionTracker->SetPosition(0, 0, 0);
+ }
+ }
+ else if(this->buffer[0] == 6)
+ {
+ if(this->buffer[1] == 1)
+ {
+ Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);
+ ourWireless->Send("Heyho!");
+ }
+ }
+}
+
+//-----------------------------------------------------------------------------
+void Command_Handler::PrintCommand()
+{
+ Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
+
+ for(uint8 i = 0; i < currentCommandLength; i++)
{
- this->displayDistanceSensors = true;
+ ourDisplay->Print(this->buffer[i], i + 1, 1);
}
}
diff --git a/source/Concept/Framework/modules/interpreter/command_handler.h b/source/Concept/Framework/modules/interpreter/command_handler.h index c3bc5bb..f376e96 100644 --- a/source/Concept/Framework/modules/interpreter/command_handler.h +++ b/source/Concept/Framework/modules/interpreter/command_handler.h @@ -12,6 +12,10 @@ public: this->moduleId = 0;
this->currentCommandLength = 0;
this->displayDistanceSensors = false;
+ this->displayPositionTracker = false;
+ this->displayBallTracker = false;
+ this->displayMouseSensors = false;
+ this->ticksPerCmOffset = 0;
for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)
{
@@ -25,6 +29,10 @@ public: this->moduleId = commandHandlerId;
this->currentCommandLength = 0;
this->displayDistanceSensors = false;
+ this->displayPositionTracker = false;
+ this->displayBallTracker = false;
+ this->displayMouseSensors = false;
+ this->ticksPerCmOffset = 0;
for(uint8 i = 0; i < COMMAND_BUFFER_SIZE; i++)
{
@@ -39,10 +47,17 @@ protected: void ExecuteCommand();
public:
- void Update();
+ void Update();
+
+ void PrintCommand();
//Command variables
bool displayDistanceSensors;
+ bool displayPositionTracker;
+ bool displayBallTracker;
+ bool displayMouseSensors;
+
+ float ticksPerCmOffset;
};
#endif diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index f6d67ac..fab8085 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,6 +3,9 @@ //-----------------------------------------------------------------------------
void Position_Tracker::Update()
{
+ Command_Handler* ourCommandHandler = parent->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
+ Display* ourDisplay = parent->GetModule<Display>(IO_DISPLAY_MAIN);
+
// We want to use the mouse-sensors
Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
@@ -40,6 +43,14 @@ void Position_Tracker::Update() movementRightY = 0;
}
+ if(ourCommandHandler->displayMouseSensors)
+ {
+ ourDisplay->Print(movementLeftX, 1, 2);
+ ourDisplay->Print(movementLeftY, 10, 2);
+ ourDisplay->Print(movementRightX, 1, 3);
+ ourDisplay->Print(movementRightY, 10, 3);
+ }
+
// Generate vector from P:left to P:right
float movementDifferenceX = movementRightX - movementLeftX;
float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
@@ -47,6 +58,11 @@ void Position_Tracker::Update() // Calculate the difference of orientation
float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
+ if(ourCommandHandler->displayMouseSensors)
+ {
+ ourDisplay->Print(orientationDiff * 180.0f / PI, 1, 4);
+ }
+
float robotMovementX = movementDifferenceX / 2.0f;
float robotMovementY = movementDifferenceY / 2.0f;
robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index 56b16c4..7a5781f 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -40,18 +40,20 @@ public: }
// returns x-koordinate in mm
- int GetPositionX() {
- return (int)((positionX*10.0f)/TICKS_PER_CM);
+ int16 GetPositionX()
+ {
+ return (int16)((positionX*10.0f)/TICKS_PER_CM);
}
// returns y-koordinate in mm
- int GetPositionY() {
- return (int)((positionY*10.0f)/TICKS_PER_CM);
+ int16 GetPositionY()
+ {
+ return (int16)((positionY*10.0f)/TICKS_PER_CM);
}
// returns orientation
- float GetOrientation() {
- //return 0.0f; //tmp!!!!!!!!!!
+ float GetOrientation()
+ {
return orientation;
}
};
diff --git a/source/Concept/Framework/modules/logic/logic.c b/source/Concept/Framework/modules/logic/logic.c index f698495..239e426 100755 --- a/source/Concept/Framework/modules/logic/logic.c +++ b/source/Concept/Framework/modules/logic/logic.c @@ -3,16 +3,51 @@ //-----------------------------------------------------------------------------
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()
{
- Wireless* ourWireless = parent->GetModule<Wireless>(IO_WIRELESS_MAIN);
- //ourWireless->Send(WIRELESS_CODE);
- //ourWireless->Send();
+}
+
+//-----------------------------------------------------------------------------
+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();
}
//-----------------------------------------------------------------------------
@@ -20,21 +55,217 @@ 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();
+ }
- // is Keeper?
- if(isKeeper) {
- // turn around al little bit...
- if((status == STATUS_KEEPER_TURN_LEFT && !ourNavigator->IsMoving()) ||
- (status != STATUS_KEEPER_TURN_LEFT && status != STATUS_KEEPER_TURN_RIGHT)) {
+ 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,200);
+ ourNavigator->RotateTo(315, DEFAULT_ROTATION_SPEED);
}
- else if(status == STATUS_KEEPER_TURN_RIGHT && !ourNavigator->IsMoving()) {
+ else
+ {
status = STATUS_KEEPER_TURN_LEFT;
- ourNavigator->RotateTo(45, 200);
+ ourNavigator->RotateTo(45, DEFAULT_ROTATION_SPEED);
}
}
- else { // is Player?
-
+}
+
+//-----------------------------------------------------------------------------
+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);
+ }
}
}
diff --git a/source/Concept/Framework/modules/logic/logic.h b/source/Concept/Framework/modules/logic/logic.h index ad8bad7..031e0b6 100755 --- a/source/Concept/Framework/modules/logic/logic.h +++ b/source/Concept/Framework/modules/logic/logic.h @@ -12,6 +12,8 @@ public: this->moduleId = 0;
this->isKeeper = false;
this->status = 0;
+ this->hasBall = false;
+ this->avoidsObstacle = false;
}
Logic(uint32 logicId)
@@ -20,16 +22,29 @@ public: this->moduleId = logicId;
this->isKeeper = false;
this->status = 0;
+ this->hasBall = false;
+ this->avoidsObstacle = false;
}
protected:
bool isKeeper;
uint8 status;
+ bool hasBall;
+ bool avoidsObstacle;
enum LogicalStatus
{
STATUS_KEEPER_TURN_RIGHT,
STATUS_KEEPER_TURN_LEFT,
+ STATUS_KEEPER_RETURN_HOME,
+ STATUS_KEEPER_HUNT_BALL,
+ STATUS_KEEPER_TURN_TO_BALL,
+ STATUS_KEEPER_DRIVE_TO_DEFEND,
+
+ STATUS_ATTACKER_TURN_TO_BALL,
+ STATUS_ATTACKER_DRIVE_TO_BALL,
+ STATUS_ATTACKER_SEARCHING_AT_HOME,
+ STATUS_ATTACKER_SEARCHING_AT_ENEMY,
};
void OnBallOwned();
@@ -51,7 +66,8 @@ public: return !isKeeper;
}
- void SetKeeper(bool newStatus) {
+ void SetKeeper(bool newStatus)
+ {
if(!this->isKeeper && newStatus)
{
this->OnBecomeKeeper();
@@ -65,8 +81,11 @@ public: bool HasBall()
{
- //fill me!
+ return this->hasBall;
}
+
+ void UpdateKeeperMovement();
+ void UpdateAttackerMovement();
};
#endif |