RiseUpRobots

/*
coasterBotSubsumption.c
by Darrell Johnson simplebot AT google’s email service DOT com

com port in makefile is set to com3
Subsumption code based on the code found in:
Mobile Robots: Inspiration to Implementation
by Joseph Jones and Anita Flynn

It’s a fixed priority arbitration scheme to pick from the various behaviors that are
coded.

I’m new to programming so this is probably lousy code.
******************************************************************************

right bumper is IO_C0
left bumper is IO_C5
*/

// Includes

#include

// Constants

//drive constants
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define LEFT_TURN 3
#define RIGHT_TURN 4

// Global variables
volatile uint16_t timer_cnt = 0;
volatile uint8_t timer_on = 0;
volatile long distance = 0;
volatile long angle = 0;

//DJ my global variables
//DJ behavior flags: initial states
volatile uint8_t cruise_active = 1;
volatile uint8_t bump_active = 0;
volatile uint8_t panic_active = 0;

//DJ behavior commands
volatile uint8_t cruise_command = 0;
volatile uint8_t bump_command = 0;
volatile uint8_t panic_command = 0;
volatile uint8_t avoid_command = 0;

// globals for the bump behavior function

volatile long turn_angle = 0;
volatile uint8_t turn_dir = 1;
volatile uint8_t turning = 0;
volatile uint8_t backing_up = 0;

// Functions
void initialize(void);
void moveRobot (uint8_t operation);

//DJ behavior prototypes
void cruise(void);
void bump(void);

//DJ arbitration prototype
void motor_control (void);

//main program
int main (void)
{ //set up the controller
initialize();
// Stop just as a precaution

moveRobot(STOP);

// STARTUP DELAY
delay_ms(9000);

// THIS IS THE MAIN LOOP
while(1)
{
// cycle through the behaviors and the motor control
bump();
cruise();
motor_control(); //this is where the subsumption priority happens
}
}

// Initialize the ATmega168 microcontroller
void initialize(void)
{

set_digital_input(IO_C0, PULL_UP_ENABLED); // left bumper
set_digital_input(IO_C5, PULL_UP_ENABLED); //right bumper

}

// movement commands used by behaviors
void moveRobot (uint8_t operation)
{
if(operation == STOP)
set_motors(0,0);
if (operation == FORWARD)
set_motors(100, 113);
if (operation == BACKWARD)//{
set_motors(-50, -50);

if (operation == LEFT_TURN)//{
set_motors(75, -75);

if (operation == RIGHT_TURN)//{
set_motors(-75, 75);
set_motors(50, 350);
}

//DJ function working area
/////////////////////////////////////////////////////////////////////

//DJ cruise behavior Always active
void cruise()
{
cruise_command = FORWARD;
cruise_active = 1;
}

//bump behavior
void bump()
{
if(turning)
{
if(backing_up)
{
if(distance >turn_angle)
{
backing_up = 0;
}
bump_command = BACKWARD;
distance ++;

}
else
{
if(turn_dir)
{
if(angle > turn_angle)
{
turning = 0;
bump_active = 0;
}
else{
angle ++;

bump_command = RIGHT_TURN; //pivot CCW
}
}
else
{
if(angle > turn_angle)
{
turning = 0;
bump_active = 0;
}
else{
angle ++;
bump_command = LEFT_TURN; //pivot CW
}
}
}
}
else if ((is_digital_input_high(IO_C0))==0 || (is_digital_input_high(IO_C5))==0) //check for a bump

{
// Set the turn parameters and reset the angle
if ((is_digital_input_high(IO_C0))==0 && (is_digital_input_high(IO_C5))==0)
{
backing_up = 1;
turning = 1;
distance = 0;
angle = 0;

bump_active = 1;
}
else if ((is_digital_input_high(IO_C0)==0) && (is_digital_input_high(IO_C5)))
{
turn_dir = 1;
turning = 1;
bump_active = 1;
angle = 0;
turn_angle = 100000;
}
else if ((is_digital_input_high(IO_C5)==0) && (is_digital_input_high(IO_C0)))
{
turn_dir = 0;
turning = 1;
bump_active = 1;
angle = 0;
turn_angle = 100000;
}

}
else
turning = 0;

}

// DJ this is the main subsumption loop
void motor_control ()
{

if (bump_active)
moveRobot(bump_command);
else if (cruise_active)
moveRobot(cruise_command);
else
moveRobot(STOP);
}

/////////////////////////////END/////////////////////////////////////