Arduino robot

FullSizeRender2

Little two wheel drive robot. The ultrasonic sensor is mounted on a servo so it can look around. The solar battery charger has two outputs, one powers the motors and the other powers the Arduino.

Needs some bump sensors at the front to detect low obstacles. The code includes some Bluetooth telemetry data but that’s not been tested yet.

//Motors
#include <stdio.h>
#include <AFMotor.h>
#include <Servo.h>
AF_DCMotor leftWheel(1);
AF_DCMotor rightWheel(2);
Servo headServo;
#define HEADSERVO 10
const int cal = 0;
int currentSpeedLeft = 0;
int currentSpeedRight = 0;
const int rightSpeed = 160;
const int leftSpeed = 159;

//Ultrasonic ping sensor
const int trig = 24;
const int echo = 22;
float lastRange;
#define RANGEOK 26
#define RANGEBAD 28
#define LASER 30
#define LASEROFF 32
#define SAFEDISTANCE 25
#define FAIRLYSAFEDISTANCE 35

//Front bumper
#define BUMPERINPUT 34
#define RIGHTWHISKER 36
#define LEFTWHISKER 38

//Temperature
//—-DHT setup
#include “DHT.h”
#define DHTTYPE DHT11   // DHT 11
#define DHTPIN 40 //Temp humidity sensor
DHT dht(DHTPIN, DHTTYPE);

//Light sensor
#define LIGHTSENSOR 12 //analog pin

//Cautious digital input
#define GOSLOW 42

int headPos = 0;
const int loopDelay = 500;
const int turn90Delay = 750;
const int turn180Delay = 1250;
void setup()
{
Serial.begin(9600);
Serial1.begin(9600);//Bluetooth

headServo.attach(HEADSERVO);

leftWheel.setSpeed(0);
leftWheel.run(RELEASE);

rightWheel.setSpeed(0);
rightWheel.run(RELEASE);

pinMode(trig,OUTPUT);
pinMode(echo,INPUT);
pinMode(RANGEOK,OUTPUT);
pinMode(RANGEBAD,OUTPUT);
pinMode(LASER,OUTPUT);

pinMode(BUMPERINPUT, INPUT);

headServo.write(90);
rampToSpeed(leftSpeed, FORWARD, rightSpeed, FORWARD);
}

void loop()
{
boolean bumbersOK;
boolean distanceOK;

bumbersOK = readBumbers();
distanceOK = measureDistance(SAFEDISTANCE);

if((!bumbersOK)||(!distanceOK))
{
changeDirection();
return;
}
sendTelemetry();
delay(loopDelay);
}

void rampToSpeed(int spdLeft, int dirLeft, int spdRight, int dirRight)
{
leftWheel.run(dirLeft);
rightWheel.run(dirRight);
currentSpeedLeft=spdLeft;
currentSpeedRight=spdRight;
leftWheel.setSpeed(currentSpeedLeft);
rightWheel.setSpeed(currentSpeedRight);
return;

float leftStep, rightStep;
if((currentSpeedLeft==spdLeft)&&(currentSpeedRight==spdRight))
{
return;
}
leftStep = (spdLeft – currentSpeedLeft)/10.0;
rightStep = (spdRight – currentSpeedRight)/10.0;
leftWheel.run(dirLeft);
rightWheel.run(dirRight);

for(int i = 1; i<=10; i++)
{
currentSpeedLeft+=leftStep;
currentSpeedRight+=rightStep;
leftWheel.setSpeed(currentSpeedLeft);
rightWheel.setSpeed(currentSpeedRight);
delay(10);
}
currentSpeedLeft=spdLeft;
currentSpeedRight=spdRight;
leftWheel.setSpeed(currentSpeedLeft);
rightWheel.setSpeed(currentSpeedRight);
}

boolean readBumbers()
{
if(digitalRead(BUMPERINPUT)==HIGH)
{
//All ok
}
else
{
//problem
}
return true;
}

