77 lines
3.3 KiB
C
Executable file
77 lines
3.3 KiB
C
Executable file
#include "position_tracker.h"
|
|
|
|
//-----------------------------------------------------------------------------
|
|
void Position_Tracker::Update()
|
|
{
|
|
// 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);
|
|
|
|
// Generate a vector for the left mouse
|
|
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;
|
|
|
|
// AVR calculates a little bit strange ;)
|
|
if(!leftX && !leftY)
|
|
{
|
|
movementLeftX = 0;
|
|
movementLeftY = 0;
|
|
}
|
|
|
|
// Generate a vector for the right mouse
|
|
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;
|
|
|
|
// AVR calculates a little bit strange ;)
|
|
if(!rightX && !rightY)
|
|
{
|
|
movementRightX = 0;
|
|
movementRightY = 0;
|
|
}
|
|
|
|
// Generate vector from P:left to P:right
|
|
float movementDifferenceX = movementRightX - movementLeftX;
|
|
float movementDifferenceY = (movementRightY + mouseRight->GetPositionY()) - (movementLeftY + mouseLeft->GetPositionY());
|
|
|
|
// Calculate the difference of orientation
|
|
float orientationDiff = atan2(movementDifferenceY, movementDifferenceX) - (PI / 2.0f);
|
|
|
|
float robotMovementX = movementDifferenceX / 2.0f;
|
|
float robotMovementY = movementDifferenceY / 2.0f;
|
|
robotMovementX += movementLeftX + mouseLeft->GetPositionX() + (-mouseLeft->GetPositionX() * cos(orientationDiff));
|
|
robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));
|
|
//float robotDistance = sqrt(robotMovementX * robotMovementX + robotMovementY * robotMovementY);
|
|
|
|
float absoluteDiffX = cos(this->orientation + (orientationDiff / 2.0f)) * robotMovementX - sin(this->orientation + (orientationDiff / 2.0f)) * robotMovementY;
|
|
float absoluteDiffY = sin(this->orientation + (orientationDiff / 2.0f) + PI / 2.0f) * robotMovementY + cos(this->orientation + (orientationDiff / 2.0f) - PI / 2.0f) * robotMovementX;
|
|
|
|
//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(robotMovementY + movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff)), 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);
|
|
}
|