Roomba
From Floor Pi Wiki
|
Obsolete |
|
First things first. The Roomba needs a name!
Put your suggestions on the Talk: page (everyone should learn to use this feature!).
Resources
Code
Wallfollow
Moves between the lounges along the west wall of hall while playing the Imperial March. Must be started facing north.
Should be compiled with the makefile and oi.h provided with the "drive" sample program.
/* drive.c
* Designed to run on Create Command Module
*
* The basic architecture of this program can be re-used to easily
* write a wide variety of Create control programs. All sensor values
* are polled in the background (using the serial rx interrupt) and
* stored in the sensors array as long as the function
* delayAndUpdateSensors() is called periodically. Users can send commands
* directly a byte at a time using byteTx() or they can use the
* provided functions, such as baud() and drive().
*/
// Includes
#include <avr/interrupt.h>
#include <avr/io.h>
#include <stdlib.h>
#include "oi.h"
// Constants
#define RESET_SONG 0
#define MARCH 1 //song length > 16 notes, so this song overflows into song 2
#define BUMP_SONG 3
#define END_SONG 4
// Global variables
volatile uint16_t timer_cnt = 0;
volatile uint8_t timer_on = 0;
volatile uint8_t sensors_flag = 0;
volatile uint8_t sensors_index = 0;
volatile uint8_t sensors_in[Sen6Size];
volatile uint8_t sensors[Sen6Size];
volatile int16_t distance = 0;
volatile int16_t angle = 0;
// Functions
void byteTx(uint8_t value);
void delayMs(uint16_t time_ms);
void delayAndUpdateSensors(unsigned int time_ms);
void initialize(void);
void powerOnRobot(void);
void baud(uint8_t baud_code);
void drive(int16_t velocity, int16_t radius);
uint16_t randomAngle(void);
void defineSongs(void);
int main (void)
{
uint8_t FOLLOWING = 0;
uint8_t BACKING = 1;
uint8_t CORRECTING = 2;
uint8_t TURNING = 3;
uint8_t LEFT = 0;
uint8_t RIGHT = 1;
uint8_t state = FOLLOWING;
uint8_t wall = LEFT;
uint8_t driving = 0;
// Set up Create and module
initialize();
LEDBothOff;
powerOnRobot();
byteTx(CmdStart);
baud(Baud28800);
defineSongs();
byteTx(CmdControl);
byteTx(CmdFull);
// Stop just as a precaution
drive(0, RadStraight);
// Play the reset song and wait while it plays
byteTx(CmdPlay);
byteTx(RESET_SONG);
delayAndUpdateSensors(750);
for(;;)
{
delayAndUpdateSensors(10);
if(UserButtonPressed)
{
// Play start song and wait
byteTx(CmdPlay);
byteTx(MARCH);
delayAndUpdateSensors(1000);
// Drive around until a button or unsafe condition is detected
while(!(UserButtonPressed))
{
/*
if (distance > 4000)
{
distance = 0;
byteTx(CmdPlay);
byteTx(MARCH);
}
*/
if (state == FOLLOWING)
{
if (wall == LEFT)
{
if (!driving)
{
drive(500, 1000);
driving = 1;
}
if (sensors[SenBumpDrop] & BumpLeft)
{
state = BACKING;
distance = 0;
driving = 0;
}
if (sensors[SenBumpDrop] & BumpRight)
{
byteTx(CmdPlay);
byteTx(BUMP_SONG);
state = TURNING;
angle = 0;
driving = 0;
}
}
else
{
if (!driving)
{
drive(500, -1000);
driving = 1;
}
if (sensors[SenBumpDrop] & BumpRight)
{
state = BACKING;
distance = 0;
driving = 0;
}
if (sensors[SenBumpDrop] & BumpLeft)
{
byteTx(CmdPlay);
byteTx(BUMP_SONG);
state = TURNING;
angle = 0;
driving = 0;
}
}
}
else if (state == BACKING)
{
if (!driving)
{
drive(-200, RadStraight);
driving = 1;
}
if (distance < -30)
{
state = CORRECTING;
angle = 0;
driving = 0;
}
}
else if (state == CORRECTING)
{
if (wall == LEFT)
{
if (!driving)
{
drive(200, RadCW);
driving = 1;
}
if (angle < -15)
{
state = FOLLOWING;
driving = 0;
}
}
else
{
if (!driving)
{
drive(200, RadCCW);
driving = 1;
}
if (angle > 15)
{
state = FOLLOWING;
driving = 0;
}
}
}
else if (state == TURNING)
{
if (wall == LEFT)
{
if (!driving)
{
drive(200, RadCCW);
driving = 1;
}
if (angle > 180)
{
// byteTx(CmdPlay);
// byteTx(MARCH);
state = FOLLOWING;
wall = RIGHT;
driving = 0;
}
}
else
{
if (!driving)
{
drive(200, RadCCW);
driving = 1;
}
if (angle > 180)
{
// byteTx(CmdPlay);
// byteTx(MARCH);
state = FOLLOWING;
wall = LEFT;
driving = 0;
}
}
}
// wait a little more than one robot tick for sensors to update
delayAndUpdateSensors(20);
}
// Stop driving
drive(0, RadStraight);
// Play end song and wait
delayAndUpdateSensors(500);
byteTx(CmdPlay);
byteTx(END_SONG);
delayAndUpdateSensors(2438);
}
}
}
// Serial receive interrupt to store sensor values
SIGNAL(SIG_USART_RECV)
{
uint8_t temp;
temp = UDR0;
if(sensors_flag)
{
sensors_in[sensors_index++] = temp;
if(sensors_index >= Sen6Size)
sensors_flag = 0;
}
}
// Timer 1 interrupt to time delays in ms
SIGNAL(SIG_OUTPUT_COMPARE1A)
{
if(timer_cnt)
timer_cnt--;
else
timer_on = 0;
}
// Transmit a byte over the serial port
void byteTx(uint8_t value)
{
while(!(UCSR0A & _BV(UDRE0))) ;
UDR0 = value;
}
// Delay for the specified time in ms without updating sensor values
void delayMs(uint16_t time_ms)
{
timer_on = 1;
timer_cnt = time_ms;
while(timer_on) ;
}
// Delay for the specified time in ms and update sensor values
void delayAndUpdateSensors(uint16_t time_ms)
{
uint8_t temp;
timer_on = 1;
timer_cnt = time_ms;
while(timer_on)
{
if(!sensors_flag)
{
for(temp = 0; temp < Sen6Size; temp++)
sensors[temp] = sensors_in[temp];
// Update running totals of distance and angle
distance += (int)((sensors[SenDist1] << 8) | sensors[SenDist0]);
angle += (int)((sensors[SenAng1] << 8) | sensors[SenAng0]);
byteTx(CmdSensors);
byteTx(6);
sensors_index = 0;
sensors_flag = 1;
}
}
}
// Initialize the Mind Control's ATmega168 microcontroller
void initialize(void)
{
cli();
// Set I/O pins
DDRB = 0x10;
PORTB = 0xCF;
DDRC = 0x00;
PORTC = 0xFF;
DDRD = 0xE6;
PORTD = 0x7D;
// Set up timer 1 to generate an interrupt every 1 ms
TCCR1A = 0x00;
TCCR1B = (_BV(WGM12) | _BV(CS12));
OCR1A = 71;
TIMSK1 = _BV(OCIE1A);
// Set up the serial port with rx interrupt
UBRR0 = 19;
UCSR0B = (_BV(RXCIE0) | _BV(TXEN0) | _BV(RXEN0));
UCSR0C = (_BV(UCSZ00) | _BV(UCSZ01));
// Turn on interrupts
sei();
}
void powerOnRobot(void)
{
// If Create's power is off, turn it on
if(!RobotIsOn)
{
while(!RobotIsOn)
{
RobotPwrToggleLow;
delayMs(500); // Delay in this state
RobotPwrToggleHigh; // Low to high transition to toggle power
delayMs(100); // Delay in this state
RobotPwrToggleLow;
}
delayMs(3500); // Delay for startup
}
}
// Switch the baud rate on both Create and module
void baud(uint8_t baud_code)
{
if(baud_code <= 11)
{
byteTx(CmdBaud);
UCSR0A |= _BV(TXC0);
byteTx(baud_code);
// Wait until transmit is complete
while(!(UCSR0A & _BV(TXC0))) ;
cli();
// Switch the baud rate register
if(baud_code == Baud115200)
UBRR0 = Ubrr115200;
else if(baud_code == Baud57600)
UBRR0 = Ubrr57600;
else if(baud_code == Baud38400)
UBRR0 = Ubrr38400;
else if(baud_code == Baud28800)
UBRR0 = Ubrr28800;
else if(baud_code == Baud19200)
UBRR0 = Ubrr19200;
else if(baud_code == Baud14400)
UBRR0 = Ubrr14400;
else if(baud_code == Baud9600)
UBRR0 = Ubrr9600;
else if(baud_code == Baud4800)
UBRR0 = Ubrr4800;
else if(baud_code == Baud2400)
UBRR0 = Ubrr2400;
else if(baud_code == Baud1200)
UBRR0 = Ubrr1200;
else if(baud_code == Baud600)
UBRR0 = Ubrr600;
else if(baud_code == Baud300)
UBRR0 = Ubrr300;
sei();
delayMs(100);
}
}
// Send Create drive commands in terms of velocity and radius
void drive(int16_t velocity, int16_t radius)
{
byteTx(CmdDrive);
byteTx((uint8_t)((velocity >> 8) & 0x00FF));
byteTx((uint8_t)(velocity & 0x00FF));
byteTx((uint8_t)((radius >> 8) & 0x00FF));
byteTx((uint8_t)(radius & 0x00FF));
}
// Define songs to be played later
void defineSongs(void)
{
// Reset song
byteTx(CmdSong);
byteTx(RESET_SONG);
byteTx(4);
byteTx(60);
byteTx(6);
byteTx(72);
byteTx(6);
byteTx(84);
byteTx(6);
byteTx(96);
byteTx(6);
// Start song
byteTx(CmdSong);
byteTx(MARCH);
byteTx(18); // 18 notes
byteTx(55); // G
byteTx(32);
byteTx(55); // G
byteTx(32);
byteTx(55); // G
byteTx(32);
byteTx(51); // D#
byteTx(24);
byteTx(58); // A#
byteTx(12);
byteTx(55); // G
byteTx(32);
byteTx(51); // D#
byteTx(24);
byteTx(58); // A#
byteTx(12);
byteTx(55); // G
byteTx(64);
byteTx(62); // D
byteTx(32);
byteTx(62); // D
byteTx(32);
byteTx(62); // D
byteTx(32);
byteTx(63); // D#
byteTx(24);
byteTx(58); // A#
byteTx(12);
byteTx(54); // F#
byteTx(32);
byteTx(51); // D#
byteTx(24);
byteTx(58); // A#
byteTx(12);
byteTx(55); // G
byteTx(64);
// Bump song
byteTx(CmdSong);
byteTx(BUMP_SONG);
byteTx(2);
byteTx(58); // A#
byteTx(32);
byteTx(55); // G
byteTx(12);
// End song
byteTx(CmdSong);
byteTx(END_SONG);
byteTx(5);
byteTx(77);
byteTx(18);
byteTx(74);
byteTx(12);
byteTx(72);
byteTx(12);
byteTx(69);
byteTx(12);
byteTx(65);
byteTx(24);
}
