A simple christmas gift from Lora. Add some opensource software and some determination and you get hours and hours of geek fun.
I just couldn't have any fun with the RCX if I used the original operating system. Lucky for me the brickOS team make a wonder alternative OS that enables you to write your programs in C or C++. Make sure you follow the installation instructions for a good clean setup.
Spike the robot
The Code
#include <conio.h>
#include <unistd.h>
#include <dmotor.h>
#include <dsensor.h>
#define ROTATE_TIME 2900.0 // time to complete one full rotation in msec
#define MAX_PING_PONGS 4 // maximum concurrent left right touches allowed
#define OPERATING_SPEED MAX_SPEED // motor speed
#define low 1
#define medium 2
#define high 3
#define critical 4
enum state_t {clear, left_bump, right_bump, both_bump, ping_pong_trap};
enum direction_t {left_turn, right_turn, forward, backward, halt, idle};
struct motor_task{
int priority;
enum direction_t direction;
int speed;
long int duration;
};
struct motor_task task; // global task contantly updated by FSM
// read continuously by motor_controler
enum state_t state; // stores the current state of the bot
enum state_t previous_state; // stores the previous state of the bot
int start_FSM(); // Finite State Machine (the brains)
int start_left_sensor(); // Continuously monitor left while setting state
int start_right_sensor(); // Continuously monitor right while setting state
wakeup_t left_touched(wakeup_t data); // left sensor monitor
wakeup_t right_touched(wakeup_t data); // right sensor monitor
int degrees_to_msec(int degrees); // convert degrees of rotation to delay time
int look(); // display ambient light 0....100 approximately
int start_motor_control(); // handles ambulatory responsibilities
int update_task(int priority, int speed, long int duration, enum direction_t direction);
long degree_time = (long)(ROTATE_TIME/360.0); // time it takes to spin one degree of rotation */
/*
* begin routines
*/
int degrees_to_msec(int degrees){
return (int)((long)degrees * degree_time);
}
int look(){
while (!shutdown_requested()){
lcd_int((int)LIGHT_2);
msleep(1000);
}
return 0;
}
int update_task(int priority, int speed, long int duration, enum direction_t direction){
if (priority > task.priority){
task.priority = priority;
task.speed = speed;
task.duration = duration;
task.direction = direction;
}
return 0;
}
int start_motor_control(){
while (!shutdown_requested()){
while(task.duration > 0){
if (shutdown_requested()){
task.duration = 0;
}
else{
--(task.duration);
}
msleep(1);
switch(task.direction){
case forward:
motor_a_speed(task.speed);
motor_c_speed(task.speed);
motor_a_dir(fwd);
motor_c_dir(fwd);
break;
case backward:
motor_a_speed(task.speed);
motor_c_speed(task.speed);
motor_a_dir(rev);
motor_c_dir(rev);
break;
case left_turn:
motor_a_speed(task.speed);
motor_c_speed(task.speed);
motor_a_dir(rev);
motor_c_dir(fwd);
break;
case right_turn:
motor_a_speed(task.speed);
motor_c_speed(task.speed);
motor_a_dir(fwd);
motor_c_dir(rev);
break;
case idle:
motor_a_speed(off);
motor_c_speed(off);
break;
case halt:
motor_a_speed(brake);
motor_c_speed(brake);
break;
}
}
/* task expired shutdown the motors */
motor_a_speed(off);
motor_c_speed(off);
}
return 0;
}
int start_left_sensor(){
while (!shutdown_requested()){
if (wait_event(&left_touched,0) !=0){
switch(state){
case clear:
state = left_bump;
break;
case right_bump:
state = both_bump;
break;
case left_bump:
/* pass */
case both_bump:
/* pass */
case ping_pong_trap:
/* pass */
break;
}
}
}
return 0;
}
int start_right_sensor(){
while (!shutdown_requested()){
if (wait_event(&right_touched,0) !=0){
switch(state){
case clear:
state = right_bump;
break;
case left_bump:
state = both_bump;
break;
case right_bump:
/* pass */
case both_bump:
/* pass */
case ping_pong_trap:
/* pass */
break;
}
}
}
return 0;
}
wakeup_t left_touched(wakeup_t data){
return (TOUCH_1);
}
wakeup_t right_touched(wakeup_t data){
return (TOUCH_3);
}
int start_FSM(){
int ping_pongs;
ping_pongs = 0;
while (!shutdown_requested()){
if (state != clear){
msleep(250);
/* stop for a few so we can sample the sensors */
update_task(high,OPERATING_SPEED,200,idle);
}
else{
msleep(250);
}
if (ping_pongs > MAX_PING_PONGS){
state = ping_pong_trap; // check to see if in a state of ping_pong_trap
}
switch(state){
case clear:
/* forward */
update_task(low,OPERATING_SPEED,5000,forward);
break;
case left_bump:
/* backup */
update_task(high,OPERATING_SPEED,500,backward);
/* veer to the right */
update_task(high,OPERATING_SPEED,degrees_to_msec(15),right_turn);
if (previous_state == right_bump){ping_pongs++;}else{ping_pongs=0;}
previous_state = state;
state = clear;
break;
case right_bump:
/* backup */
update_task(high,OPERATING_SPEED,500,backward);
/* veer to the left */
update_task(high,OPERATING_SPEED,degrees_to_msec(15),left_turn);
if (previous_state == left_bump){ping_pongs++;}else{ping_pongs=0;}
previous_state = state;
state = clear;
break;
case both_bump:
/* backup */
update_task(high,OPERATING_SPEED,500,backward);
/* make a sharp right turn */
update_task(high,OPERATING_SPEED,degrees_to_msec(90),left_turn);
previous_state = state;
state = clear;
break;
case ping_pong_trap:
/* backup */
update_task(high,OPERATING_SPEED,500,backward);
/* spin around */
update_task(high,OPERATING_SPEED,degrees_to_msec(90),left_turn);
previous_state = state;
ping_pongs = 0;
state = clear;
break;
default:
state = clear;
}
}
return 0;
}
int main(int argc, char *argv[]){
ds_passive(&SENSOR_2);
/* even robots should be nice. */
cputs("hello");
/* the time it takes the bot to spin one degree of rotation */
degree_time = (long)(ROTATE_TIME/360.0);
/* give me a second to get my fingers out of the way */
sleep(1);
/* set default startup states */
state = clear;
previous_state = clear;
/* set the initial task to move forward */
task.priority = low;
task.speed = OPERATING_SPEED;
task.duration = 5000;
task.direction = forward;
execi(&look,0,NULL,1,DEFAULT_STACK_SIZE); // light sensor thread
execi(&start_left_sensor,0,NULL,1,DEFAULT_STACK_SIZE); // left sensor thread
execi(&start_right_sensor,0,NULL,1,DEFAULT_STACK_SIZE); // right sensor thread
execi(&start_motor_control,0,NULL,1,DEFAULT_STACK_SIZE); // right sensor thread
execi(&start_FSM,0,NULL,1,DEFAULT_STACK_SIZE); // FSM thread
/* loop from now till the end of the battery life or shutdown requested */
while(!shutdown_requested()){
msleep(10); // lets other threads have some cpu time
}
return 0;
}