Autonomous Robot Part 3 – Marco Robo

Marco Robo

Team members: Ruben Markowitz, Bryan Gardiner, Sara Johnson

Roles: Bryan Gardiner(Integrator and Scribe), Sara Johnson(Integrator and Designer), Ruben Markowitz(Designer and Tutor)

Introduction

Marco Robo is a robot that explores its surroundings and creates a visual and physical map that shows how the robot perceives the outside world. In the third iteration of the project, we redesigned the device to rotate around a fixed axis so Marco Robo interprets the change in a room over time.

Marco Robo rolls around a center pivot, and uses a sonar sensor to measure the distance of objects in front of it. If the robot sees nothing in its range, it will rotate on the outer ring of its motion. When objects move into its field of view, it will mark what it sees by driving inward toward its axis and making a divot in its path. Overtime, the robot creates a circular map of the location and frequency of disturbances.

Video

Technical Specs

The movement of Marco Robo is driven by a geared motor, controlling its angular position, and a stepper motor, controlling its radial location. The robot moves on two omni wheels that allow free radial and axial movement, and one fixed wheel that drives it radially. Marco Robo uses a sonar sensor to collect and average the distance of objects immediately in front of it, and then scales the object size to the length of the gear arm. The drive box then moves this distance as it turns, moving the marker fixed to the drive box, and creates a map over time.

Circuit Schematic

Code

//Marco Robo
//Ruben Markowitz, Bryan Gardiner, Sara Johnson

//*************************IMPORTS**************************************************//

#include <Wire.h>
#include <NewPing.h>
#include <AccelStepper.h>
//*************************GLOBAL DEFINITIONS***************************************//

//Digital Pin Definitions
int rMotorDirectionPin = 2;
int rMotorStepPin = 3;
int thetaMotorDirectionPin1 = 4;
int thetaMotorDirectionPin2 = 5;
int innerHomeSensorLightPin = 6;
int outerHomeSensorLightPin = 7;
int rangeEcho = 8;
int rangeTrig = 9;
int rangeMax = 200;

//Analog Pin Definitions
int innerHomeSensorPin = 0;
int outerHomeSensorPin = 1;
int gyroPin = 2;

//DirectionModifiers
boolean rMotorDirectionSet = false;
boolean thetaMotorDirectionSet = true;

//Threshold Modifiers
int innerHomeSensorLow = 700;
int outerHomeSensorLow = 200;
int sonarSamples = 20;
float driveGearCircum = 1.67894;
int driveWheelCircum = 54;
int stepperStepsPerRev = 200;
int robotLen = 15.1;

//Dynamics
float currentTheta = 0;
float currentDistance = 0;
float currentR = 0;
float nextR = 10;
float travelLength = 0;
float currentCircum = 0;
float rangeToRFactor = 0;

//Class Instances
NewPing range(rangeTrig, rangeEcho, rangeMax);

//****************************DRIVE CODE***************************************//
void driveR(String dir){
if(rMotorDirectionSet){
if(dir == “forward”){
digitalWrite(rMotorDirectionPin, HIGH);
}
else if(dir == “backward”){
digitalWrite(rMotorDirectionPin, LOW);
}
}
else{
if(dir == “forward”){
digitalWrite(rMotorDirectionPin, LOW);
}
else if(dir == “backward”){
digitalWrite(rMotorDirectionPin, HIGH);
}
}
//Serial.print(“Stepping “);
//Serial.println(dir);
digitalWrite(rMotorStepPin, HIGH);
delay(1);
digitalWrite(rMotorStepPin, LOW);
delay(1);
}

void driveTheta(String dir){
if(thetaMotorDirectionSet){
if(dir == “forward”){
digitalWrite(thetaMotorDirectionPin1, HIGH);
digitalWrite(thetaMotorDirectionPin2, LOW);
}
else if(dir == “backward”){
digitalWrite(thetaMotorDirectionPin1, LOW);
digitalWrite(thetaMotorDirectionPin2, HIGH);
}
}
else{
if(dir == “forward”){
digitalWrite(thetaMotorDirectionPin1, LOW);
digitalWrite(thetaMotorDirectionPin2, HIGH);
}
else if(dir == “backward”){
digitalWrite(thetaMotorDirectionPin1, HIGH);
digitalWrite(thetaMotorDirectionPin2, LOW);
}
}
//Serial.print(“Stepping “);
//Serial.println(dir);

delay(1);
}
//**************************RANGE CODE********************************************//
void detectRange(){
//Serial.println(“Gathering Sonar Data…”);
int pingData = 0;
for(int i = 0; i < sonarSamples; i ++){
pingData = (range.ping() / US_ROUNDTRIP_CM);
//Serial.print(“Ping Data: “);
//Serial.println(pingData);
currentDistance += pingData;
}
currentDistance /= sonarSamples;
if(currentDistance>=50) currentDistance=55;
//Serial.print(“Current Distance is: “);
//Serial.println(currentDistance);
}

