Project-K9/SensorHelper.cpp at master · MLenterman/Project-K9 · 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
#include "SensorHelper.h"

SensorHelper::SensorHelper(Sensor* sensor){
	this->sensor = sensor;
	this->frontWall = false;
}

SensorHelper::~SensorHelper(){

}

void SensorHelper::addSensorDeviationPoint(CoordSet beforeDeviationPoint, CoordSet deviationPoint){
	this->sensorDeviationPoints.push_back(beforeDeviationPoint);
	this->sensorDeviationPoints.push_back(deviationPoint);
}

void SensorHelper::addSensorFrontWall(CoordSet frontWallPos){
	this->sensorDeviationPoints.push_back(frontWallPos);
	this->frontWall = true;
}

bool SensorHelper::analyze(){
	if(frontWall){
		// calculate on what axis the measurement was added/substracted based on current sensor and k9 orientation
		bool plusAxisX = false;
		bool minusAxisX = false;
		bool plusAxisY = false;
		bool minusAxisY = false;

		this->calcMeasurementOperationFromOrientations(sensor->getOrientation(), k9->positionControl->getCurrentOrientation(), plusAxisX, minusAxisX, plusAxisY, minusAxisY);

		// Process all deviation points except for the frontwall point
		for(int i = 0; i < this->sensorDeviationPoints.size() - 1; i + 2){
			// Calculate center between the CoordSet before the deviation and at the deviation
			CoordSet centerPoint;

			// Based on the measurement operation that was performed
			if(plusAxisY || minusAxisY){

			}
			if(plusAxisX || minusAxisX){

			}
		}

	}
}

void SensorHelper::calcMeasurementOperationFromOrientations(Direction sensorOrientation, Direction k9Orientation, bool& plusAxisX, bool& minusAxisX, bool& plusAxisY, bool& minusAxisY){
	if(sensorOrientation == NORTH){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	plusAxisY = true;
					break;
		case EAST:	plusAxisX = true;
					break;
		case SOUTH:	minusAxisY = true;
					break;
		case WEST:	minusAxisX = true;
					break;
		}
	}
	if(sensorOrientation == EAST){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	plusAxisX = true;
					break;
		case EAST:	minusAxisY = true;
					break;
		case SOUTH:	minusAxisX = true;
					break;
		case WEST:	plusAxisY = true;
					break;
		}
	}
	if(sensorOrientation == SOUTH){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	minusAxisY = true;
					break;
		case EAST:	minusAxisX = true;
					break;
		case SOUTH:	plusAxisY = true;
					break;
		case WEST:	plusAxisX = true;
					break;
		}
	}
	if(sensorOrientation == WEST){ // Sensor orientation
		switch(k9Orientation){ // k9 orientation
		case NORTH:	minusAxisX = true;
					break;
		case EAST:	plusAxisY = true;
					break;
		case SOUTH:	plusAxisX = true;
					break;
		case WEST:	minusAxisY = true;
					break;
		}
	}
}

bool SensorHelper::isSimplePath(){
	if(this->sensorDeviationPoints.size() < 6){
		CoordSet wallPosBetweenFirstDeviation;
		CoordSet wallPosBetweenSecondDeviation;

		bool measurementOnX = false;
		bool measurementOnY = false;

		// Calculate center between the position before and during the deviation
		wallPosBetweenFirstDeviation = calcCenterBetweenMeasurementPoints(sensorDeviationPoints[0], sensorDeviationPoints[1]);

		// Fill the missing axis with the measurement before the first deviation
		if(wallPosBetweenFirstDeviation.x == 0){
			wallPosBetweenFirstDeviation.x = sensorDeviationPoints[0].x;
			measurementOnX = true;
		}
		if(wallPosBetweenFirstDeviation.y == 0){
			wallPosBetweenFirstDeviation.y = sensorDeviationPoints[0].y;
			measurementOnY = true;
		}

		if(sensorDeviationPoints.size() == 3 && frontWall){
			CoordSet correctedFrontWallPos = sensorDeviationPoints[3];

			// Set measurement axis to the measurement before the first deviation
			if(measurementOnX)
				correctedFrontWallPos.x = sensorDeviationPoints[0].x;
			if(measurementOnY)
				correctedFrontWallPos.y = sensorDeviationPoints[0].y;

			CoordSet nodePos = calcCenterBetweenPoints(wallPosBetweenFirstDeviation, correctedFrontWallPos);
			nodePos
			Node* newNode = new Node();
			newNode->setMarked(true);
			newNode->setPosition(


		}
	}
}

bool SensorHelper::isWideningPath(){
}