SoccerTeam, Stuff from Magdeburg
This commit is contained in:
sicarius
parent
343397ecf6
commit
f61eb90087
17 changed files with 666 additions and 122 deletions
File diff suppressed because one or more lines are too long
|
|
@ -34,13 +34,13 @@ HEX_EEPROM_FLAGS += --change-section-lma .eeprom=0
|
|||
|
||||
|
||||
## Include Directories
|
||||
INCLUDES = -I"Y:\Concept\Framework\modules" -I"Y:\Concept\Framework\modules\executor" -I"Y:\Concept\Framework\modules\input" -I"Y:\Concept\Framework\modules\interpreter" -I"Y:\Concept\Framework\modules\logic" -I"Y:\Concept\Framework\modules\output"
|
||||
INCLUDES = -I"Z:\rc2007-soccer\source\Concept\Framework\modules" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\executor" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\input" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\interpreter" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\logic" -I"Z:\rc2007-soccer\source\Concept\Framework\modules\output"
|
||||
|
||||
## Libraries
|
||||
LIBS = -lm
|
||||
|
||||
## Objects that must be built in order to link
|
||||
OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o aktuator.o wireless.o logic.o
|
||||
OBJECTS = io_module.o atmega128io.o main.o robot.o tools.o distance_sensor.o ir_sensor.o keyboard.o mouse_sensor.o sensor.o position_tracker.o display.o dribbler.o engine.o kicker.o led.o ball_tracker.o navigator.o aktuator.o wireless.o logic.o command_handler.o obstacle_tracker.o
|
||||
|
||||
## Objects explicitly added by the user
|
||||
LINKONLYOBJECTS =
|
||||
|
|
@ -112,6 +112,12 @@ wireless.o: ../modules/wireless.c
|
|||
logic.o: ../modules/logic/logic.c
|
||||
$(CC) $(INCLUDES) $(CFLAGS) -c $<
|
||||
|
||||
command_handler.o: ../modules/interpreter/command_handler.c
|
||||
$(CC) $(INCLUDES) $(CFLAGS) -c $<
|
||||
|
||||
obstacle_tracker.o: ../modules/interpreter/obstacle_tracker.c
|
||||
$(CC) $(INCLUDES) $(CFLAGS) -c $<
|
||||
|
||||
##Link
|
||||
$(TARGET): $(OBJECTS)
|
||||
$(CC) $(LDFLAGS) $(OBJECTS) $(LINKONLYOBJECTS) $(LIBDIRS) $(LIBS) -o $(TARGET)
|
||||
|
|
|
|||
|
|
@ -60,6 +60,9 @@
|
|||
#define DISTANCE_PER_VALUE 1
|
||||
#define TICKS_PER_CM 205.0f
|
||||
//#define TICKS_PER_CM 90.0f
|
||||
#define ULTRASONIC_PER_CM 2.0f
|
||||
|
||||
#define WIRELESS_ACTIVE false
|
||||
|
||||
#define PI 3.14159265358979323846f
|
||||
#define CYCLES_PER_CORRECTION 20
|
||||
|
|
@ -67,8 +70,39 @@
|
|||
|
||||
#define BALL_HELD_INTENSITY 900
|
||||
|
||||
#define KEEPER_LEFT_ANGLE 20.0f * PI / 180.0f
|
||||
#define KEEPER_RIGHT_ANGLE (2 * PI) - (20.0f * PI / 180.0f)
|
||||
|
||||
#define COMMAND_BUFFER_SIZE 20
|
||||
|
||||
#define DEFAULT_SPEED 200
|
||||
#define DEFAULT_ROTATION_SPEED 200
|
||||
|
||||
#define AREA_BOUNDS_X 20000.0f
|
||||
#define AREA_BOUNDS_Y 5000.0f
|
||||
|
||||
#define WALL_DISTANCE_TOLERANCE 30.0f
|
||||
#define ENEMY_ROBOT_RADIUS 40.0f
|
||||
|
||||
#define HOME_LOC_X -500.0f
|
||||
#define HOME_LOC_Y 0
|
||||
#define ENEMY_LOC_X 500.0f
|
||||
#define ENEMY_LOC_Y 0
|
||||
#define DEFENCE_L_LOC_X -500.0f
|
||||
#define DEFENCE_L_LOC_Y 70.0f
|
||||
#define DEFENCE_R_LOC_X -500.0f
|
||||
#define DEFENCE_R_LOC_Y -70.0f
|
||||
|
||||
#define DISTANCE_TOLERANCE 1.0f
|
||||
#define ORIENTATION_TOLERANCE 0.1f
|
||||
|
||||
#define MAX_OBSTACLE_COUNT 20
|
||||
|
||||
#define OBSTACLE_DECAY 100
|
||||
|
||||
#define BALL_DIRECTION_FRONT_L 45.0f * PI / 180.0f
|
||||
#define BALL_DIRECTION_FRONT_R 315.0f * PI / 180.0f
|
||||
|
||||
#define WIRELESS_CODE "SPASS!"
|
||||
enum WirelessCommands
|
||||
{
|
||||
|
|
@ -183,9 +217,17 @@ enum IOModuleNames
|
|||
|
||||
IO_POSITION_TRACKER_END,
|
||||
|
||||
//Position Tracker
|
||||
//Obstacle Tracker
|
||||
|
||||
IO_BALL_TRACKER_START = IO_POSITION_TRACKER_END,
|
||||
IO_OBSTACLE_TRACKER_START = IO_POSITION_TRACKER_END,
|
||||
|
||||
IO_OBSTACLE_TRACKER_MAIN,
|
||||
|
||||
IO_OBSTACLE_TRACKER_END,
|
||||
|
||||
//Ball Tracker
|
||||
|
||||
IO_BALL_TRACKER_START = IO_OBSTACLE_TRACKER_END,
|
||||
|
||||
IO_BALL_TRACKER_MAIN,
|
||||
|
||||
|
|
|
|||
|
|
@ -130,6 +130,14 @@ int main()
|
|||
newCommandHandler = NULL;
|
||||
}
|
||||
|
||||
//Init Obstacle Tracker
|
||||
for(uint8 i = IO_OBSTACLE_TRACKER_START; i < IO_OBSTACLE_TRACKER_END; i++)
|
||||
{
|
||||
Obstacle_Tracker* newObstacleTracker = new Obstacle_Tracker(i);
|
||||
localRobot->AddModule(newObstacleTracker);
|
||||
newObstacleTracker = NULL;
|
||||
}
|
||||
|
||||
//Init Position Tracker
|
||||
for(uint8 i = IO_POSITION_TRACKER_START; i < IO_POSITION_TRACKER_END; i++)
|
||||
{
|
||||
|
|
@ -179,33 +187,62 @@ int main()
|
|||
Command_Handler* ourCommandHandler = localRobot->GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN);
|
||||
uint32 i = 1;
|
||||
Navigator* ourNavigator = localRobot->GetModule<Navigator>(IO_NAVIGATOR_MAIN);
|
||||
Position_Tracker* ourPosition_Tracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
||||
Position_Tracker* ourPositionTracker = localRobot->GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN);
|
||||
Ball_Tracker* ourBallTracker = localRobot->GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN);
|
||||
Dribbler* ourDribbler = localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN);
|
||||
ourDribbler->SetSpeed(1);
|
||||
ourDribbler->SetEnabled(true);
|
||||
//ourDribbler->SetSpeed(1);
|
||||
//ourDribbler->SetEnabled(true);
|
||||
Aktuator* ourAktuator = localRobot->GetModule<Aktuator>(IO_AKTUATOR_MAIN);
|
||||
Logic* ourLogic = localRobot->GetModule<Logic>(IO_LOGIC_MAIN);
|
||||
Wireless* ourWireless = localRobot->GetModule<Wireless>(IO_WIRELESS_MAIN);
|
||||
|
||||
float rotation = 0;
|
||||
float speed = 200;
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(200);
|
||||
|
||||
//Mouse_Sensor* mouse_left = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
|
||||
//Mouse_Sensor* mouse_right = localRobot->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
|
||||
|
||||
ourDisplay->Clear();
|
||||
//sleep(2);
|
||||
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(200);
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);
|
||||
|
||||
//sleep(2);
|
||||
|
||||
|
||||
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(200);
|
||||
//(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
|
||||
|
||||
/*(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetSpeed(200);
|
||||
(localRobot->GetModule<Dribbler>(IO_DRIBBLER_MAIN))->SetEnabled(true);*/
|
||||
|
||||
/*(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetSpeed(80);
|
||||
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_RIGHT))->SetEnabled(true);*/
|
||||
|
||||
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetSpeed(255);
|
||||
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_BACK))->SetEnabled(true);
|
||||
|
||||
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetSpeed(255);
|
||||
(localRobot->GetModule<Engine>(IO_ENGINE_DRIVE_LEFT))->SetEnabled(true);
|
||||
|
||||
sleep(2);
|
||||
|
||||
//Run
|
||||
while(true)
|
||||
{
|
||||
ourDisplay->Print(i++,1,1);
|
||||
|
||||
//msleep(50);
|
||||
|
||||
if(ourCommandHandler->displayDistanceSensors && !(i % 20))
|
||||
i++;
|
||||
|
||||
/*if(!(i % 20))
|
||||
{
|
||||
ourDisplay->Clear();
|
||||
msleep(10);
|
||||
ourCommandHandler->PrintCommand();
|
||||
}
|
||||
|
||||
localRobot->Update();
|
||||
|
||||
if(ourCommandHandler->displayDistanceSensors && !(i % 20))
|
||||
{
|
||||
distanceSensor = localRobot->GetModule<Distance_Sensor>(IO_SENSOR_DISTANCE_0_DEG);
|
||||
value = distanceSensor->GetDistance();
|
||||
ourDisplay->Print(value, 1, 4);
|
||||
|
|
@ -223,69 +260,43 @@ int main()
|
|||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(";");
|
||||
}
|
||||
|
||||
/*uint8 someInput = ourKeyboard->GetInput();
|
||||
//ourDisplay->Print("Ready to accept...", 1, 2);
|
||||
switch(someInput)
|
||||
|
||||
if(ourCommandHandler->ticksPerCmOffset && !(i % 20))
|
||||
{
|
||||
case 0:
|
||||
ourNavigator->Stop();
|
||||
return 0;
|
||||
break;
|
||||
case 1:
|
||||
ourNavigator->Drive(225, rotation, speed, rotation);
|
||||
break;
|
||||
case 2:
|
||||
ourNavigator->Drive(180, rotation, speed, rotation);
|
||||
break;
|
||||
case 3:
|
||||
ourNavigator->Drive(135, rotation, speed, rotation);
|
||||
break;
|
||||
case 4:
|
||||
ourNavigator->Drive(270, rotation, speed, rotation);
|
||||
break;
|
||||
case 5:
|
||||
ourNavigator->Drive(0, rotation, 0, rotation);
|
||||
break;
|
||||
case 6:
|
||||
ourNavigator->Drive(90, rotation, speed, rotation);
|
||||
break;
|
||||
case 7:
|
||||
ourNavigator->Drive(315, rotation, speed, rotation);
|
||||
break;
|
||||
case 8:
|
||||
ourNavigator->Drive(0, rotation, speed, rotation);
|
||||
break;
|
||||
case 9:
|
||||
ourNavigator->Drive(45, rotation, speed, rotation);
|
||||
break;
|
||||
case 10:
|
||||
ourPosition_Tracker->SetPosition(0,0,0); // Reset Position_Tracker
|
||||
break;
|
||||
case 11:
|
||||
ourLogic->SetKeeper(true); // Reset Position_Tracker
|
||||
break;
|
||||
}*/
|
||||
|
||||
//ourDisplay->Clear();
|
||||
if(!(i % 20))
|
||||
ourDisplay->Print(ourCommandHandler->ticksPerCmOffset * 10.0f, 10, 1);
|
||||
}
|
||||
|
||||
if(ourCommandHandler->displayPositionTracker && !(i % 20))
|
||||
{
|
||||
ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
|
||||
ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
|
||||
ourDisplay->Print(ourPositionTracker->GetOrientation(), 1, 4);
|
||||
}
|
||||
|
||||
if(ourCommandHandler->displayPositionTracker && !(i % 20))
|
||||
{
|
||||
ourDisplay->Print(ourPositionTracker->GetPositionX(), 1, 2);
|
||||
ourDisplay->Print(ourPositionTracker->GetPositionY(), 1, 3);
|
||||
ourDisplay->Print(ourPositionTracker->GetOrientation() * 180.0f / PI, 1, 4);
|
||||
}
|
||||
|
||||
if(ourCommandHandler->displayBallTracker && !(i % 20))
|
||||
{
|
||||
/*ourDisplay->Clear();
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_0_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value, 1, 2);
|
||||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_30_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(value, 6, 2);
|
||||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_60_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(value, 11, 2);
|
||||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_100_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(value, 16, 2);
|
||||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_180_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
|
|
@ -293,15 +304,15 @@ int main()
|
|||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_260_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(value, 6, 3);
|
||||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_300_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(value, 11, 3);
|
||||
ourDisplay->Print(";");
|
||||
ourSensor = localRobot->GetModule<IR_Sensor>(IO_SENSOR_IR_330_DEG);
|
||||
value = ourSensor->GetIRIntensity();
|
||||
ourDisplay->Print(value);
|
||||
ourDisplay->Print(value, 16, 3);
|
||||
ourDisplay->Print(";");
|
||||
if(ourBallTracker->KnowsBallDirection())
|
||||
{
|
||||
|
|
@ -309,15 +320,27 @@ int main()
|
|||
}
|
||||
else
|
||||
{
|
||||
ourDisplay->Print("Not found oder so", 1, 4);
|
||||
}*/
|
||||
ourDisplay->Print("Ball not found", 1, 4);
|
||||
}
|
||||
|
||||
if(ourBallTracker->HasBall()) {
|
||||
ourDisplay->Print("Has Ball", 5, 4);
|
||||
}
|
||||
}
|
||||
|
||||
localRobot->Update();
|
||||
|
||||
//ourDisplay->Print(ourBallTracker->GetBallDirection() * 180.0f / PI, 1, 4);
|
||||
//if(!(i % 20))
|
||||
//{
|
||||
// ourDisplay->Print(ourWireless->Recieve(), 10, 1);
|
||||
//}
|
||||
|
||||
//ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
|
||||
if(ourBallTracker->KnowsBallDirection())
|
||||
{
|
||||
ourNavigator->Drive(ourBallTracker->GetBallDirection() * 180.0f / PI, 0, 255, 0);
|
||||
}
|
||||
else
|
||||
{
|
||||
ourNavigator->Stop();
|
||||
}*/
|
||||
}
|
||||
|
||||
//Cleanup
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -214,6 +214,16 @@ public:
|
|||
{
|
||||
return positionY;
|
||||
}
|
||||
|
||||
void AdjustPositionX(float newPosX)
|
||||
{
|
||||
this->positionX = newPosX;
|
||||
}
|
||||
|
||||
void AdjustPositionY(float newPosY)
|
||||
{
|
||||
this->positionY = newPosY;
|
||||
}
|
||||
};
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -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
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
{
|
||||
this->displayDistanceSensors = true;
|
||||
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++)
|
||||
{
|
||||
ourDisplay->Print(this->buffer[i], i + 1, 1);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
// 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)) {
|
||||
status = STATUS_KEEPER_TURN_RIGHT;
|
||||
ourNavigator->RotateTo(315,200);
|
||||
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 if(status == STATUS_KEEPER_TURN_RIGHT && !ourNavigator->IsMoving()) {
|
||||
status = STATUS_KEEPER_TURN_LEFT;
|
||||
ourNavigator->RotateTo(45, 200);
|
||||
else
|
||||
{
|
||||
UpdateAttackerMovement();
|
||||
}
|
||||
}
|
||||
else { // is Player?
|
||||
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -90,15 +90,15 @@ void Robot::Update()
|
|||
{
|
||||
GetModule<Command_Handler>(IO_COMMAND_HANDLER_MAIN)->Update();
|
||||
|
||||
GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
|
||||
|
||||
GetModule<Position_Tracker>(IO_POSITION_TRACKER_MAIN)->Update();
|
||||
|
||||
GetModule<Logic>(IO_LOGIC_MAIN)->Update();
|
||||
GetModule<Ball_Tracker>(IO_BALL_TRACKER_MAIN)->Update();
|
||||
|
||||
GetModule<Obstacle_Tracker>(IO_OBSTACLE_TRACKER_MAIN)->Update();
|
||||
|
||||
//GetModule<Logic>(IO_LOGIC_MAIN)->Update();
|
||||
|
||||
GetModule<Navigator>(IO_NAVIGATOR_MAIN)->Update();
|
||||
|
||||
//insert code here
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -22,6 +22,7 @@
|
|||
#include "mouse_sensor.h"
|
||||
#include "command_handler.h"
|
||||
#include "position_tracker.h"
|
||||
#include "obstacle_tracker.h"
|
||||
#include "ball_tracker.h"
|
||||
#include "navigator.h"
|
||||
#include "logic.h"
|
||||
|
|
|
|||
Reference in a new issue