|
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
|
Modificato: Ottobre 2012
|
|
6
|
Modificato: Marzo 2013
|
|
7
|
Inserito un arresto al momento del riconoscimento di un ostacolo.
|
|
8
|
Rallentato il movimento nell'andatura in avanti.
|
|
9
|
Autore : Cristina Begliomini
|
|
10
|
*/
|
|
11
|
#include <Servo.h>
|
|
12
|
|
|
13
|
Servo frontServo;
|
|
14
|
Servo rearServo;
|
|
15
|
/* Servo motors - global variables */
|
|
16
|
int centerPos = 90;
|
|
17
|
int frontRightUp = 72;
|
|
18
|
int frontLeftUp = 108;
|
|
19
|
int backRightForward = 75;
|
|
20
|
int backLeftForward = 105;
|
|
21
|
int walkSpeed = 150; // How long to wait between steps in milliseconds
|
|
22
|
int centerTurnPos = 96;
|
|
23
|
int frontTurnRightUp = 81;
|
|
24
|
int frontTurnLeftUp = 99;
|
|
25
|
int backTurnRightForward = 81;
|
|
26
|
int backTurnLeftForward = 111;
|
|
27
|
|
|
28
|
/* Misurazione della distanza - global variables */
|
|
29
|
int pingPin = 4;
|
|
30
|
long int duration;
|
|
31
|
long distanceFront=0; //cm
|
|
32
|
int startAvoidanceDistance=20; //distanza massima in cm per considerare un ostacolo
|
|
33
|
|
|
34
|
int danger = 0; // no obstacle
|
|
35
|
int frontRight = 0; // front right leg in low position
|
|
36
|
int slowMotion = 50; // delay amount for moving forwards
|
|
37
|
|
|
38
|
long microsecondsToCentimeters(long microseconds)
|
|
39
|
{
|
|
40
|
return microseconds / 29 / 2;
|
|
41
|
}
|
|
42
|
|
|
43
|
long distanceCm(){
|
|
44
|
pinMode(pingPin, OUTPUT);
|
|
45
|
digitalWrite(pingPin, LOW);
|
|
46
|
delayMicroseconds(2);
|
|
47
|
digitalWrite(pingPin, HIGH);
|
|
48
|
delayMicroseconds(5);
|
|
49
|
digitalWrite(pingPin, LOW);
|
|
50
|
|
|
51
|
pinMode(pingPin, INPUT);
|
|
52
|
duration = pulseIn(pingPin, HIGH);
|
|
53
|
|
|
54
|
return microsecondsToCentimeters(duration);
|
|
55
|
}
|
|
56
|
|
|
57
|
void moveForward()
|
|
58
|
{
|
|
59
|
int frontPos = frontServo.read(); //checks frontServo position
|
|
60
|
int rearPos = rearServo.read(); //checks rearServo position
|
|
61
|
if (frontRight==0) { // moves front right leg up
|
|
62
|
for (; frontPos >= frontRightUp; frontPos--) {
|
|
63
|
frontServo.write(frontPos);
|
|
64
|
delay(slowMotion);
|
|
65
|
}
|
|
66
|
for (; rearPos <= backLeftForward; rearPos++) {
|
|
67
|
rearServo.write(rearPos);
|
|
68
|
delay(slowMotion);
|
|
69
|
}
|
|
70
|
frontRight = 1;
|
|
71
|
} else { // moves front left leg up
|
|
72
|
for (; frontPos <= frontLeftUp; frontPos++) {
|
|
73
|
frontServo.write(frontPos);
|
|
74
|
delay(slowMotion);
|
|
75
|
}
|
|
76
|
for (; rearPos >= backRightForward; rearPos--) {
|
|
77
|
rearServo.write(rearPos);
|
|
78
|
delay(slowMotion);
|
|
79
|
}
|
|
80
|
frontRight = 0;
|
|
81
|
}
|
|
82
|
}
|
|
83
|
|
|
84
|
void moveBackRight()
|
|
85
|
{
|
|
86
|
frontServo.write(frontRightUp);
|
|
87
|
rearServo.write(backRightForward-6);
|
|
88
|
delay(125);
|
|
89
|
frontServo.write(centerPos);
|
|
90
|
rearServo.write(centerPos-6);
|
|
91
|
delay(65);
|
|
92
|
frontServo.write(frontLeftUp+9);
|
|
93
|
rearServo.write(backLeftForward-6);
|
|
94
|
delay(125);
|
|
95
|
|
|
96
|
frontServo.write(centerPos);
|
|
97
|
rearServo.write(centerPos);
|
|
98
|
delay(65);
|
|
99
|
}
|
|
100
|
|
|
101
|
void moveTurnLeft()
|
|
102
|
{
|
|
103
|
frontServo.write(frontTurnRightUp);
|
|
104
|
rearServo.write(centerTurnPos);
|
|
105
|
delay(65);
|
|
106
|
rearServo.write(backTurnLeftForward);
|
|
107
|
delay(125);
|
|
108
|
frontServo.write(centerTurnPos);
|
|
109
|
delay(65);
|
|
110
|
frontServo.write(frontTurnLeftUp);
|
|
111
|
rearServo.write(centerTurnPos);
|
|
112
|
delay(65);
|
|
113
|
rearServo.write(backTurnRightForward);
|
|
114
|
delay(125);
|
|
115
|
frontServo.write(centerTurnPos);
|
|
116
|
delay(65);
|
|
117
|
}
|
|
118
|
|
|
119
|
void setup()
|
|
120
|
{
|
|
121
|
frontServo.attach(2);
|
|
122
|
rearServo.attach(3);
|
|
123
|
pinMode(pingPin, OUTPUT);
|
|
124
|
}
|
|
125
|
|
|
126
|
void loop()
|
|
127
|
{
|
|
128
|
distanceFront=distanceCm();
|
|
129
|
if (distanceFront > 1){ // Filters out any stray 0.00 error readings
|
|
130
|
if (distanceFront<startAvoidanceDistance) {
|
|
131
|
if (danger==0) { // stops at first obstacle detection
|
|
132
|
delay(1000);
|
|
133
|
danger = 1;
|
|
134
|
}
|
|
135
|
for(int i=0; i<=8; i++) {
|
|
136
|
moveBackRight();
|
|
137
|
delay(walkSpeed);
|
|
138
|
}
|
|
139
|
for(int i=0; i<=10; i++) {
|
|
140
|
distanceFront=distanceCm(); // inseramo un controllo della presenza di ostacoli anche in fase di cambio di direzione
|
|
141
|
if (distanceFront > 1){ // Filters out any stray 0.00 error readings
|
|
142
|
if (distanceFront<startAvoidanceDistance) {
|
|
143
|
moveBackRight();
|
|
144
|
delay(walkSpeed);
|
|
145
|
} else {
|
|
146
|
danger = 0;
|
|
147
|
moveTurnLeft();
|
|
148
|
}
|
|
149
|
}
|
|
150
|
}
|
|
151
|
} else {
|
|
152
|
moveForward();
|
|
153
|
}
|
|
154
|
}
|
|
155
|
}
|
|
156
|
|