neocturne/rc2007-soccer
Archived
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

+++ Codework, Navigator&Positiontracker done (buggy?)

Browse source
This commit is contained in:
sicarius 2007-02-20 21:01:02 +00:00
parent ec2a18e931
commit 22433e52a7
9 changed files with 168 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

62
source/Concept/Framework/modules/interpreter/position_tracker.c
View file

@ -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);
}

6
source/Concept/Framework/modules/interpreter/position_tracker.h
View file

@ -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: