From f61eb900879fe941460a284ff9e4681b0958b944 Mon Sep 17 00:00:00 2001 From: sicarius Date: Mon, 26 Feb 2007 21:25:01 +0000 Subject: SoccerTeam, Stuff from Magdeburg --- .../Concept/Framework/modules/executor/navigator.c | 6 +- .../Concept/Framework/modules/executor/navigator.h | 15 ++ .../Concept/Framework/modules/input/mouse_sensor.h | 10 + .../Framework/modules/interpreter/ball_tracker.c | 15 +- .../Framework/modules/interpreter/ball_tracker.h | 7 +- .../modules/interpreter/command_handler.c | 170 +++++++++++++- .../modules/interpreter/command_handler.h | 17 +- .../modules/interpreter/position_tracker.c | 16 ++ .../modules/interpreter/position_tracker.h | 14 +- source/Concept/Framework/modules/logic/logic.c | 257 +++++++++++++++++++-- source/Concept/Framework/modules/logic/logic.h | 23 +- 11 files changed, 511 insertions(+), 39 deletions(-) (limited to 'source/Concept/Framework/modules') 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(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(IO_DISPLAY_MAIN))->Print(vLeft,10,2); + (parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,2); //(parent->GetModule(IO_DISPLAY_MAIN))->Print(vBack,10,3); //(parent->GetModule(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(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(IO_DISPLAY_MAIN); Keyboard* ourKeyboard = parent->GetModule(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(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(IO_NAVIGATOR_MAIN); + ourNavigator->RotateTo(180, 200); + } + else if(this->buffer[1] == 2) + { + Aktuator* ourAktuator = parent->GetModule(IO_AKTUATOR_MAIN); + ourAktuator->Kick(); + } + else if(this->buffer[1] == 3) + { + Navigator* ourNavigator = parent->GetModule(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(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* ourRightMouse = parent->GetModule(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(IO_SENSOR_MOUSE_LEFT); + Mouse_Sensor* ourRightMouse = parent->GetModule(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(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(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(IO_WIRELESS_MAIN); + ourWireless->Send("Heyho!"); + } + } +} + +//----------------------------------------------------------------------------- +void Command_Handler::PrintCommand() +{ + Display* ourDisplay = parent->GetModule(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(IO_COMMAND_HANDLER_MAIN); + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); + // We want to use the mouse-sensors Mouse_Sensor* mouseLeft = parent->GetModule(IO_SENSOR_MOUSE_LEFT); Mouse_Sensor* mouseRight = parent->GetModule(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(IO_AKTUATOR_MAIN); + + ourAktuator->Kick(); + this->SetKeeper(wasKeeper); + } } //----------------------------------------------------------------------------- void Logic::OnBallLost() { - Wireless* ourWireless = parent->GetModule(IO_WIRELESS_MAIN); - //ourWireless->Send(WIRELESS_CODE); - //ourWireless->Send(); +} + +//----------------------------------------------------------------------------- +void Logic::OnBecomeKeeper() +{ + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Navigator* ourNavigator = parent->GetModule(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(IO_NAVIGATOR_MAIN); + + ourNavigator->Stop(); + + UpdateAttackerMovement(); } //----------------------------------------------------------------------------- @@ -20,21 +55,217 @@ void Logic::Update() { // We want to use a navigator Navigator* ourNavigator = parent->GetModule(IO_NAVIGATOR_MAIN); + Ball_Tracker* ourBallTracker = parent->GetModule(IO_BALL_TRACKER_MAIN); + Display* ourDisplay = parent->GetModule(IO_DISPLAY_MAIN); + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Obstacle_Tracker* ourObstacleTracker = parent->GetModule(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(IO_BALL_TRACKER_MAIN); + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Navigator* ourNavigator = parent->GetModule(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(IO_BALL_TRACKER_MAIN); + Position_Tracker* ourPositionTracker = parent->GetModule(IO_POSITION_TRACKER_MAIN); + Navigator* ourNavigator = parent->GetModule(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 -- cgit v1.2.3