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:


#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;
}
}
}