RobotSupervision/RobotObject.cpp at master · geg2048/RobotSupervision · 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
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#include "RobotObject.h"
#include "stdlib.h"

RobotObject::RobotObject() :
		_RobotState(UNKNOWN), _robId(0) {
}

RobotObject::RobotObject(int robId) :
		RobotObject() {
	_robId = robId;
}

RobotObject::~RobotObject() {
}

// Returns Point of aIndex
Vect2D RobotObject::GetPoint(int aIndex) {
	return _PointArray.at(aIndex);
}
// Return true when RobotObject has Points
bool RobotObject::HasPoints() {
	return (_PointArray.size() > 0);
}
// Accessing the value of the OldPointArray
Vect2D RobotObject::GetOldPoint(int aIndex) {
	return _OldPointArray.at(aIndex);
}
// Saving the new position of the Robot
void RobotObject::SetNewPoints(std::vector<Vect2D> aPoints) {
	_OldPointArray = _PointArray;
	_PointArray = aPoints;
	CalcNewCenter();
	CalcNewSpeed();
}
// Accessing the value of the CenterPoint
Vect2D RobotObject::GetCenter() {
	return _CenterPoint;
}
// Accessing the value of the OldCenterPoint
Vect2D RobotObject::GetOldCenter() {
	return _OldCenterPoint;
}
// Accessing the value of the SppedVect
Vect2D RobotObject::GetSpeed() {
	return _SpeedVect;
}
// Returns a Vector that points in the direction the Robot points to
Vect2D RobotObject::GetDirVect() {

	std::vector<double> arr;
	arr.resize(_PointArray.size());
	int i, j, maxFound = 0;
	double maxValue = 0;

	for (i = 0; i < _PointArray.size(); i++) {
		double sum = 0;
		for (j = 0; j < _PointArray.size(); j++) {
			if (i != j) {
				sum += _PointArray[i].DistBetweenPoints(_PointArray[j]);
			}
		}
		arr[i] = sum;
	}

	for (i = 0; i < _PointArray.size(); i++) {
		if (arr[i] > maxValue) {
			maxValue = arr[i];
			maxFound = i;
		}
	}

	return VectBetweenPoints(GetCenter(), _PointArray[maxFound]);
}
int RobotObject::GetRobRad() {
	return this->GetDirVect().VectLength();
}
// Sets Vars for turning by aGrad
void RobotObject::StartRotationBy(int aGrad, bool aClockwise) {
	if (aGrad > 170) {
		GradToTurn = 170;
	} else {
		GradToTurn = aGrad;
	}
	StartingVect = GetDirVect();
	if (aClockwise) {
		//RCTurnClockwise();
	} else {
		//RCTurnCounterClockwise();
	}
}
// Check if Rotation is finished
bool RobotObject::CheckIfRotationIsDone() {
	if (std::abs(StartingVect.AngleBetweenVect_Grad(GetDirVect()))
			>= GradToTurn) {
		RCForward();
		return true;
	} else {
		return false;
	}
}
// Changing the state of the Robot ( one function per state )
void RobotObject::SetRobotToIDLE() {
	_RobotState = IDLE;
}
void RobotObject::SetRobotToDANGERBYROBOT() {
	_RobotState = DANGERBYROBOT;
}
void RobotObject::SetRobotToDANGERBYWALL() {
	_RobotState = DANGERBYWALL;
}
void RobotObject::SetRobotToLOST() {
	_RobotState = LOST;
}
e_RobotStates RobotObject::GetRobotState() {
	return _RobotState;
}
// Socket Verwaltung

void RobotObject::CalcNewCenter() {
	if (_PointArray.size() == 3) {
		int SumOfXCords = 0, SumOfYCords = 0;

		for (int i = 0; i < _PointArray.size(); i++) {
			SumOfXCords += _PointArray[i].m_X;
			SumOfYCords += _PointArray[i].m_Y;
		}
		Vect2D tmp = Vect2D(SumOfXCords / 3, SumOfYCords / 3, false);
		_OldCenterPoint = _CenterPoint;
		_CenterPoint = tmp;
	}
}

void RobotObject::CalcNewSpeed() {
	_SpeedVect = VectBetweenPoints(_OldCenterPoint, _CenterPoint);
}

int RobotObject::GetRobId() {
	return _robId;
}

void RobotObject::RCHalt() {
	controlCmd = HALT;
}

void RobotObject::RCForward() {
	controlCmd = FORWARD;
}

void RobotObject::RCTurnClockwise() {
	controlCmd = TURNCLOCKWISE;
}
void RobotObject::RCTurnCounterClockwise() {
	controlCmd = TURNCOUNTERCLOCKWISE;
}