Mindstorm Robots

A gift from Lora. Add some open source 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;
}
                

Comments