Steamworks/Position.cpp at master · Techbrick/Steamworks · GitHub

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
97
98
99
/*

 * Position.cpp
 *
 *  Created on: Jan 14, 2017
 *      Author: Owner
 */

/*
#include "Position.h"


using namespace std;


Coordinates Position::getRawCenter() const
{

	float centerX = navX.GetDisplacementX() * 39.307 + Constants::navXDistance * std::cos(Constants::navXAngle + angleOffset) + initial.x;
	float centerY = navX.GetDisplacementY() * 39.307 - Constants::navXDistance * std::cos(Constants::navXAngle + angleOffset)+ initial.y;
	Coordinates tempC(centerX, centerY);
	return tempC;

}


Coordinates Position::getCenter() const
{

	Coordinates raw = getRawCenter();
	float centerX = raw.x + xOffset;
	float centerY = raw.y + yOffset;
	Coordinates center(centerX, centerY);
	return center;

}


void Position::updateWithBoiler(const float& angle, const float& range)
{

	float trueAngle = (getAngle() + angle >= 360) ? getAngle() + angle - 360 : getAngle() + angle;
	bool blueBoiler = trueAngle >= 270 || trueAngle <= 90;
	float cameraX = 0.0, cameraY = 0.0;

	if (blueBoiler)
	{

		cameraX = Constants::blueBoilerX + range * std::sin(trueAngle);
		cameraY = Constants::blueBoilerY - range * std::cos(trueAngle);

	}
	else
	{

		cameraX = Constants::redBoilerX + range * sin(trueAngle);
		cameraY = Constants::redBoilerY - range * std::cos(trueAngle);

	}

	float cameraAngleFromCenter = (Constants::cameraAngle + getAngle() >= 360) ? Constants::cameraAngle + getAngle() - 360 : Constants::cameraAngle + getAngle();
	float robotCenterX = cameraX + Constants::cameraDistance * std::cos(cameraAngleFromCenter);
	float robotCenterY = cameraY - Constants::cameraDistance * std::cos(cameraAngleFromCenter);

	Coordinates tempRaw = getRawCenter();
	xOffset = robotCenterX - tempRaw.x;
	yOffset = robotCenterY - tempRaw.y;

	long int* temp = NULL;

	fixed = time(temp);

}


Compiled Position::getAll() const
{

	Compiled all;
	long int* temp = NULL;
	Coordinates center = getCenter();
	all.position = center;
	Coordinates offsets(xOffset, yOffset);
	all.navXError = offsets;
	all.timeSinceLastFix = time(temp) - fixed;
	return all;

}*/