|
Marco Robo
Team members: Ruben Markowitz, Bryan Gardiner, Sara Johnson
Introduction
We are creating an automous robot that explores its surroundings, and maps its journey in attempt to understand its place in the world. Marco Robo travels until it encounters an obstacle, and turns to journey in new, unknown directions. It draws its progression on a map on its back during travel to retain what it has learned.
Technical Notes
Our robot travels on four wheels: two are driven by geared down motors and two are omni-wheels that allow movement in any direction. The drawing mechanism sweeps the pen radially with a stepper motor, and forward and back on a winch, also driven by a stepper motor. The motors are driven by two stepper motors, communicating with an arduino uno. The robot maintains its reference location with a compass, and the map-drawing is scaled to the size of the room.
Circuit Diagram
 Marco Robo internals.  Platform holding paper.  Marker housing driven by winch.
Code
#include <Wire.h>
#include <Adafruit_Sensor.h>
#include <Adafruit_LSM303.h>
int step1 = 12;
int direct1 = 11;
int step2 = 10;
int direct2 = 9;
int leftMotorPlus=8;
int leftMotorMinus=7;
int rightMotorPlus=6;
int rightMotorMinus=5;
int posR=0;
int posTheta=0;
int movTheta=0;
int movR=0;
int zR=0;
int zTheta=0;
int zX=0;
int zY=0;
int temp1=0;
int temp2=0;
int drawing=0;
int turning=0;
int minDistance=2;
float heading2=0;
/* Assign a unique ID to this sensor at the same time */
Adafruit_LSM303_Mag mag = Adafruit_LSM303_Mag(12345);
void setup() {
pinMode(step1,OUTPUT);
pinMode(direct1,OUTPUT);
pinMode(step2,OUTPUT);
pinMode(direct2,OUTPUT);
pinMode(leftMotorPlus,OUTPUT);
pinMode(leftMotorMinus,OUTPUT);
pinMode(rightMotorPlus,OUTPUT);
pinMode(rightMotorMinus,OUTPUT);
pinMode(A2,OUTPUT);
pinMode(A3,OUTPUT);
sensors_event_t event;
mag.getEvent(&event);
float Pi = 3.14159;
heading2 = (atan2(event.magnetic.y,event.magnetic.x) * 180) / Pi;
if (heading2 < 0)
{
heading2 = 360 + heading;
heading2=heading2/360;
heading2=heading2*(2*pi);
}
}
void loop()
{
sensors_event_t event;
mag.getEvent(&event);
float Pi = 3.14159;
float movTheta = (atan2(event.magnetic.y,event.magnetic.x) * 180) / Pi;
if (movTheta < 0)
{
movTheta = 360 + movTheta;
movTheta=movTheta/360;
movTheta=movTheta*(2*pi);
movTheta=movTheta-heading2;
}
turning=0;
if(analogRead(A2)>minDistance or analogRead(A3)>minDistance)
{
turning=1;
}
if(drawing==1)
{
temp1=posR*cos(posTheta);
temp2=movR*cos(movTheta);
zX=temp1+temp2;
temp1=posR*sin(posTheta);
temp2=movR*sin(movTheta);
zY=temp1+temp2;
zR=sqrt(zX*zX+zY*zY);
zTheta=atan(zY/zX);
for(int i=0;i<abs(zR-posR);i++)
{
if((zR-posR)>=0)
{
digitalWrite(direct1,HIGH);
}
else
{
digitalWrite(direct1,LOW);
}
digitalWrite(step1,HIGH);
delay(1);
}
digitalWrite(step1,LOW);
for(int i=0;i<abs(zTheta-posTheta);i++)
{
if((zTheta-posTheta)>=0)
{
digitalWrite(direct2,HIGH);
}
else
{
digitalWrite(direct2,LOW);
}
digitalWrite(step2,HIGH);
delay(1);
}
digitalWrite(step2,LOW);
posR=zR;
posTheta=zTheta;
drawing=0;
movR=0;
}
if(turning==1 && drawing==0)
{
digitalWrite(leftMotorPlus,LOW);
digitalWrite(leftMotorMinus,HIGH);
digitalWrite(rightMotorPlus,LOW);
digitalWrite(rightMotorMinus,LOW);
digitalWrite(step1,LOW);
digitalWrite(step2,LOW);
delay(500);
movTheta+=1;
}
if(turning==0 && drawing==0)
{
digitalWrite(leftMotorPlus,HIGH);
digitalWrite(leftMotorMinus,LOW);
digitalWrite(rightMotorPlus,HIGH);
digitalWrite(rightMotorMinus,LOW);
digitalWrite(step1,LOW);
digitalWrite(step2,LOW);
delay(1000);
movR+=5;
drawing=1;
}
}
|
