Difference between revisions of "Roomba"
From Floor Pi Wiki
Line 21: | Line 21: | ||
* Designed to run on Create Command Module | * Designed to run on Create Command Module | ||
* | * | ||
− | * The basic architecture of this program can be re-used to easily | + | * The basic architecture of this program can be re-used to easily |
* write a wide variety of Create control programs. All sensor values | * write a wide variety of Create control programs. All sensor values | ||
− | * are polled in the background (using the serial rx interrupt) and | + | * are polled in the background (using the serial rx interrupt) and |
− | * stored in the sensors array as long as the function | + | * stored in the sensors array as long as the function |
* delayAndUpdateSensors() is called periodically. Users can send commands | * delayAndUpdateSensors() is called periodically. Users can send commands | ||
− | * directly a byte at a time using byteTx() or they can use the | + | * directly a byte at a time using byteTx() or they can use the |
* provided functions, such as baud() and drive(). | * provided functions, such as baud() and drive(). | ||
*/ | */ | ||
− | |||
− | |||
// Includes | // Includes | ||
Line 37: | Line 35: | ||
#include <stdlib.h> | #include <stdlib.h> | ||
#include "oi.h" | #include "oi.h" | ||
− | |||
// Constants | // Constants | ||
#define RESET_SONG 0 | #define RESET_SONG 0 | ||
− | #define | + | #define MARCH 1 |
#define BUMP_SONG 3 | #define BUMP_SONG 3 | ||
#define END_SONG 4 | #define END_SONG 4 | ||
− | |||
// Global variables | // Global variables | ||
Line 55: | Line 51: | ||
volatile int16_t distance = 0; | volatile int16_t distance = 0; | ||
volatile int16_t angle = 0; | volatile int16_t angle = 0; | ||
− | |||
− | |||
− | |||
// Functions | // Functions | ||
Line 70: | Line 63: | ||
void defineSongs(void); | void defineSongs(void); | ||
− | int main (void) | + | int main (void) |
{ | { | ||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
uint8_t follow_left = 0; | uint8_t follow_left = 0; | ||
uint8_t follow_right = 1; | uint8_t follow_right = 1; | ||
Line 86: | Line 71: | ||
uint8_t left_correct = 4; | uint8_t left_correct = 4; | ||
uint8_t right_correct = 5; | uint8_t right_correct = 5; | ||
− | |||
+ | 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 = 0; | ||
+ | uint8_t wall = 0; //initialize in left-following (moving north) | ||
// Set up Create and module | // Set up Create and module | ||
Line 106: | Line 100: | ||
byteTx(RESET_SONG); | byteTx(RESET_SONG); | ||
delayAndUpdateSensors(750); | delayAndUpdateSensors(750); | ||
− | |||
for(;;) | for(;;) | ||
Line 116: | Line 109: | ||
// Play start song and wait | // Play start song and wait | ||
byteTx(CmdPlay); | byteTx(CmdPlay); | ||
− | byteTx( | + | byteTx(MARCH); |
delayAndUpdateSensors(1000); | delayAndUpdateSensors(1000); | ||
− | + | state = follow_left; | |
− | + | ||
// Drive around until a button or unsafe condition is detected | // Drive around until a button or unsafe condition is detected | ||
while(!(UserButtonPressed)) | while(!(UserButtonPressed)) | ||
{ | { | ||
+ | /* | ||
if (distance > 4000) | if (distance > 4000) | ||
{ | { | ||
distance = 0; | distance = 0; | ||
byteTx(CmdPlay); | byteTx(CmdPlay); | ||
− | byteTx( | + | byteTx(MARCH); |
} | } | ||
+ | */ | ||
− | if (state == | + | if (state == FOLLOWING) |
{ | { | ||
− | + | if (wall == LEFT) | |
− | if ( | + | |
− | + | ||
− | + | ||
{ | { | ||
− | byteTx(CmdPlay); | + | drive(500, 1000); |
− | byteTx(BUMP_SONG); | + | if (sensors[SenBumpDrop] & BumpLeft) |
− | state = | + | { |
+ | state = BACKING; | ||
+ | distance = 0; | ||
+ | } | ||
+ | if (sensors[SenBumpDrop] & BumpRight) | ||
+ | { | ||
+ | byteTx(CmdPlay); | ||
+ | byteTx(BUMP_SONG); | ||
+ | state = TURNING; | ||
+ | angle = 0; | ||
+ | } | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | drive(500, -1000); | ||
+ | if (sensors[SenBumpDrop] & BumpRight) | ||
+ | { | ||
+ | state = BACKING; | ||
+ | distance = 0; | ||
+ | } | ||
+ | if (sensors[SenBumpDrop] & BumpLeft) | ||
+ | { | ||
+ | byteTx(CmdPlay); | ||
+ | byteTx(BUMP_SONG); | ||
+ | state = TURNING; | ||
+ | angle = 0; | ||
+ | } | ||
+ | } | ||
+ | } | ||
+ | else if (state == BACKING) | ||
+ | { | ||
+ | drive(-200, RadStraight); | ||
+ | if (distance < -30) | ||
+ | { | ||
+ | state = CORRECTING; | ||
+ | angle = 0; | ||
+ | } | ||
+ | } | ||
+ | else if (state == CORRECTING) | ||
+ | { | ||
+ | if (wall == LEFT) | ||
+ | { | ||
+ | if (angle < -15) | ||
+ | state = FOLLOWING; | ||
+ | drive(200, RadCW); | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | if (angle > 15) | ||
+ | state = FOLLOWING; | ||
+ | drive(200, RadCCW); | ||
+ | } | ||
+ | } | ||
+ | else if (state == TURNING) | ||
+ | { | ||
+ | if (wall == LEFT) | ||
+ | { | ||
+ | if (angle > 180) | ||
+ | { | ||
+ | // byteTx(CmdPlay); | ||
+ | // byteTx(MARCH); | ||
+ | state = FOLLOWING; | ||
+ | wall = RIGHT; | ||
+ | } | ||
+ | drive(200, RadCCW); | ||
+ | } | ||
+ | else | ||
+ | { | ||
+ | if (angle > 180) | ||
+ | { | ||
+ | byteTx(CmdPlay); | ||
+ | byteTx(MARCH); | ||
+ | state = FOLLOWING; | ||
+ | wall = LEFT; | ||
+ | } | ||
+ | drive(200, RadCCW); | ||
} | } | ||
− | |||
} | } | ||
+ | |||
else if (state == follow_right) | else if (state == follow_right) | ||
{ | { | ||
Line 157: | Line 223: | ||
} | } | ||
angle = 0; | angle = 0; | ||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
} | } | ||
else if (state == right_correct) | else if (state == right_correct) | ||
{ | { | ||
if (angle > 7) | if (angle > 7) | ||
− | + | state = follow_right; | |
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
drive(200, RadCCW); | drive(200, RadCCW); | ||
} | } | ||
Line 179: | Line 233: | ||
{ | { | ||
if (angle > 180) | if (angle > 180) | ||
− | + | { | |
− | drive(200, RadCCW); | + | byteTx(CmdPlay); |
+ | byteTx(MARCH); | ||
+ | state = follow_left; | ||
+ | } | ||
+ | drive(200, RadCCW); | ||
} | } | ||
− | + | ||
// wait a little more than one robot tick for sensors to update | // wait a little more than one robot tick for sensors to update | ||
delayAndUpdateSensors(20); | delayAndUpdateSensors(20); | ||
Line 195: | Line 253: | ||
byteTx(END_SONG); | byteTx(END_SONG); | ||
delayAndUpdateSensors(2438); | delayAndUpdateSensors(2438); | ||
− | |||
} | } | ||
} | } | ||
} | } | ||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
// Serial receive interrupt to store sensor values | // Serial receive interrupt to store sensor values | ||
Line 211: | Line 261: | ||
{ | { | ||
uint8_t temp; | uint8_t temp; | ||
− | |||
temp = UDR0; | temp = UDR0; | ||
Line 222: | Line 271: | ||
} | } | ||
} | } | ||
− | |||
− | |||
− | |||
// Timer 1 interrupt to time delays in ms | // Timer 1 interrupt to time delays in ms | ||
Line 234: | Line 280: | ||
timer_on = 0; | timer_on = 0; | ||
} | } | ||
− | |||
− | |||
− | |||
// Transmit a byte over the serial port | // Transmit a byte over the serial port | ||
Line 244: | Line 287: | ||
UDR0 = value; | UDR0 = value; | ||
} | } | ||
− | |||
− | |||
− | |||
// Delay for the specified time in ms without updating sensor values | // Delay for the specified time in ms without updating sensor values | ||
Line 255: | Line 295: | ||
while(timer_on) ; | while(timer_on) ; | ||
} | } | ||
− | |||
− | |||
− | |||
// Delay for the specified time in ms and update sensor values | // Delay for the specified time in ms and update sensor values | ||
Line 284: | Line 321: | ||
} | } | ||
} | } | ||
− | |||
− | |||
− | |||
// Initialize the Mind Control's ATmega168 microcontroller | // Initialize the Mind Control's ATmega168 microcontroller | ||
Line 315: | Line 349: | ||
sei(); | sei(); | ||
} | } | ||
− | |||
− | |||
− | |||
void powerOnRobot(void) | void powerOnRobot(void) | ||
Line 335: | Line 366: | ||
} | } | ||
} | } | ||
− | |||
− | |||
− | |||
// Switch the baud rate on both Create and module | // Switch the baud rate on both Create and module | ||
Line 383: | Line 411: | ||
} | } | ||
} | } | ||
− | |||
− | |||
− | |||
// Send Create drive commands in terms of velocity and radius | // Send Create drive commands in terms of velocity and radius | ||
Line 396: | Line 421: | ||
byteTx((uint8_t)(radius & 0x00FF)); | byteTx((uint8_t)(radius & 0x00FF)); | ||
} | } | ||
− | |||
− | |||
− | |||
// Define songs to be played later | // Define songs to be played later | ||
Line 418: | Line 440: | ||
// Start song | // Start song | ||
byteTx(CmdSong); | byteTx(CmdSong); | ||
− | byteTx( | + | byteTx(MARCH); |
− | byteTx(18); // 18 notes | + | byteTx(18); // 18 notes |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(51); // D# | + | byteTx(51); // D# |
− | byteTx(24); | + | byteTx(24); |
− | byteTx(58); // A# | + | byteTx(58); // A# |
− | byteTx(12); | + | byteTx(12); |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(51); // D# | + | byteTx(51); // D# |
− | byteTx(24); | + | byteTx(24); |
− | byteTx(58); // A# | + | byteTx(58); // A# |
− | byteTx(12); | + | byteTx(12); |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(64); | + | byteTx(64); |
− | byteTx(62); // D | + | byteTx(62); // D |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(62); // D | + | byteTx(62); // D |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(62); // D | + | byteTx(62); // D |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(63); // D# | + | byteTx(63); // D# |
− | byteTx(24); | + | byteTx(24); |
− | byteTx(58); // A# | + | byteTx(58); // A# |
− | byteTx(12); | + | byteTx(12); |
− | byteTx(54); // F# | + | byteTx(54); // F# |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(51); // D# | + | byteTx(51); // D# |
− | byteTx(24); | + | byteTx(24); |
− | byteTx(58); // A# | + | byteTx(58); // A# |
− | byteTx(12); | + | byteTx(12); |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(64); | + | byteTx(64); |
// Bump song | // Bump song | ||
Line 461: | Line 483: | ||
byteTx(BUMP_SONG); | byteTx(BUMP_SONG); | ||
byteTx(2); | byteTx(2); | ||
− | byteTx(58); // A# | + | byteTx(58); // A# |
− | byteTx(32); | + | byteTx(32); |
− | byteTx(55); // G | + | byteTx(55); // G |
− | byteTx(12); | + | byteTx(12); |
− | + | ||
// End song | // End song |
Revision as of 17:03, 10 February 2008
This article or section could benefit from MOAR data. You can help by sharing your wisdom on the subject, and by applying peer pressure to other contributors who might have something useful to say. |
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 #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 follow_left = 0; uint8_t follow_right = 1; uint8_t turning_left = 2; uint8_t turning_right = 3; uint8_t left_correct = 4; uint8_t right_correct = 5; 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 = 0; uint8_t wall = 0; //initialize in left-following (moving north) // 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); state = follow_left; // 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) { drive(500, 1000); if (sensors[SenBumpDrop] & BumpLeft) { state = BACKING; distance = 0; } if (sensors[SenBumpDrop] & BumpRight) { byteTx(CmdPlay); byteTx(BUMP_SONG); state = TURNING; angle = 0; } } else { drive(500, -1000); if (sensors[SenBumpDrop] & BumpRight) { state = BACKING; distance = 0; } if (sensors[SenBumpDrop] & BumpLeft) { byteTx(CmdPlay); byteTx(BUMP_SONG); state = TURNING; angle = 0; } } } else if (state == BACKING) { drive(-200, RadStraight); if (distance < -30) { state = CORRECTING; angle = 0; } } else if (state == CORRECTING) { if (wall == LEFT) { if (angle < -15) state = FOLLOWING; drive(200, RadCW); } else { if (angle > 15) state = FOLLOWING; drive(200, RadCCW); } } else if (state == TURNING) { if (wall == LEFT) { if (angle > 180) { // byteTx(CmdPlay); // byteTx(MARCH); state = FOLLOWING; wall = RIGHT; } drive(200, RadCCW); } else { if (angle > 180) { byteTx(CmdPlay); byteTx(MARCH); state = FOLLOWING; wall = LEFT; } drive(200, RadCCW); } } else if (state == follow_right) { drive(500, -1000); if (sensors[SenBumpDrop] & BumpRight) state = right_correct; if (sensors[SenBumpDrop] & BumpLeft) { byteTx(CmdPlay); byteTx(BUMP_SONG); state = turning_right; } angle = 0; } else if (state == right_correct) { if (angle > 7) state = follow_right; drive(200, RadCCW); } else if (state == turning_right) { if (angle > 180) { byteTx(CmdPlay); byteTx(MARCH); state = follow_left; } drive(200, RadCCW); } // 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); }