/*--------------------------------------------------------------------*/
//
// Final Project - Collision Avoiding Robot
// Author: Demian Raven and Man Chun Liu
// Class: CSE 444 Spring 2001
// File Name: sonarobot.c
// Date: December 18th, 2001
//
/*--------------------------------------------------------------------*/
/*------------------------------------------------------------------------
// LED DISPLAY OF MOTOR CONTROL
// displayed in 8-segment led
// case 1: both leds off - motor is off
// case 2: led 1 on, led 2 off - motor is forward
// case 3: led 1 off, led 2 on - motor in reverse
// case 4: both leds on - motor is off
-------------------------------------------------------------------------*/
#include
/*------------------------ global constant ---------------------------*/
#define WAIT_FOR_NEXT_MEASUREMENT 34000 // about 100ms
#define MAX_WAIT 65535 // not use now
#define DELAY 130 // about 500us
#define TOO_CLOSE 450
#define MOTOR_CONSTANCY 3
/*------------------------ hardware macros ---------------------------*/
#define _ENABLE_INTERRUPTS { EA = 1; }
#define _DISABLE_INTERRUPTS { EA = 0; }
#define _ENABLE_TIMER2_INTERRUPT { ET2 = 1; }
#define _DISABLE_TIMER2_INTERRUPT { ET2 = 0; }
#define _TIMER2_ON { TR2 = 1; }
#define _TIMER2_OFF { TR2 = 0; }
#define _CLEAR_TIMER2_FLAGS { TF2 = 0; EXF2 = 0; }
#define INIT P1_5
#define HIGH 1
#define LOW 0
#define TRUE 1
#define FALSE 0
#define MOTOR1_INPUT_A RXD
#define MOTOR1_INPUT_B INT0
#define MOTOR2_INPUT_A T0
#define MOTOR2_INPUT_B RD
/*--------------------------- Global Data ----------------------------*/
static unsigned int sonarValue = 0;
static bit trigger = FALSE;
static bit ignore = FALSE;
static bit left_triggered = FALSE;
static bit right_triggered = FALSE;
/*----------------------- Function Prototypes ------------------------*/
void initialize();
void forward();
void spin_left();
void spin_right();
/*-------------------------- Main Function ---------------------------*/
void main(void) {
// counter values
int i=0;
unsigned int j=0;
// initializing timer 2
initialize();
// for debugging, ON button
P2_3 = LOW; // for led
trigger = FALSE;
while (trigger == FALSE)
{
trigger = P2_1;// wait for button pressed
}
P2_3 = HIGH;
// continuous loop
while (TRUE)
{
// clear led display
P3 |= 0xFF;
//HERE: if left or right, then stop reversing.... go forward
if (left_triggered == TRUE || right_triggered == TRUE) forward();
// clear the triggers
left_triggered = FALSE;
right_triggered = FALSE;
// number of times we loop before we clear the triggers - allows for
// fine-tuning spinning away from obstacles
for (i = 0; i < MOTOR_CONSTANCY; i++)
{
// INIT go high
INIT = HIGH;
// delay 500us
ignore = TRUE;
_TIMER2_ON;
for (j=0; j < DELAY; j++);
ignore = FALSE;
// sleep
PCON |= 0x01;
//HERE: if both then go to the right (case 1,1)
//HERE: if left then spin right (case 1,0)
//HERE: if right, then spin left (case 0,1)
//HERE: otherwise, go forward... (case 0,0)
if (left_triggered == TRUE) spin_right();
else
{
if (right_triggered == TRUE) spin_left();
else forward();
}
// individual display of triggering of sonar's minimal distances
P2_2 = (left_triggered == TRUE) ? LOW : HIGH;
P2_4 = (right_triggered == TRUE) ? LOW : HIGH;
//dalay loop between sonar measurements
for (j=0; j < WAIT_FOR_NEXT_MEASUREMENT; j++);
}
}
}
/*-------------------------- Sonar Top ISR ---------------------------*/
void sonar_top(void) interrupt 5 {
// clear interrupt flags
_CLEAR_TIMER2_FLAGS;
// if correct time to read measuremets
if (ignore == FALSE) {
// NOTE!!! turn sonar off here
sonarValue = ((RCAP2H << 8) + RCAP2L) >> 4;
if (sonarValue < TOO_CLOSE)
{
// if detecting potential obstacle and P1_2 is low,
// then it is the left sonar
if (P1_2 == LOW)
{
left_triggered = TRUE;
}
// otherwise P1_2 is high, it is the right sonar
else right_triggered = TRUE;
}
_TIMER2_OFF; // turn off timer2
INIT = LOW; // INIT go low
// clear timer2 registers
TH2 = 0x00;
TL2 = 0x00;
RCAP2H = 0x00;
RCAP2L = 0x00;
}
return;
}
/*-------------------------- initialization --------------------------*/
void initialize() {
PSW = 0x00;
_DISABLE_INTERRUPTS; // disable all interrupts
T2CON |= 0x09; // set timer 2 to negative-edge capture mode
_ENABLE_INTERRUPTS; // enable all interrupts
_ENABLE_TIMER2_INTERRUPT; // enable timer 2 interrupt
INIT = LOW; // set init to low
//HERE: motor is stopped
P3 = 0xFF;
}
void forward()
{
MOTOR1_INPUT_A = LOW;
MOTOR1_INPUT_B = HIGH;
MOTOR2_INPUT_A = HIGH;
MOTOR2_INPUT_B = LOW;
}
void spin_right()
{
MOTOR1_INPUT_A = LOW;
MOTOR1_INPUT_B = HIGH;
MOTOR2_INPUT_A = LOW;
MOTOR2_INPUT_B = HIGH;
}
void spin_left()
{
MOTOR1_INPUT_A = HIGH;
MOTOR1_INPUT_B = LOW;
MOTOR2_INPUT_A = HIGH;
MOTOR2_INPUT_B = LOW;
}
/*--------------------------------------------------------------------*/
// end sonarobot