//********************CALIBRATE CODE*********************************************//
void calibrateR(){
/*
//Serial.println(“Calibrating R”);
digitalWrite(innerHomeSensorLightPin, HIGH);
//Serial.println(“Inner Sensor Light is On”);
int homeSensorVal = analogRead(innerHomeSensorPin);
innerHomeSensorLow=homeSensorVal;
//Serial.print(“Current Light Sensor Value is: “);
//Serial.println(homeSensorVal);
while((homeSensorVal-innerHomeSensorLow)<25 and (innerHomeSensorLow-homeSensorVal)<25){
driveR(“backward”);
homeSensorVal = analogRead(innerHomeSensorPin);
//Serial.print(“Current Light Sensor Value is: “);
//Serial.println(homeSensorVal);
}
//Serial.println(“Switching Direction”);
digitalWrite(innerHomeSensorLightPin, LOW);
digitalWrite(outerHomeSensorLightPin, HIGH);
delay(500);
homeSensorVal = analogRead(outerHomeSensorPin);
//Serial.print(“Current Light Sensor Value is: “);
//Serial.println(homeSensorVal);
while(homeSensorVal >= outerHomeSensorLow){
driveR(“forward”);
travelLength += 1;
homeSensorVal = analogRead(innerHomeSensorPin);
//Serial.print(“Current Light Sensor Value is: “);
//Serial.println(homeSensorVal);
}
digitalWrite(outerHomeSensorLightPin, LOW);
travelLength = travelLength / 200;
travelLength = travelLength / driveGearCircum;
//Serial.print(“Travel Length is: “);
//Serial.print(travelLength);
//Serial.println(” cm”);
currentR = travelLength;
//Serial.println(“Finished Calibrating R”);
*/
currentR=10;
travelLength=50;
}

void calibrateRange(){
//Serial.println(“Calibrating Sonar Sensor LOL”);
float rangeOffsetMean;
int pingData = 0;
for(int i = 0; i < sonarSamples; i ++){
pingData = (range.ping() / US_ROUNDTRIP_CM);
//Serial.print(“Ping Data: “);
//Serial.println(pingData);
rangeOffsetMean += pingData;
}
rangeOffsetMean /= sonarSamples;
//Serial.print(“Average reading of: “);
//Serial.println(rangeOffsetMean);

}
//*****************************MATH FUNCTIONS**********************************//
void rangeToR(){
rangeToRFactor = (travelLength / rangeMax);
nextR = currentDistance * rangeToRFactor;
}
//*****************************MAIN CODE***************************************//

void setup(){
//Pin Setup
pinMode(rMotorDirectionPin, OUTPUT);
digitalWrite(rMotorDirectionPin, LOW);
pinMode(rMotorStepPin, OUTPUT);
digitalWrite(rMotorStepPin, LOW);
pinMode(thetaMotorDirectionPin1, OUTPUT);
digitalWrite(thetaMotorDirectionPin1, LOW);
pinMode(thetaMotorDirectionPin2, OUTPUT);
digitalWrite(thetaMotorDirectionPin2, LOW);
pinMode(innerHomeSensorLightPin, OUTPUT);
digitalWrite(innerHomeSensorLightPin, LOW);
pinMode(outerHomeSensorLightPin, OUTPUT);
digitalWrite(outerHomeSensorLightPin, LOW);
pinMode(rangeEcho, INPUT);
pinMode(rangeTrig, OUTPUT);
digitalWrite(rangeTrig, LOW);

//Start Serial
Serial.begin(9600);
Wire.begin();

//Run Calibration Sequence
//Serial.println(“Calibrating R”);
calibrateR();
//Serial.println(“Now Calibrating Theta”);
calibrateRange();
//Serial.println(“Finished Calibrating!”);
//Serial.println(“”);
//Serial.println(“”);
//Serial.println(“”);
//Serial.println(“”);
//Serial.println(“Begin Behavior”);
}

void loop(){
//Serial.print(“Current Position: R = “);
//Serial.print(currentR);
//Serial.print(“, Theta = “);
//Serial.println(currentTheta);
//Serial.print(“Detecting range…”);
detectRange();
rangeToR();
//Serial.print(“Moving R to “);
//Serial.println(nextR);
if(currentR – nextR < 0){
while(currentR < nextR){
driveR(“forward”);
currentR = currentR +(driveGearCircum/stepperStepsPerRev);
//Serial.print(“Current Position: R = “);
//Serial.print(currentR);
}
}
else if(currentR – nextR > 0){
while(currentR > nextR){
driveR(“backward”);
currentR = currentR – (driveGearCircum/stepperStepsPerRev);
//Serial.print(“Current Position: R = “);
//Serial.print(currentR);
}
}
//Serial.println(“Moving to next Theta”);
currentCircum = (currentR + robotLen) * 2 * 3.14;
for(int steps = 0; steps <= ((currentCircum/360)/driveWheelCircum)*200; steps ++){
driveTheta(“forward”);
}
currentTheta += 1;
delay(100);
}

Project Progression

Iteration 1 – gantry arm, threaded rod, rectangular base, 4 wheels

Iteration 2 – gantry arm, timing belt, semicircular base, 3 wheels