diff options
Diffstat (limited to 'source/Concept/Framework/modules/interpreter')
6 files changed, 218 insertions, 21 deletions
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;
}
};
|