Wallbots: Autonomous Magnetic Robots that Traverse Vertical Surfaces

Step 11 Upload the Code onto the Arduino

I'm pasting my code below. Each sensor is sampled, and the robot moves depending on which sensor detects a human hand, and whether or not it's red, green, or yellow (if it should move away or towards objects).

/*
Wallbots Code
Stacey Kuznetsov
May 6, 2009
for Making Things Interactive, Spring '09

This is the basic code to drive robotic movement of 2 sevo motors based on input
from 4 light sensors. Motion supports several settings, based on the robot mode.
Red robots move fast, towards objects (when light sensors detect darkness)
Green robots move at medium speed, away from objects (away from darker areas)
Yellow robots move slowerly, and stop to blink when objects are detected

The purpose of these robots is to move on walls using magnetic wheels.
Supported movement includes right, left and forward directions. Several
speeds are implemented based on the robot mode.

The light sensors auto-calibrate on reboot or when the top sensor is covered
for more than 3 seconds.

*/

#include <Servo.h>

// Right and left servos
Servo servo1;
Servo servo2;

// Light Sensors
int topSensor = 0; //700
int leftSensor = 1; /// Threshhold is 400
int frontSensor = 2; //400
int rightSensor = 3; //300

// Hardcoded thresholds (not used because we auto-calibrate)
int topThreshhold = 400;
int leftThreshhold = 550;
int frontThreshhold = 200;
int rightThreshhold = 650;

// Current robot type (red gree or yellow)
int STATE = 0;

// State values
int RED = 0;
int GREEN = 1;
int ORANGE = 2;

// Pins to drive the top tri-color LED
int redPin = 5;
int greenPin = 6;

// Values to hold sensor readings
int front;
int right;
int left;
int top;

// Auto-calibrate light sensor thresholds
void calibrate() {
Serial.println("CALIBRATING");
long int val = 0;
for (int i = 0; i<5; i++) {
val += analogRead(frontSensor);
delay(10);
}
frontThreshhold = (val /5) - 80;
val = 0;

for (int i = 0; i<5; i++) {
val = val + analogRead(topSensor);
Serial.println(analogRead(topSensor));
Serial.println(val);
delay(10);
}

topThreshhold = (val /5) -200;

val = 0;
for (int i = 0; i<5; i++) {
val += analogRead(rightSensor);
}
rightThreshhold = (val /5) - 100;
val = 0;
for (int i = 0; i<5; i++) {
val += analogRead(leftSensor);
}
leftThreshhold = (val /5) - 100;

// Print threshold values for debug
Serial.print("top: ");
Serial.println(topThreshhold);
Serial.print("right: ");
Serial.println(rightThreshhold);
Serial.print("left: ");
Serial.println(leftThreshhold);
Serial.print("front: ");
Serial.println(frontThreshhold);

}

void setup()
{
// turn on pin 13 for debug
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
// setup sensor pins
for (int i = 0; i<4; i++) {
pinMode(i, INPUT);
}
Serial.begin(9600);
calibrate();
// generate a random state
STATE = random(0, 3);
setColor(STATE);
}

// MOTOR FUNCTIONS

void turnLeft()
{
Serial.println("LEFT");

start();
delay(20);
for (int i = 0; i<20; i++) {
servo2.write(179);
servo1.write(1);
delay(20);
}
stop();
delay(20);
}

void turnRight() {
Serial.println("RIGHT");
start();
delay(20);
for (int i = 0; i<20; i++) {

servo2.write(1);
servo1.write(179);
delay(20);
}
stop();
delay(20);
}

void goForward(int del = 20) {
Serial.println("FORWARD");
start();
delay(20);
for (int i = 0; i<20; i++) {
servo1.write(179);
servo2.write(179);
delay(del);
}
stop();
delay(20);
}

void stop() {
servo1.detach();
servo2.detach();
delay(10);
}

void start() {
servo1.attach(10);
servo2.attach(9);

}

// Set the color of the top tri-color LED based on the current state
void setColor(int color) {
if (color == RED) {
digitalWrite(greenPin, 0);
analogWrite(redPin, 180);
}
else if (color == GREEN) {
digitalWrite(redPin, 0);
analogWrite(greenPin, 180);
}
else if (color == ORANGE) {
analogWrite(redPin, 100);
analogWrite(greenPin, 100);
}
}

// Blink the yellow color (when robot is confused)
void blinkOrange() {
for (int i = 0; i<5; i++) {
analogWrite(redPin, 100);
analogWrite(greenPin, 100);
delay(300);
digitalWrite(redPin, 0);
digitalWrite(greenPin, 0);
delay(300);
}

analogWrite(redPin, 100);
analogWrite(greenPin, 100);

}

void loop()
{

top = analogRead(topSensor);
long int time = millis();
while (analogRead(topSensor) < topThreshhold) {
delay(10); // while there is an arm wave from the user don't do anything
}
if ((millis() - time) > 3000) {
// if the sensor was covered for more than 3 seconds, re-calibrate
calibrate();
}

// if the top sensor was covered, we change state
if (top < topThreshhold) {
STATE = (STATE+1) %3;
setColor(STATE);
Serial.print("CHANGED STATE: ");
Serial.println(STATE);
}

// Read the other sensors
right = analogRead(rightSensor);
left = analogRead(leftSensor);
front = analogRead(frontSensor);

if (STATE == RED) {
// go towards objects
if (front < frontThreshhold) {
goForward();
} else if (right < rightThreshhold) {
turnRight();
} else if (left<leftThreshhold) {
turnLeft();
} else {
goForward();
}
}
if (STATE == GREEN) {
// go away from objects
if (front < frontThreshhold) {
int dir = random(0,2);
if (dir == 0 && right > rightThreshhold) {
turnRight();
} else if (dir == 1 && left > leftThreshhold) {
turnLeft();
}
} else if (right < rightThreshhold) {
if (left > leftThreshhold) {
turnLeft();
} else {
goForward();
}
} else if (left<leftThreshhold) {
if (right > rightThreshhold) {
turnRight();
} else {
goForward();
}
} else {
goForward();
}
delay(200);
}

if (STATE == ORANGE) {
// only move if there are no hand motions- otherwise blink
int dir = random(0, 3);
if (left<leftThreshhold || right<rightThreshhold ||
front<leftThreshhold) {
blinkOrange();
} else {
if (dir == 0) {
goForward();
} else if (dir == 1) {
turnRight();
} else if (dir == 2) {
turnLeft();
}
delay(1000);
}
delay(10);
}
}