boolean measureDistance(int minDistance)
{
long duration;
long distance;
boolean measureOK = false;
Serial.println(“Measure”);
for(int i=0; i<5;i++)
{
duration = doMeasure();
distance = microsecondsToCentimeters(duration);
Serial.println(distance);
lastRange = distance;

if(distance>0)
{
measureOK = true;
break;
}
Serial.println(“Distance Error”);
digitalWrite(RANGEOK,HIGH);
digitalWrite(RANGEBAD,HIGH);
delay(10);
}
if(!measureOK)
{
return true;//usually over range??
}

Serial.println(distance);

if(distance<minDistance)
{
digitalWrite(RANGEOK,LOW);
digitalWrite(RANGEBAD,HIGH);
return false;
}
else
{
digitalWrite(RANGEOK,HIGH);
digitalWrite(RANGEBAD,LOW);
return true;
}
return false;
}

long doMeasure()
{
digitalWrite(trig, LOW);
delayMicroseconds(2);
digitalWrite(trig,HIGH);
delayMicroseconds(5);
digitalWrite(trig,LOW);
long duration = pulseIn(echo, HIGH);
return duration;
}

void changeDirection()
{
leftWheel.run(FORWARD);
leftWheel.setSpeed(0);
rightWheel.run(FORWARD);
rightWheel.setSpeed(0);

//Check right
headServo.write(20);
delay(1500);
Serial.println(“Check right”);
if(measureDistance(FAIRLYSAFEDISTANCE))
{
rampToSpeed(leftSpeed, FORWARD, rightSpeed, BACKWARD);
sendTelemetry();
delay(turn90Delay);
rampToSpeed(0, FORWARD, 0, FORWARD);
headServo.write(90);
sendTelemetry();
delay(500);
rampToSpeed(leftSpeed, FORWARD, rightSpeed, FORWARD);
return;
}
//Check left
headServo.write(160);
delay(1500);
Serial.println(“Check left”);
if(measureDistance(FAIRLYSAFEDISTANCE))
{
rampToSpeed(leftSpeed, BACKWARD, rightSpeed, FORWARD);
sendTelemetry();
delay(turn90Delay);
rampToSpeed(0, FORWARD, 0, FORWARD);
headServo.write(90);
sendTelemetry();
delay(500);
rampToSpeed(leftSpeed, FORWARD, rightSpeed, FORWARD);
return;
}
//full 180
rampToSpeed(leftSpeed, BACKWARD, rightSpeed, BACKWARD);
sendTelemetry();
delay(turn90Delay);
rampToSpeed(leftSpeed, FORWARD, rightSpeed, BACKWARD);
sendTelemetry();
delay(turn180Delay);
rampToSpeed(0, FORWARD, 0, FORWARD);
headServo.write(90);
sendTelemetry();
rampToSpeed(leftSpeed, FORWARD, rightSpeed, FORWARD);
delay(500);
}
union data
{
float f;
byte b[4];
};
void sendTelemetry()
{
union data converter;
byte digital;
Serial1.print(“#”);
digital = digitalRead(BUMPERINPUT);
Serial1.write(digital);
digital = digitalRead(RIGHTWHISKER);
Serial1.write(digital);
digital = digitalRead(LEFTWHISKER);
Serial1.write(digital);
converter.f = currentSpeedLeft;
Serial1.write(converter.b, 4);
converter.f = currentSpeedRight;
Serial1.write(converter.b, 4);
converter.f = dht.readHumidity();
Serial1.write(converter.b, 4);
converter.f = dht.readTemperature();
Serial1.write(converter.b, 4);
converter.f = analogRead(LIGHTSENSOR);
Serial1.write(converter.b, 4);
converter.f = 0.0f;
Serial1.write(converter.b, 4);
converter.f = 0.0f;
Serial1.write(converter.b, 4);
converter.f = 0.0f;
Serial1.write(converter.b, 4);
converter.f = 0.0f;
Serial1.write(converter.b, 4);
converter.f = 0.0f;
Serial1.write(converter.b, 4);
converter.f = 0.0f;
Serial1.write(converter.b, 4);
converter.f = lastRange;
Serial1.write(converter.b, 4);
}

long microsecondsToCentimeters(long microseconds)
{
return microseconds/29/2;
}

Add a Comment

Your email address will not be published. Required fields are marked *