VSANAND & MMONG – DEMO4

The robot is designed to respond negatively when someone gets close to it. using the ultrasonic sensor, it detects whether an object is too close to it. When the approaching object gets to close, the robot panics and attempts to scare the approaching threat by shaking its arms vigorously. When the object retreats, the robot calms down and returns to its initial state.

The robot is constructed using 6mm plywood, 2 servo motors (one for each arm) and an ultrasonic sensor.

CAD:Demo4 (1)

VIDEO:

 

CODE:

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
#include <NewPing.h>
#include <Servo.h>
 
const int ARM1_PIN = 9;
const int ARM2_PIN = 10;
 
Servo arm1;
Servo arm2;
 
int armPos1 = 12;
int armPos2 = 12;
 
static int state = 0;
static long elapsed_time = 0;
static long lastPingTime = 0;
 
//static int avgAc = 10;
static int distanceVals[10];
static int updateCount = 0;
 
static int distance = -1; // start at -1 to avoid triggering any behaviors before setup is complete. Check 0< when doing cases
static unsigned long last_update_clock = 0; // from class slides to set up timer
 
#define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 11 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
 
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
 
void setup() {
Serial.begin(115200); // Open serial monitor at 115200 baud to see ping results.
arm1.attach(ARM1_PIN);
arm2.attach(ARM2_PIN);
}
 
//void moveHead(int directionMotion, int location, int timeMove)
//{
 
//}
 
long takeAverage(int a[], int len)
{
int sum = 0;
for (int i = 0; i < len; i++)
{
sum += a[i];
}
return sum / (double)len;
}
 
int moveServo(Servo servo, int lastPos, int nextPos, int duration)
{
unsigned long last_update_clock = micros();
 
int currentLocation = lastPos;
long magnitude = (abs(nextPos - lastPos));
if(magnitude < 5){
return 0;
}
long timeSteps = duration / magnitude/1000;
int servoSteps = magnitude / (duration/1000 / timeSteps);
int loops = 1;
int runningTotal = lastPos;
 
while (duration > 0) {
unsigned long now = micros();
unsigned long interval = now - last_update_clock;
last_update_clock = now;
duration -= interval;
if (lastPos > nextPos)
{
servo.write(lastPos + (loops * servoSteps));
}
else
{
servo.write(lastPos - (loops * servoSteps));
}
loops += 1;
 
updateDistance(interval);
 
if (updateState() != 0)
{
return 1;
}
}
return 0;
}
 
int updateState()
{
if (distance > 70 || distance == 0) //account of for out of range
{
if (state != 0)
{
state = 0;
return 1;
}
}
 
if (distance < 40 && distance >0)
{
if (state != 1)
{
state = 1;
return 1;
}
}
return 0;
}
 
void updateDistance(long delays)
{
if (lastPingTime <= 0)
{
if (updateCount = 10)
{
distance = takeAverage(distanceVals, 10); //change to take an average of a few measurements
updateCount = 0;
}
distanceVals[updateCount] = int(sonar.ping_cm());
updateCount += 1;
distance = sonar.ping_cm();
lastPingTime = 1000; // set delay here
}
else
{
lastPingTime -= delays;
}
}
 
void loop() {
unsigned long now = micros();
unsigned long interval = now - last_update_clock;
last_update_clock = now;
 
updateDistance(interval);
updateState();
 
switch (state)
{
case 0:
if (moveServo(arm1, armPos1, 20, 1000)) {
break;
}
break;
 
case 1:
if (moveServo(arm1, armPos1, 180, 2000)) {
break;
}
}
}