|
1
|
/*
|
|
2
|
Progetto : Insetto Robot - HackLab Terni
|
|
3
|
Il robot controlla la presenza di ostacoli, se ci sono cambia direzione, altrimenti procede in avanti.
|
|
4
|
Luglio 2012
|
|
5
|
Autore : Cristina Begliomini
|
|
6
|
*/
|
|
7
|
#include <Servo.h>
|
|
8
|
|
|
9
|
Servo frontServo;
|
|
10
|
Servo rearServo;
|
|
11
|
/* Servo motors - global variables */
|
|
12
|
int centerPos = 90;
|
|
13
|
int frontRightUp = 72;
|
|
14
|
int frontLeftUp = 108;
|
|
15
|
int backRightForward = 75;
|
|
16
|
int backLeftForward = 105;
|
|
17
|
int walkSpeed = 150; // How long to wait between steps in milliseconds
|
|
18
|
int centerTurnPos = 96;
|
|
19
|
int frontTurnRightUp = 81;
|
|
20
|
int frontTurnLeftUp = 99;
|
|
21
|
int backTurnRightForward = 81;
|
|
22
|
int backTurnLeftForward = 111;
|
|
23
|
|
|
24
|
/* Misurazione della distanza - global variables */
|
|
25
|
int pingPin = 4;
|
|
26
|
long int duration;
|
|
27
|
long distanceFront=0; //cm
|
|
28
|
int startAvoidanceDistance=20; //distanza massima in cm per considerare un ostacolo
|
|
29
|
|
|
30
|
long microsecondsToCentimeters(long microseconds)
|
|
31
|
{
|
|
32
|
return microseconds / 29 / 2;
|
|
33
|
}
|
|
34
|
|
|
35
|
long distanceCm(){
|
|
36
|
pinMode(pingPin, OUTPUT);
|
|
37
|
digitalWrite(pingPin, LOW);
|
|
38
|
delayMicroseconds(2);
|
|
39
|
digitalWrite(pingPin, HIGH);
|
|
40
|
delayMicroseconds(5);
|
|
41
|
digitalWrite(pingPin, LOW);
|
|
42
|
|
|
43
|
pinMode(pingPin, INPUT);
|
|
44
|
duration = pulseIn(pingPin, HIGH);
|
|
45
|
|
|
46
|
return microsecondsToCentimeters(duration);
|
|
47
|
}
|
|
48
|
|
|
49
|
void moveForward()
|
|
50
|
{
|
|
51
|
frontServo.write(frontRightUp);
|
|
52
|
rearServo.write(centerPos);
|
|
53
|
delay(65);
|
|
54
|
rearServo.write(backLeftForward);
|
|
55
|
delay(125);
|
|
56
|
frontServo.write(centerPos);
|
|
57
|
delay(65);
|
|
58
|
frontServo.write(frontLeftUp);
|
|
59
|
rearServo.write(centerPos);
|
|
60
|
delay(65);
|
|
61
|
rearServo.write(backRightForward);
|
|
62
|
delay(125);
|
|
63
|
frontServo.write(centerPos);
|
|
64
|
delay(65);
|
|
65
|
}
|
|
66
|
|
|
67
|
void moveBackRight()
|
|
68
|
{
|
|
69
|
frontServo.write(frontRightUp);
|
|
70
|
rearServo.write(backRightForward-6);
|
|
71
|
delay(125);
|
|
72
|
frontServo.write(centerPos);
|
|
73
|
rearServo.write(centerPos-6);
|
|
74
|
delay(65);
|
|
75
|
frontServo.write(frontLeftUp+9);
|
|
76
|
rearServo.write(backLeftForward-6);
|
|
77
|
delay(125);
|
|
78
|
|
|
79
|
frontServo.write(centerPos);
|
|
80
|
rearServo.write(centerPos);
|
|
81
|
delay(65);
|
|
82
|
}
|
|
83
|
|
|
84
|
void moveTurnLeft()
|
|
85
|
{
|
|
86
|
frontServo.write(frontTurnRightUp);
|
|
87
|
rearServo.write(centerTurnPos);
|
|
88
|
delay(65);
|
|
89
|
rearServo.write(backTurnLeftForward);
|
|
90
|
delay(125);
|
|
91
|
frontServo.write(centerTurnPos);
|
|
92
|
delay(65);
|
|
93
|
frontServo.write(frontTurnLeftUp);
|
|
94
|
rearServo.write(centerTurnPos);
|
|
95
|
delay(65);
|
|
96
|
rearServo.write(backTurnRightForward);
|
|
97
|
delay(125);
|
|
98
|
frontServo.write(centerTurnPos);
|
|
99
|
delay(65);
|
|
100
|
}
|
|
101
|
|
|
102
|
void setup()
|
|
103
|
{
|
|
104
|
frontServo.attach(2);
|
|
105
|
rearServo.attach(3);
|
|
106
|
pinMode(pingPin, OUTPUT);
|
|
107
|
}
|
|
108
|
|
|
109
|
void loop()
|
|
110
|
{
|
|
111
|
distanceFront=distanceCm();
|
|
112
|
if (distanceFront > 1){ // Filters out any stray 0.00 error readings
|
|
113
|
if (distanceFront<startAvoidanceDistance) {
|
|
114
|
for(int i=0; i<=8; i++) {
|
|
115
|
moveBackRight();
|
|
116
|
delay(walkSpeed);
|
|
117
|
}
|
|
118
|
for(int i=0; i<=10; i++) {
|
|
119
|
moveTurnLeft();
|
|
120
|
}
|
|
121
|
} else {
|
|
122
|
moveForward();
|
|
123
|
}
|
|
124
|
}
|
|
125
|
}
|