For this lab you will program your robot to seek light while also avoiding obstacles. (Note, the light sensors generally work better when they are angled slightly away from the the front of the robot.) Implement your solution in the subsumption style. A simple example of how to do this in Interactive C is provided below.
Your robots will be tested in a series of progressively more difficult configurations. You must use the same Interactive C program for each situation. Finding the light means having your robot touch the base of the light. The light source will be significantly brighter than the ambient light.
Your robot should be able to pass the first three tests, but the last may be more difficult. At our next lab meeting, each team will demonstrate their robot's abilities on these test cases. The starting location and orientation of the robot will be arbitrary. Bring a print out of your code to hand in.
The code below is just a starting point. Feel free to modify the given behaviors and to add your own behaviors. For example, you may want to use the bend sensors to implement wall following. Or you may want to use infrared sensors instead of (or in addition to) the touch sensors to deal with obstacle avoidance. You may also wish to add some simple state so that you can detect when the robot is stuck.
Make sure you understand the given code. Then test it in its current form. Finally modify it to better solve the test cases.
/*
Here is one possible way to implement a subsumption style
architecture in Interactive C code. This method is described in
Mobile Robots by Jones and Flynn publised in 1993 by AK Peters.
Each behavior has a pair of global variables associated with it: a
command and a flag. When the flag is true, the command represents
the action chosen by the behavior. An arbitrator gives control to
the highest priority behavior whose flag is true.
In this simple example, there are three behaviors: cruise which
always wants to go forward, follow which turns towards light, and
escape which backs and turns away from obstacles. Cruise has the
lowest priority and escape has the highest priority. Each behavior
is implemented as a separate IC process.
Each process in IC is by default given 5 ticks. This is enough time
to complete multiple iterations of each behavior's loop. To
implement subsumption successfully, we must have each behavior give
up control immediately after a single loop iteration. To do this, add
a call to the function defer() at the end of each loop (as shown
below).
It is important to note that IC only seems to be able to handle a
maximum of 4 processes running simulataneously. This program uses 3
processes, but it would be relatively easy to eliminate the
cruise behavior so as to have more room for other more interesting
behaviors.
*/
#define Llight 2
#define Rlight 0
#define Lmotor 3
#define Rmotor 1
#define Lbump 9
#define Rbump 7
#define FORWARD 0
#define BACKWARD 1
#define TURNLEFT 2
#define TURNRIGHT 3
#define BACKthenLEFT 4
#define BACKthenRIGHT 5
int cruise_command;
int cruise_output_flag;
int follow_command;
int follow_output_flag;
int escape_command;
int escape_output_flag;
int motor_input;
/*
The order in which the behavior results are checked determines the
priority. The first checked have low priority and last checked have
high priority. This is called in motor_driver.
*/
void arbitrate() {
if (cruise_output_flag)
motor_input = cruise_command;
if (follow_output_flag)
motor_input = follow_command;
if (escape_output_flag)
motor_input = escape_command;
}
void motor_driver(){
while (1) {
arbitrate();
if (motor_input == FORWARD) {
printf("FORWARD\n");
motor(Lmotor, 100);
motor(Rmotor, 100);
}
else if (motor_input == BACKWARD) {
printf("BACKWARD\n");
motor(Lmotor, -100);
motor(Rmotor, -100);
}
else if (motor_input == TURNLEFT) {
printf("TURNLEFT\n");
motor(Lmotor, -100);
motor(Rmotor, 100);
}
else if (motor_input == TURNRIGHT) {
printf("TURNRIGHT\n");
motor(Lmotor, 100);
motor(Rmotor, -100);
}
else if (motor_input == BACKthenLEFT) {
printf("BACKthenLEFT\n");
motor(Lmotor, -100);
motor(Rmotor, -100);
sleep(.6);
motor(Lmotor, -100);
motor(Rmotor, 100);
sleep(.4);
}
else if (motor_input == BACKthenRIGHT) {
printf("BACKthenRIGHT\n");
motor(Lmotor, -100);
motor(Rmotor, -100);
sleep(.6);
motor(Lmotor, 100);
motor(Rmotor, -100);
sleep(.4);
}
else {
printf("MOTORS OFF\n");
motor(Lmotor, 0);
motor(Rmotor, 0);
}
}
}
void cruise() {
while(1) {
cruise_command = FORWARD;
cruise_output_flag = 1;
defer();
}
}
int abs(int value) {
if (value >= 0)
return(value);
else return(-1*value);
}
void follow() {
int Llightval, Rlightval;
int delta, threshold;
/* you should experiment with the proper threshold */
threshold = 5;
while(1) {
Llightval = analog(Llight);
Rlightval = analog(Rlight);
delta = Rlightval - Llightval;
if (abs(delta) > threshold) {
if (delta > 0)
follow_command = TURNLEFT;
else
follow_command = TURNRIGHT;
follow_output_flag = 1;
}
else
follow_output_flag = 0;
defer();
}
}
void escape() {
int Lbumpval, Rbumpval;
while (1) {
Lbumpval = digital(Lbump);
Rbumpval = digital(Rbump);
if (Lbumpval && Rbumpval) {
escape_command = BACKthenLEFT;
escape_output_flag = 1;
}
else if (Lbumpval) {
escape_command = BACKthenRIGHT;
escape_output_flag = 1;
}
else if (Rbumpval) {
escape_command = BACKthenLEFT;
escape_output_flag = 1;
}
else
escape_output_flag = 0;
defer();
}
}
/* Initiate all the processes. */
void main() {
start_process(cruise());
start_process(follow());
start_process(escape());
motor_driver();
}