/*******************************************************************************
*
- TITLE: navigate.c
- VERSION: 1.0
- DATE: 08-Jan-2004
- AUTHOR: Richard D. Petras
- COMMENTS:
- CHANGE LOG:
- DATE REV DESCRIPTION
-
- 04-Dec-2003 0.2 RDP Added Tracker to drive robot to goal
- 06-Dec-2003 0.3 RDP First implementation of drive algorithm
- 07-Dec-2003 0.4 RDP Got rid of servo glich by NOT disabling interrupts in
-
Timer 1 handler
- 08-Jan-2004 1.0 RDP The version used at the 2004 kickoff
*******************************************************************************/
#include “ifi_picdefs.h”
#include “ifi_default.h”
#include “ifi_aliases.h”
#include “ifi_utilities.h”
#include “printf_lib.h”
#include “user_routines.h”
#include “navigate.h”
// Global Variables
int Navigator_State = WAIT_TO_DRIVE;
/*******************************************************************************
*
- FUNCTION: Navigate()
- PURPOSE: Controls the left and right drive motors in order to
-
drive to an infrared beacon. This routine depends on two
-
beacon trackers to point the the IR beacon. It then uses
-
the current servo angles to determine when to turn
-
the robot to get to the goal. When the angles are
-
sufficiently small, we are at the target and stop.
-
The vehicle starts out stopped (WAIT_TO_DRIVE)
-
You might want to skip this state in autonomous mode :-)
-
When a button is pressed it starts to drive (DRIVE_TO_CENTER).
-
This drive is very short(just enough to move away from the
-
wall. Next we search until one of the trackers sees the
-
beacon (TURN_TO_TARGET). Then we drive toward the target
-
(DRIVE_TO_TARGET) until we pass it (PASSED_TARGET). Then
-
drive until the trackers are both beyond the goal point
-
(AT_TARGET). During all of this we are watching the current
-
to see if it exceeds the expected maximum value(OVER_CURRENT).
- PARAMETERS: none
- RETURNS: nothing
- COMMENTS: The values set in navigate.h are very dependent on the
-
particular robot configuration and will require some tuning
-
for your robot.
*******************************************************************************/
void Navigate()
{
// Note: Statics are used to retain the variable values across
// calls. Otherwise, the values stored inside a function are not
// guarenteed to remain the same.
static unsigned int Old_Clock = 0; // A place to store the last clock value
static unsigned char drive_timer = 0; // Timer for driving forward in 26 msec steps
static unsigned char left_drive_set = LSTOP; // Start out stopped
static unsigned char right_drive_set = RSTOP;
static unsigned char left_servo_set = 127; // Trackers pointed straight ahead
static unsigned char right_servo_set = 127;
int left_error, right_error; // Errors from the tracker goal
int left_state, right_state; // Left and right tracker state
static unsigned int left_current[10] = {0}; // This is a circular buffer for
static unsigned int right_current[10] = {0}; // storing current values. We
// need this because the sensors
// are noisy. We’ll take an avg
// of the last ten values.
static int i = 0; // Current buffer counter
unsigned int left_current_avg, right_current_avg;
int n; // Counter
int drive_state = DR_UNDEFINED;
if (Clock > Old_Clock) // stay synchronized to beacon data updates (this is important)
{
left_current* = Get_Analog_Value( rc_ana_in01 ); // Get the currents for
right_current* = Get_Analog_Value( rc_ana_in02 ); // each wheel. Just comment
// out all the current code
// if not using current sensors
// If we get a high current value, check the last ten to be sure its not just a glitch
if (((left_current* > MAX_CURRENT) || (right_current* > MAX_CURRENT)) &&
(Navigator_State != WAIT_TO_DRIVE) && (Navigator_State != AT_TARGET))
{
// Maybe this is real, but current sensors can be noisy
// Check the last ten values to see if they agree
left_current_avg = 0;
right_current_avg = 0;
for (n = 0; n < 10; n++)
{
left_current_avg += left_current[n];
right_current_avg += right_current[n];
}
left_current_avg /= 10;
right_current_avg /= 10;
if ((left_current_avg > MAX_CURRENT) || (right_current_avg > MAX_CURRENT))
{
Navigator_State = OVER_CURRENT;
printf("Left_current = %d Right_current = %d
", left_current_avg, right_current_avg);
printf("OVER_CURRENT
"); // Over current
drive_timer = 0;
}
}
if (i++ >= 10) i=0; // Reset our circular buffer counter if needed
// This is the main Navigator state machine implemented as a switch statement
switch (Navigator_State)
{
// Just wait until a button is pressed before we start (skip for autonomous mode)
// case WAIT_TO_DRIVE:
{
// Don’t go anywhere
left_drive_set = LSTOP;
right_drive_set = RSTOP;
// Eyes forward
left_servo_set = 127;
right_servo_set = 127;
if (p1_sw_trig)
{
Navigator_State = DRIVE_TO_CENTER;
printf("DRIVE_TO_CENTER
"); // Driving to center
left_servo_set = 127;
right_servo_set = 127;
}
break;
}
case DRIVE_TO_CENTER:
{
// Drive away from the wall.
left_drive_set = FFWD;
right_drive_set = FFWD;
if (drive_timer++ > DRIVE_TO_CENTER_TIME)
{
Navigator_State = TURN_TO_TARGET;
printf("TURN_TO_TARGET
"); // Done driving to center
left_servo_set = 127;
right_servo_set = 127;
}
break;
}
case OVER_CURRENT:
{
// If we get here let’s back off a little and start over
// Note: we could do better here if we remembered that we
// passed the target. (Exercise left to the student :-))
left_drive_set = LSTOP;
right_drive_set = RSTOP;
if (drive_timer++ > DRIVE_TO_CENTER_TIME)
{
Navigator_State = TURN_TO_TARGET;
printf("TURN_TO_TARGET
"); // Done driving to center
left_servo_set = 127;
right_servo_set = 127;
}
break;
}
case TURN_TO_TARGET:
{
// Point trackers straight ahead, then turn
// until at least one tracker sees the beacon
left_servo_set = 127;
right_servo_set = 127;
left_drive_set = SBWD;
right_drive_set = SFWD;
if (((LL) && (LR)) || ((RL) && (RR)))
{
Navigator_State = DRIVE_TO_TARGET; // At least one sensor sees the target
printf("DRIVE_TO_TARGET
");
}
break;
}
case DRIVE_TO_TARGET:
case PASSED_TARGET:
{
// As long as we are locked on to the beacons with both targets
// drive toward them
Track_Beacon(LEFT); // allow the left beacon tracker state machine to run
Track_Beacon(RIGHT); // allow the right beacon tracker state machine to run
// update the servo PWM values with the values calculated by Track_Beacon() above
left_servo_set = Tracker_Data[LEFT].Position;
right_servo_set = Tracker_Data[RIGHT].Position;
if ((Tracker_Data[LEFT].Status == BOTH_IN_VIEW) &&
(Tracker_Data[RIGHT].Status == BOTH_IN_VIEW))
{
// We are locked on with both targets, so drive!
// Find out which way the servoes are pointed
if (left_servo_set < 127) // Left servo pointed left
{
left_state = DR_LEFT;
}
else if (left_servo_set > LEFT_GOAL) // Left servo pointed right
{
left_state = DR_RIGHT;
}
else
{
left_state = DR_CENTER;
}
if (right_servo_set > 127) // Right servo pointed right
{
right_state = DR_RIGHT;
}
else if (right_servo_set < RIGHT_GOAL) // Right servo pointed left
{
right_state = DR_LEFT;
}
else
{
right_state = DR_CENTER;
}
// Now find out the drive state
if (left_state == DR_LEFT && right_state == DR_LEFT)
drive_state = DR_BOTH_LEFT;
else if (left_state == DR_RIGHT && right_state == DR_RIGHT)
drive_state = DR_BOTH_RIGHT;
else if (left_state == DR_RIGHT && right_state == DR_LEFT)
drive_state = DR_BOTH_CENTER; // At goal
else if (left_state == DR_CENTER && right_state == DR_CENTER)
drive_state = DR_BOTH_STRAIGHT;
else if (left_state == DR_CENTER)
drive_state = DR_LEFT_STRAIGHT;
else if (right_state == DR_CENTER)
drive_state = DR_RIGHT_STRAIGHT;
else
drive_state = DR_REFLECTION; // Impossible angle, but could happen
drive_timer = 0; // As long as we see the target keep driving
// Drive logic starts here
switch (drive_state)
{
case DR_UNDEFINED:
{
// We should not be able to get here
printf("DR_UNDEFINED
");
break;
}
case DR_BOTH_LEFT:
{
// Go straight until we pass the target
if (Navigator_State == DRIVE_TO_TARGET)
{
// Don’t turn left
left_drive_set = SFWD;
right_drive_set = SFWD;
}
else
{
// We are in PASSED_TARGET state
// Turn left
left_drive_set = SBWD;
right_drive_set = SFWD;
}
if (left_servo_set < LEFT_PASS)
{
Navigator_State = PASSED_TARGET;
printf("PASSED_TARGET
");
}
break;
}
case DR_BOTH_RIGHT:
{
// Go straight until we pass the target
if (Navigator_State == DRIVE_TO_TARGET)
{
// Don’t turn right
left_drive_set = SFWD;
right_drive_set = SFWD;
}
else
{
// We are in PASSED_TARGET state
// Turn right
left_drive_set = SFWD;
right_drive_set = SBWD;
}
if (right_servo_set > RIGHT_PASS)
{
Navigator_State = PASSED_TARGET;
printf("PASSED_TARGET
");
}
break;
}
// The beacon is ahead in all these cases so just drive
case DR_BOTH_STRAIGHT:
case DR_LEFT_STRAIGHT:
case DR_RIGHT_STRAIGHT:
// Note: we don't know what to do here but we might as well keep
// going.
case DR_REFLECTION:
{
// Find the error from ideal on each side
left_error = (int)LEFT_GOAL - (int)Tracker_Data[LEFT].Position;
right_error = (int)Tracker_Data[RIGHT].Position - (int)RIGHT_GOAL;
// Scale the error
left_error *= DRIVE_GAIN;
right_error *= DRIVE_GAIN;
// Use these if you want fractional gain values
// Be sure to comment out the above gain lines
// left_error /= DRIVE_GAIN;
// right_error /= DRIVE_GAIN;
// Make sure the errors are in range
if (left_error > (255 - SFWD)) left_error = 255 - SFWD;
if (right_error > (255 - SFWD)) right_error = 255 - SFWD;
if (left_error < -SFWD) left_error = -SFWD;
if (right_error < -SFWD) right_error = -SFWD;
// If we are pointed left, drive the right drive faster and vice versa
// Use the next two lines to turn toward the goal
// left_drive_set = SFWD + right_error;
// right_drive_set = SFWD + left_error;
// These two lines keep driving straight
left_drive_set = SFWD;
right_drive_set = SFWD;
// printf("Lerr = %d Rerr = %d
",left_error,right_error);
// End of drive logic
break;
}
case DR_BOTH_CENTER:
{
Navigator_State = AT_TARGET; // Both servos are within the target threshold
printf("AT_TARGET
");
break;
}
}
}
else
{
// We are not locked on - Stop
// give the trackers a chance to catch up
left_drive_set = LSTOP;
right_drive_set = RSTOP;
drive_timer++ ;
if (drive_timer > LOST_TARGET_TIMEOUT)
{
Navigator_State = TURN_TO_TARGET;
printf("TURN_TO_TARGET
"); // Done driving to center
left_servo_set = 127;
right_servo_set = 127;
}
}
break;
}
case AT_TARGET:
{
// We are at the goal - Stop driving
left_drive_set = LSTOP;
right_drive_set = RSTOP;
break;
}
default:
{
left_drive_set = LSTOP;
right_drive_set = RSTOP;
printf("Not a valid state
");
break;
}
}
// Send all the control values to the pwm controller
//printf("Left = %d Right = %d
", (int)left_drive_set, (int)right_drive_set);
left_servo = left_servo_set;
right_servo = right_servo_set;
left_drive = left_drive_set;
right_drive = right_drive_set;
// left_drive = 255 - left_drive_set; // This drive is reversed
// right_drive = 255 - right_drive_set; // This drive is reversed
Old_Clock = Clock; // take a snapshot of the clock
}
}
//PLEASE HELP ME IF YOU CAN!!!****