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 --- .../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 +- 6 files changed, 218 insertions(+), 21 deletions(-) (limited to 'source/Concept/Framework/modules/interpreter') 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; } }; -- cgit v1.2.3