IRbeacon.h
#ifndef IRBEACON_H
#define IRBEACON_H
//NOTE: This module assumes input is high when beacon is off
//and low when beacon is on.
//NOTE: Only works for 1.25 kHz beacon
//constants for FindBeacon()
#define LEFT 0
#define RIGHT 1
/*This module uses T6 to detect beacons and calculate duty cycles
It enables interrupts and uses a clock scale value of 4 (166 ns/tic)*/
//accessible functions
void BeaconInit(void);
/*Sets up timers and interrupts for interrupt-driven signal measuring*/
unsigned char BeaconDcycle(void);
/*Returns duty cycle of beacon currently in sight
returns zero if no beacon is detected*/
void FindBeacon(unsigned char Dcycle, unsigned char direction);
/*Turns robot in given direction until desired beacon duty cycle is found.*/
#endif
IRbeacon.c
#include <hidef.h> /* common defines and macros */
#include <mc9s12e128.h> /* derivative information */
#include <S12E128bits.h> /* E128 bit definitions */
#include <bitdefs.h> /* BIT0HI etc. */
#include <stdio.h>
#include <stdlib.h>
#include "S12eVec.h"
#include "IRbeacon.h"
#include "Startup.h"
#include "MotorControl.h"
//----------------------------------------------------------------
//--------------------- Module Variables -------------------------
//----------------------------------------------------------------
//module variable for tracking time w/ overflow
typedef union
{
struct
{
unsigned int high;
unsigned int low;
}ByInt;
unsigned long AsLong;
}LongByInts;
static LongByInts ThisTime;
static LongByInts LastTime;
static unsigned long LoTime;
static unsigned long HiTime;
static unsigned long Delta;
static unsigned int NumOverFlow;
//constants
static const unsigned int SignalPeriod = 4802;
//number of tics in a 0.8 ms period (1250 Hz)
//------------------------ Interrupts ----------------------------
//~~~~~~~~~~ Beacon Interrupt ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Calculates the time between rising and falling edges,
if a low period just ended, stores the result in LoTime
if a high period just ended, stores the result in HiTime. */
void interrupt 38 T6beaconChange(void)
{
//check for recent overflow
if( ((TIM1_TFLG2 & _S12_TOF) == _S12_TOF) && (TIM1_TC6<0x8000) )
{
NumOverFlow++;
//reset flag
TIM1_TFLG2 = _S12_TOF;
}
//calculate period
ThisTime.ByInt.high = NumOverFlow;
ThisTime.ByInt.low = TIM1_TC6;
if(PTT & BIT6HI) //if currently high, a falling edge just occurred
{ //calculate low time
Delta = (ThisTime.AsLong - LastTime.AsLong);
HiTime = Delta;
}
else
{
Delta = (ThisTime.AsLong - LastTime.AsLong);
LoTime = Delta;
}
LastTime = ThisTime;
//reset flag
TIM1_TFLG1 = _S12_C6F;
return;
}
//~~~~~~~~~~~ Overflow Interrupt ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Updates overflow count each time there is an overflow*/
void interrupt 40 Overflow(void)
{
NumOverFlow++;
//reset flag
TIM1_TFLG2 = _S12_TOF;
return;
}
//-------------------------- Functions -------------------------
//~~~~~~~~~~~~ BeaconInit ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Sets up input capture on T2 (Timer 1 channel 6) and enables interrupts*/
void BeaconInit(void)
{
EnableInterrupts;
//enable Timer 1
TIM1_TSCR1 = _S12_TEN;
//scale timer
TIM1_TSCR2 = _S12_PR1; //divide M by 4: 166 ns/tic
//input capture on T2 (Timer 1 channel 6)
//Setup T2 as Input Capture (0)
TIM1_TIOS = (TIM1_TIOS & (~_S12_IOS6));
//Set T2 to respond to all edges
TIM1_TCTL3 = (TIM1_TCTL3 | (_S12_EDG6A | _S12_EDG6B));
//Enable interrupts on T2
TIM1_TIE = (TIM1_TIE | _S12_C6I);
//reset flag
TIM1_TFLG1 = _S12_C6F;
//enable overflow interrupt
TIM1_TSCR2 = (TIM1_TSCR2 | _S12_TOI);
return;
}
//~~~~~~~~~~~~ Find Beacon ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Rotates robot in given direction until desired beacon duty cycle is found.
In order to see a beacon, the robot needs to see the the correct duty cycle
(range is +/- 5) NumCheck times in a row.*/
void FindBeacon(unsigned char Dcycle, unsigned char direction)
{
int j=0;
int NumCheck=10;
unsigned char DC;
if (direction == RIGHT)
{
RightWheelBackward(60);
LeftWheelForward(60);
} else if (direction == LEFT)
{
LeftWheelBackward(60);
RightWheelForward(60);
}
DC=0;
while(j<NumCheck) //check NumCheck times that it's the right beacon
{
DC = BeaconDcycle();
if(DC <(Dcycle-5) || DC >(Dcycle+5))
{
j=0; //reset number of times beacon is seen if a value
} //outside of the range appears
while (DC <(Dcycle-5) || DC >(Dcycle+5)) //check until duty cycle is correct
{
DC = BeaconDcycle();
}
//delay for 1ms between checks (gives time to measure duty cycle between checks)
delay(2);
j++;
}
//stop turning when beacon is found
StopRight();
StopLeft();
// correct for overshoot
if (direction == RIGHT)
{
RightWheelForward(60);
LeftWheelBackward(60);
} else if (direction == LEFT)
{
LeftWheelForward(60);
RightWheelBackward(60);
}
delay(100);
StopRight();
StopLeft();
return;
}
//~~~~~~~~~~~~ BeaconDcycle ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/*Uses values strored in HiTime and LowTime to calculate the
duty cycle of the beacon in sight. Returns zero if no beacon. */
unsigned char BeaconDcycle(void)
{
static unsigned char Dcycle = 0;
LongByInts CurrentTime;
CurrentTime.ByInt.high = NumOverFlow;
CurrentTime.ByInt.low = TIM1_TCNT;
//if no rising/falling edges in a time period longer than the
//frequency of the beacon, return zero if currently high 100 if currently low
if((CurrentTime.AsLong - LastTime.AsLong) >= SignalPeriod)
{
if(PTT & BIT2HI)
{
Dcycle = 0;
}
else
{
Dcycle = 100;
}
}
else if(LoTime >=SignalPeriod)
{
Dcycle = 0;
}
else if(HiTime >= SignalPeriod)
{
Dcycle = 100;
}
else
{
//calculate Dutycycle
Dcycle = ((long) HiTime*100)/SignalPeriod;
}
return Dcycle;
}