Arduino_Robot/CollisionAvoidance.cpp at master · Christiaan2/Arduino_Robot · 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
//
//
//

#include "CollisionAvoidance.h"

CollisionAvoidance::CollisionAvoidance(int trigPin, int echoPin, int maxDistance, Propulsion* propulsion)
	: sonar(trigPin, echoPin, maxDistance), flagVeryClose(false), flagClose(false), propulsion(propulsion), timer(FREQ)
{//trigPin and echoPin can be the same
}

void CollisionAvoidance::run()
{
	if (flagVeryClose || flagClose)
	{
		avoidObstical();
	}
	else
	{
		unsigned int distance = sonar.ping_cm();
		if (distance <= VeryClose && distance != 0)
		{
			flagVeryClose = true;
		}
		else if (distance <= Close && distance != 0)
		{
			flagClose = true;
		}
		else
		{
			propulsion->setForwards();
		}
	}
}

void CollisionAvoidance::avoidObstical()
{
	static int distanceL;
	static int distanceR;
	switch (propulsion->getProgress()) {
	case 0:
		propulsion->setAllowSpeedControl(false);
		propulsion->setSpeed(2);
		servo.write(175);
		timer.setTimer(500);
		propulsion->incrementProgress();
		break;
	case 1:
		if (timer.fire())
		{
			distanceR = sonar.ping_cm();
			servo.write(5);
			timer.setTimer(800);
			propulsion->incrementProgress();
		}
		break;
	case 2:
		if (timer.fire())
		{
			distanceL = sonar.ping_cm();
			servo.write(90);
			propulsion->reset();
			timer.setTimer(800);
			propulsion->incrementProgress();
		}
		break;
	case 3:
		if (flagVeryClose)
		{
			propulsion->setBackwards(40);
		}
		else
		{
			propulsion->incrementProgress();
		}
		break;
	case 4:
		if (distanceL >= distanceR && distanceR != 0)
		{
			//turn 120 degrees to left
			propulsion->setLeftRotation(18);
		}
		else
		{
			propulsion->setRightRotation(18);
		}
		break;
	case 5:
		timer.setTimer(800);
		propulsion->incrementProgress();
		break;
	default:
		if (timer.fire())
		{
			propulsion->setProgress(0);
			propulsion->setForwards();
			flagVeryClose = false;
			flagClose = false;
		}
	}
}

void CollisionAvoidance::initialize(int servoPin)
{ // Can't use attach inside constructor
	servo.attach(servoPin);
	servo.write(90);
}