diff options
Diffstat (limited to 'source/Concept/Framework/modules/interpreter')
| -rwxr-xr-x | source/Concept/Framework/modules/interpreter/position_tracker.c | 62 | ||||
| -rwxr-xr-x | source/Concept/Framework/modules/interpreter/position_tracker.h | 6 |
2 files changed, 67 insertions, 1 deletions
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.c b/source/Concept/Framework/modules/interpreter/position_tracker.c index 262069a..c8cd52d 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.c +++ b/source/Concept/Framework/modules/interpreter/position_tracker.c @@ -3,5 +3,65 @@ //-----------------------------------------------------------------------------
void Position_Tracker::Update()
{
- //insert code here
+ Mouse_Sensor* mouseLeft = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_LEFT);
+ Mouse_Sensor* mouseRight = parent->GetModule<Mouse_Sensor>(IO_SENSOR_MOUSE_RIGHT);
+
+ int8 leftX = mouseLeft->GetXMovement();
+ int8 leftY = mouseLeft->GetYMovement();
+ float distanceLeft = sqrt(leftX * leftX + leftY * leftY);
+ float angleLeft = easyAngle(atan2(leftY, leftX) + (225.0f * PI / 180.0f));
+
+ float movementLeftX = cos(angleLeft) * distanceLeft;
+ float movementLeftY = sin(angleLeft) * distanceLeft;
+
+ if(!leftX && !leftY)
+ {
+ movementLeftX = 0;
+ movementLeftY = 0;
+ }
+
+ int8 rightX = mouseRight->GetXMovement();
+ int8 rightY = mouseRight->GetYMovement();
+ float distanceRight = sqrt(rightX * rightX + rightY * rightY);
+ float angleRight = easyAngle(atan2(rightY, rightX) - (45.0f * PI / 180.0f));
+
+ float movementRightX = cos(angleRight) * distanceRight;
+ float movementRightY = sin(angleRight) * distanceRight;
+
+ if(!rightX && !rightY)
+ {
+ movementRightX = 0;
+ movementRightY = 0;
+ }
+
+ float movementDifferenceX = movementRightX - movementLeftX;
+ float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
+
+ float robotMovementX = movementDifferenceX / 2.0f;
+ float robotMovementY = movementDifferenceY / 2.0f;
+ robotMovementX += movementLeftX;
+ robotMovementY += movementLeftY + mouseLeft->GetPositionY();
+ float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);
+
+ float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
+
+ float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementX);
+ float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f)) * robotDistance * sign(robotMovementY);
+
+ if(!robotMovementX && !robotMovementY)
+ {
+ absoluteDiffX = 0;
+ absoluteDiffY = 0;
+ }
+
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(" ", 5, 1);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffX, 5, 1);
+ //(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(absoluteDiffY, 12, 1);
+
+ this->positionX += absoluteDiffX;
+ this->positionY += absoluteDiffY;
+
+ this->orientation += orientationDiff;
+
+ this->orientation = easyAngle(this->orientation);
}
diff --git a/source/Concept/Framework/modules/interpreter/position_tracker.h b/source/Concept/Framework/modules/interpreter/position_tracker.h index b31dd8f..e93ef0c 100755 --- a/source/Concept/Framework/modules/interpreter/position_tracker.h +++ b/source/Concept/Framework/modules/interpreter/position_tracker.h @@ -10,12 +10,18 @@ public: {
this->parent = NULL;
this->moduleId = 0;
+ this->positionX = 0;
+ this->positionY = 0;
+ this->orientation = 0;
}
Position_Tracker(uint32 trackerId)
{
this->parent = NULL;
this->moduleId = trackerId;
+ this->positionX = 0;
+ this->positionY = 0;
+ this->orientation = 0;
}
protected:
|