1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
#include "position_tracker.h"
//-----------------------------------------------------------------------------
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);
// Generate a vector for the left mouse
int8 leftX = mouseLeft->GetXMovement();
int8 leftY = mouseLeft->GetYMovement();
// Generate a vector for the right mouse
int8 rightX = mouseRight->GetXMovement();
int8 rightY = mouseRight->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;
}
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;
}
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());
// 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));
robotMovementY += movementLeftY + mouseLeft->GetPositionY() + (mouseLeft->GetPositionX() * sin(orientationDiff));
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;
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);
//(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(" ", 5, 1);
/*(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Clear();
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->orientation * 180.0f / PI, 5, 1);
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionX, 1, 2);
(parent->GetModule<Display>(IO_DISPLAY_MAIN))->Print(this->positionY, 1, 3);*/
}
|
