Final Psuedo-Code
Main: This main coordinates the sensor input and actuator outputs to the E128 in order to play SPDL Happy Fun Ball
void main(void)
unsigned
char keyPress;
unsigned char state;
unsigned char tapeState;
unsigned char sensorCheck;
unsigned char balls = 0;
unsigned char tripNumber = 0;
Initialize all modules
Initial state is waiting for flash
while(1)
If 2 min up, end of game
stop drive and drawbridge motors
break from loop
If flash has occurred
Handle flash event
If sensing not 0% DC IR
Handle IR sensor event
Update all tape sensor values
If driving toward black tape
Check which field robot is on
If driving on black tape
Check if robot at the corner
If middle tape sensor over tape
Handle tape event
Else
Handle off tape event
If requesting balls
For(balls = 1; balls < 5 ; balls++){
Request ball
tripNumber++;
if(tripNumber<4)
Point robot toward Goal 3
Else
Point robot toward Goal 1
If releasing balls
Lower drawbridge
Point robot toward ball dispenser
If drawbridge is up (switch high)
Stop drawbridge motor
IR Sensor Module: This module works with a phototransistor in a trans-resistive circuit with signal conditioning to detect IR signals emitted at 1.25 kHz using the Port T6 and T7 of E128 board as inputs. Measures the signals duty cycle using input capture interrupts on those ports.
Module Variables
typedef union
struct
unsigned int high;
unsigned int low;
AsInt;
unsigned long AsLong;
LongByInts;
static unsigned long IRSignalPeriod = 2400; // 0.8 ms
static unsigned int TIM0_OC6_Period = 4800; // 1.6 ms
static unsigned int TIM0_OC7_Period = 24000; // 8 ms
static LongByInts TimeOfRisingEdge;
static LongByInts TimeOfFallingEdge;
static LongByInts CurrentTime;
static unsigned long TimeOfLastFallingEdge;
static unsigned long HighTime;
static unsigned long DutyCycle;
static unsigned long Period;
static unsigned int Overflows;
static unsigned long n; // number of DC terms averaged over
static unsigned long SumDutyCycle; // running sum of n number of DC terms
static unsigned long AvgDutyCycle; // SumDutyCycle/n
Module Functions
void IRSensor_Init(void):
Initializes Timer 0 for IR signal
detection.
Enable TIM0
Enable TIM0 overflow interrupt & set TIM_0 to 3 MHz tick rate
Clear TIM0 overflow counter
Clear overflow flag
Enable TIM0_TC4 interrupt
Set TIM_0 IC4 to capture rising edges
Clear TIM_0 IC4 flag
TimeOfRisingEdge.AsInt.high = 0;
TimeOfRisingEdge.AsInt.low = current time
Enable TIM0_TC5 interrupt
Set TIM0_TC5 to capture falling edges
Clear TIM0_TC5 flag
TimeOfFallingEdge.AsInt.high = 0;
TimeOfFallingEdge.AsInt.low = current time
TimeOfLastFallingEdge = current time
Enable TIM0_TC6 interrupt
Set TIM0_TC6 to output capture
Set no pin connect for TIM0_OC6
Set first compare for TIM0_OC6
Clear flag for TIM0_OC6
Enable TIM0_TC7 interrupt
Set TIM0_TC7 to output capture
Set no pin connect for TIM0_OC7
Set first compare for TIM0_OC7
Clear flag for TIM0_OC7
SumDutyCycle = 0;
AvgDutyCycle = 0;
n = 0;
void interrupt _Vec_tim0ch4 IRSensor_DetectRisingEdge(void): Checks for rising edges of IR signal on
Timer IC4 (Port T0)
Check for pending Timer 0 overflow
clear Timer 0 overflow flag
Overflows++;
Update time of rising edge
Clear Timer 0 IC4 flag
void interrupt _Vec_tim0ch5 IRSensor_DetectFallingEdge(void): Checks for falling edges of IR signal on
Timer IC5 (Port T1) and computes IR signal high time.
Check for pending TIM0 overflow
clear TIM0 overflow flag
Overflows++;
Update time of falling edge
Calculate high time
Calculate period
Calculate duty cycle
Clear Timer 0 IC5 flag
Add new duty cycle to SumDutyCycle
n++;
void interrupt _Vec_tim0ovf IRSensor_CheckOverflows(void): Checks for timer overflows.
clear Timer 0 overflow flag
Overflows++;
void interrupt _Vec_tim0ch6 IRSensor_CheckZeroDutyCycle(void): Checks if duty cycle is zero.
Check for pending TIM0 overflow
Clear TIM0 overflow flag
Overflows++;
Update current time
If duty cycle update time > IRSignalPeriod
DutyCycle = 0
Program next compare for TIM0_0C6
Clear TIM0_0C6 flag
void interrupt _Vec_tim0ch7 IRSensor_CalcAvgDutyCycle(void): Every TIM_OC7 period, calculates the average
duty cycle of the DC measured over the period
Check for pending TIM0 overflow
Clear TIM0 overflow flag
Overflows++;
Program next compare for TIM0_0C7
Clear TIM0_0C7 flag
if (n!=0)
AvgDutyCycle = SumDutyCycle/n;
n = 0;
SumDutyCycle = 0;
Else
AvgDutyCycle = 0;
unsigned long IRSensor_GetDutyCycle(void): Returns the duty cycle of the detected IR
signal.
return DutyCycle;
unsigned long IRSensor_GetAvgDutyCycle(void):
Returns the average duty cycle of
the detected IR signal over the specified time in ms.
return AvgDutyCycle;
unsigned char IRSensor_Check(void): Returns 1 if detecting an average duty
cycle != 0, else returns 0.
if(AvgDutyCycle != 0)
return 1;
else
return 0;
unsigned char IRSensor_Handle(unsigned
char state): If in the
SEARCH_FOR_50IR state and seeing close to a 50% DC, stops turning, and returns
TO_BLACK_TAPE state. Else returns same state passed in.
If state = SEARCH_FOR_50IR)
If (AvgDutyCycle > 45 and < 55)
Stop robot driving
return TO_BLACK_TAPE;
return
state
Flash Sensor Module: This module works with a phototransistor input to Port T2 of the E128 board to detect a flash (a voltage HI).
Module Variabless
unsigned char Flash = 0; // = 0 if no Flash, = 1 if Flash has occurred
Module Functions
void FlashSensor_Init(void):
Initializes Port T2 to be an
input for flash sensor to check for flash.
Set Port T2 to be input
unsigned char FlashSensor_Check(void):
Checks for game start flash.
Return 0 if no flash is seen, returns 1 if flash is seen
If Port T2 is lo (flash has not occurred)
return 0;
else
Flash = 1
return 1
unsigned char FlashSensor_Handle(unsigned
char state): If FlashSensor event has occurred during WAIT_FOR_FLASH state,
starts bot turning and returns state SEARCH_FOR_50IR.
If state = WAIT_FOR_FLASH and Flash = 1
Turn robot left
return SEARCH_FOR_50IR
else
return state
Tape Sensor
Module: Initializes
ports for tape sensor inputs. Checks tape sensor values, returning 1 if on
tape, and responds to tape events with calling motor functions to drive and
position the bot correctly.
E128 connections
Port AD0 - Left tape
sensor input
Port AD1 - Middle
tape sensor input
Port AD2 - Right
tape sensor input
Port AD3 - Front
right tape sensor input
Port AD4 - Front
left tape sensor input
Port AD5 - Back left
right tape sensor input
Port AD6 - Back
right tape sensor input
Module Variables
#define ON_TAPE 1
#define WHITE_FLOOR
0
#define BLACK_TAPE 2
#define RED_TAPE 3
#define GREEN_TAPE 4
#define SIDE_A 1
#define SIDE_B 2
#define SIDE_FOUND 1
#define LOOKING_FOR_SIDE 0
#define CORNER_FOUND 1
#define LOOKING_FOR_CORNER 0
// maximum threshold AD values for tapes for the:
// left, middle, right, front right, front left, back left, back right sensors
static unsigned int greenTape[] = {800,750,600,400,600,700,700}; // for green tape
static unsigned int redTape[] = {600,450,350,200,250,450,350}; // for red tape
static unsigned int blackTape[] = {400,350,250,175,200,300,250}; // for black tape
// = #defined values above if on tape, else = 0
static unsigned int tapeSensor[] = {0,0,0,0,0,0,0};
static unsigned int ADvalue;
static unsigned char FieldSide; // = SIDE_A or SIDE_B
static unsigned char checkSide; // = SIDE_FOUND or LOOKING_FOR_SIDE
static unsigned char checkCorner; // = CORNER_FOUND or LOOKING_FOR_CORNER
static unsigned char fourthTrip; // = 1 if on fourth trip to a goal, else = 0
Module Functions
void InitTapeSensors(void):
Initialize AD ports to be analog.
Set Port AD pins to be analog
checkSide = LOOKING_FOR_SIDE;
checkCorner = LOOKING_FOR_CORNER;
fourthTrip = 0;
unsigned char CheckTape(unsigned
char sensorNum): Reads the AD input of the tape sensor corresponding to the sensor ID
passed in. Returns whether the tape sensor is ON_TAPE, else returns 0.
Read A/D value that corresponds to sensorNum
if (ADvalue < threshold for green tape for sensorNum
if (ADvalue < threshold for red tape for sensorNum
if(ADvalue < threshold for black tape for sensorNum
tapeSensor[sensorNum] = BLACK_TAPE
return ON_TAPE;
tapeSensor[sensorNum] = RED_TAPE;
return ON_TAPE;
tapeSensor[sensorNum] = GREEN_TAPE;
return ON_TAPE;
tapeSensor[sensorNum] = WHITE_FLOOR;
return 0;
void UpdateTapeSensors(void):
Calls CheckTape
for all the tape sensors, so tapeSensor variable has current state of all the
sensors.
Check left tape sensor
Check right tape sensor
Check front left tape sensor
Check front right tape sensor
Check back left tape sensor
Check back right tape sensor
unsigned char HandleTape(unsigned
char state): Given the state you
are in, initiates sequence in response to tape sensor being over tape. Returns the new state value.
If state = driving to black tape
and middle sensor is over black tape
Set left and right motor drive duty cycles to slower speed
If FieldSide = SIDE_B
While front left tape sensor is not on tape
Go forward
If drawbridge switch is closed
Stop drawbridge motor
While middle tape sensor is not on tape
Turn robot left
If drawbridge switch is closed
Stop drawbridge motor
Stop robot driving
Else if FieldSide == SIDE_A
While front right tape sensor is not on tape
Go forward
If drawbridge switch is closed
Stop drawbridge motor
While middle tape sensor is not on tape
Turn robot right
If drawbridge switch is closed
Stop drawbridge motor
Stop robot driving
Else
Print "you don't know what side you're on! AAAAHHHH!"
Drive robot forward
Return FOLLOW_BLACK_TAPE;
Else if driving to green tape (from Ball Dispenser) and middle sensor is over green tape
Set left and right motor drive duty cycles to slower speed
Drive forward until back left or right tape sensors are not on tape
Stop robot driving
If FieldSide = SIDE_A
While middle tape sensor is on tape
Turn robot right
Stop
return FOLLOW_GREEN_TAPE
Else if FieldSide = SIDE_B
While middle tape sensor is on tape
Turn robot left
Stop robot driving
return FOLLOW_GREEN_TAPE
Else if driving to black tape from Goal 3 and middle sensor is on black tape
Set left and right motor drive duty cycles to slower speed
Drive robot forward
While back left or right tape sensor is not on tape
Wait
If drawbridge switch is closed
Stop drawbridge motor
Stop robot driving
DriveMotor_Drive(STOP);
If FieldSide = SIDE_A
While middle tape sensor is not on tape
Turn robot right
If drawbridge switch is closed
Stop drawbridge motor
Stop
return FOLLOW_BLACK_TAPE
else if FieldSide = SIDE_B
While middle tape sensor is not on tape
Turn robot left
If drawbridge switch is closed
Stop drawbridge motor
Stop
return FOLLOW_BLACK_TAPE
Else if driving to black tape (from Ball Dispenser) and middle sensor is on black tape
Set left and right motor drive duty cycles to slower speed
Drive Robot forward
Wait 300 ms
Set left and right motor drive duty cycles to faster speed
If FieldSide = SIDE_A
While middle tape sensor is not on tape
Turn robot right
Stop robot driving
Return FOLLOW_BLACK_TAPE;
If FieldSide = SIDE_B
While middle tape sensor is not on tape
Turn robot left
Stop robot driving
Return FOLLOW_BLACK_TAPE;
Else if driving on black tape (to Ball Dispenser) and middle sensor is on black tape
If front left and right sensor not on white floor
Stop robot driving
Wait 1000ms
If fourthTrip
Do reverse sequence
Turn robot Right 90Degrees
return RELEASING_BALLS
else
return REQUESTING_BALLS;
Else
Drive robot forward
return state
Else If driving on green tape (to Goal 3) and middle sensor is on green tape
If front right and left tape sensor = WHITE_FLOOR
If FieldSide=SIDE_A
Stop robot driving
Drive robot backward
Wait 300 ms
Stop robot driving
return RELEASING_BALLS
else
Turn robot Left 90Degrees
While middle tape sensor is not on tape
Turn robot left
Stop robot driving
Drive robot forward
Wait 300 ms
Stop robot driving
return RELEASING_BALLS;
Else
Drive robot forward
Return state
unsigned char HandleOffTape(unsigned
char state): Given the state you are in, initiates
sequence in response to tape sensor not being over tape. Returns
the state value.
If driving to black or green tape
Set left and right motor drive duty cycles to slower speed
Drive robot forward
If driving along tape
If right tape sensor is not on white floor
Turn robot right
If left tape sensor is not on white floor
Turn robot left
return state;
void CheckSide(void):
When robot crosses black tape
(when driving from starting box, use the tape sensor values to determine if bot is on Side A or B and record that.
If checkSide=LOOKING_FOR_SIDE
If right sensor is on black tape
FieldSide = SIDE_B;
If right sensor is on black tape
FieldSide = SIDE_A;
checkSide = SIDE_FOUND;
Stop robot driving
void CheckCorner(void): When robot drives along black tape (when
driving from starting box) check to see if at the right angle turn in the tape and
if robot is, conducts sequence so turns to keep driving along black tape.
If checkCorner=LOOKING_FOR_CORNER and checkSide=SIDE_FOUND
Update tape sensor values
If front right sensor over black tape
Drive robot forward
Wait 300 ms
Stop robot driving
While middle tape sensor is not on tape
Turn robot right
Stop robot driving
checkCorner = CORNER_FOUND
If front left sensor over black tape
Drive robot forward
Wait 300 ms
Stop robot driving
While middle tape sensor is not on tape
Turn robot left
Stop robot driving
checkCorner = CORNER_FOUND;
void TurnLeft90Degrees(void): Control drive motors so robot turns left 90
degrees
Turn robot left
Wait 900 ms
Stop robot driving
void TurnRight90Degrees(void): Control drive motors so robot turns right 90
degrees
Turn robot right
Wait 800 ms
Stop robot driving
void TurnLeft45Degrees(void): Control drive motors so robot turns left 45
degrees
Turn robot left
Wait 400 ms
Stop robot driving
void TurnRight45Degrees(void): Control drive motors so robot turns right 45
degrees
Turn robot right
Wait 400 ms
Stop robot driving
void Reverse(void): Control drive motors so robot backs up a set
distance
Drive robot backward
Wait 600ms
Stop robot driving
void Forward(void): Control drive motors so robot goes forward a
set distance
Drive robot forward
Wait 600ms
Stop robot driving
unsigned char PointToGoal3(void): Executes sequence so robot goes from
pointing toward Ball Dispenser to being pointed toward green tape to Goal 3
Reverse sequence
If FieldSide=SIDE_B
Turn robot Left
90Degrees
Else if FieldSide=SIDE_A
Turn robot Right 90Degrees
Else print “error. unknown side!”
Drive robot forward
return TO_GREEN_TAPE;
unsigned char PointToBallDispenser(void):
Executes sequence so robot goes
from pointing toward Goal 3 to being pointed toward black tape to Ball
Dispenser
if fourthTrip = 1
return GAME_DONE
if FieldSide=SIDE_B
Drive robot forward
Wait 1000ms
Stop robot driving
Turn robot Right 90Degrees
if FieldSide=SIDE_B
Drive robot backward
Wait 1000ms
Stop robot driving
Turn robot Right 90Degrees
else print "error. unknown side”
Set left and right motor drive duty cycles to faster speed
Drive robot forward
return BACK_TO_BLACK_TAPE
unsigned char PointToGoal1(void): Executes sequence so robot goes from
pointing toward Ball Dispenser to being pointed toward black tape to Goal 1
Revese sequence
If FieldSide=SIDE_A
Turn robot left 135Degrees
If FieldSide=SIDE_B
Turn robot right 135Degrees
Else print “error. unknown side!"
Drive robot forward
fourthTrip = 1;
return TO_BLACK_TAPE_TO_GOAL1
Switch Module: This module works with Port U4 on an E128 board to check if its switch input is high (i.e. drawbrdige closed) or low (drawbrdige open)
Module Variabless
#define CLOSED 1
#define OPEN 0
Module Functions
void Switch_Init(void):
Initializes Port U 4-7 to be
inputs
Set Port U4-7 to be inputs
unsigned char CheckSwitchHit(unsigned
char switchNum): Returns HIT if the switchNum switch is
closed and NO_HIT if it's open.
switch(switchNum)
case DRAWBRIDGE_SWITCH:
If Port U4 is hi
return HIT
else
return NO_HIT
default:
print "error in switch. you should not be in here.”
Drive Motor Module: This module controls the drive motors of the robot using the clock of Port U0 and U1 of the E128 board and PWM outputs to Port T3 & T4.
Module Variabless
static unsigned char LeftDutyCycle
static unsigned char RightDutyCycle
Module Functions
void DriveMotor_Init(void):
Initializes the E128 Port U clock
to control PWM outputs to Port T3 & T4, the drive motors
Set Clock A to 12 MHz
Set Clock SA to 23.4 kHz
Set PWM Channel 0 & 1 to use Clock SA
Set period to 100 Hz
Set polarity to hi
Set initial duty cycle to 0%
Map to Ports U0 & U1
Enable PWM
Set Ports T3 & T4 to be outputs for direction
void DriveMotor_Drive(unsigned
char command): Sets the duty
cycle of the PWM signal to the left and right drive DC motors to make the robot
go forward, backward, stop, turn left, and turn right.
switch(command)
case FORWARD :
Set Right PWM duty = RightDutyCycle * period / 100
Set Right direction low
Set Left PWM duty = LeftDutyCycle * period / 100
Set Left direction low
case BACKWARD :
Set Right PWM duty = (100-RightDutyCycle)* period / 100
Set Right direction hi
Set Left PWM duty = (100-LeftDutyCycle)* period / 100
Set Left direction hi
case LEFT :
Set Right PWM duty = (100-RightDutyCycle)* period / 100
Set Right direction hi
Set Left PWM duty = LeftDutyCycle * period / 100
Set Left direction low
case RIGHT :
Set Right PWM duty = RightDutyCycle * period / 100
Set Right direction low
Set Left PWM duty = (100-LeftDutyCycle)* period / 100
Set Left direction hi
case STOP
Set Right PWM duty = 0
Set Right direction low
Set Left PWM duty = 0
Set Left direction low
default :
print “DriveMotor_Drive error: Invalid drive command. Stopping motors."
Set Right PWM duty = 0
Set Right direction low
Set Left PWM duty = 0
Set Left direction low
unsigned char DriveMotor_GetDuty(unsigned
char direction): Returns the on
duty cycle value of the left or right drive motor
switch(direction)
case 1:
return LeftDutyCycle
case 2:
return RightDutyCycle
void DriveMotor_SetLeftDuty(unsigned
char dutyCycle): Sets the duty cycle value of the left drive motor to the value passed
in
LeftDutyCycle = dutyCycle
void DriveMotor_SetRightDuty(unsigned
char dutyCycle): Sets the rightduty cycle value of the left
drive motor to the value passed in
RightDutyCycle = dutyCycle
Ball
Requester Module: This module controls the drive motors of the robot in order to drive
forward into the Ball Dispenser for one second to depress a switch on it.
Module Functions
void BallRequester_GetBall(void):
Sequence that controls the drive motors so the bot
moves forward to hit the Ball dispenser, stops to keep it depressed and then
backs up to prepare to depress again
Drive robot backward
Wait 300ms
Stop
Wait 300 ms
Drive robot forward
Wait 400 ms
Stop robot driving
Wait 1100 ms
Drawbridge
Motor Module: This module controls the drawbridge
motor of the robot using Port U2 of the E128 board with PWM output to T5
Module Variabless
static unsigned char DutyCycle in %
Module Functions
void DrawbridgeMotor_Init(void): Initializes the E128 Port U clock to
control the PWM output to Port T5, the drawbridge motor
Set Clock B to 375 kHz
Set Clock SB to 46875 Hz
Set PWM Channel 2 to use Clock B
Set period to 2000 Hz
Set polarity to HI
Set initial duty cycle to 0%
Map to Ports U2
Enable PWM
Set Port T5 to be output for direction
void DrawbridgeMotor_Drive(unsigned
char command): Sets the PWM
signal to the DC motor to raise, lower, or pause the motion of the drawbridge
switch(command)
case PAUSE :
Set duty cycle = 0
Set direction lo
case RAISE :
Set duty cycle = (DutyCycle * period) / 100
Set direction lo
case LOWER :
Set duty cycle = ((100-DutyCycle) * period) / 100
Set direction hi
default :
print "DrawbridgeMotor_Drive error: Invalid drive command. Stopping motors"
Set duty cycle = 0
Set direction lo
void DrawbridgeMotor_Release(void):
Sequence that lowers the
drawbridge to its lower limit pause with it open for the balls to roll out, and
then starts to raise it.
Lower drawbridge
Wait 4300 ms
Pause drawbridge
Wait 500ms
Raise drawbridge
Game Timer Module: This module works with Timer 2 on an E128 board to set a flag after a 2 min time interval, the time limit for the game.
Module Variabless
typedef union
struct
unsigned int high;
unsigned int low;
AsInt;
unsigned long AsLong;
LongByInts;
static unsigned int TIM2_OC4_Period = 65535; // 0.35 s
static LongByInts CurrentTime;
static unsigned int Overflows;
static long MaxTime = 22500000; // 120 s
Module Functions
void GameTimer_Init(void): Initializes Timer 2 for game timer.
Enable TIM2
Enable TIM2 overflow interrupt
Set TIM_2 to 187.5 kHz tick rate
Clear TIM2 overflow counter and flag
Overflows = 0;
Set up TIM2_TC4 for output compare
Set TIM2_TC4 to be interrupt
Set TIM2_TC4 to output compare
Set no pin connect for TIM2_OC4
Set first compare for TIM2_OC4
Clear flag for TIM2_OC4
void interrupt _Vec_tim2ch4 GameTimer_UpdateTime(void): When timer expires (every .35 seconds), reset timer and the value of
how much time has passed.
If pending TIM2 overflow
Increment overflow counter
Clear TIM2 overflow flag
Update current time variable
Program next compare for TIM2_0C4 (.35 sec)
Clear TIM2_0C4 flag
void interrupt _Vec_tim2ovf GameTimer_CheckOverflows(void): Checks for timer overflows.
Increment overflow counter
Clear Timer 2 overflow flag
unsigned char GameTimer_TimeExpired(void): Check if the current elapsed time is >
the max time of the game (2 min) and if so, returns 1, else returns 0.
If(CurrentTime > MaxTime)
return 1;
else
return 0;
Wait Module: This module works with Timer 1 on an E128 board to wait for a specified time interval.
Module Variabless
static unsigned int TIM1_OC4_Period = 6000; // 1 ms
Module Functions
void Wait_Init(void): Initializes Timer 1 for wait function
Enable TIM1
Set TIM1 to 6 MHz tick rate
Set up TIM1_TC4
Set TIM1_TC4 to output compare
Set no pin connect for TIM1_OC4
void Wait(unsigned int time):
Waits for the specified period if time in ms.
unsigned int counter = 0
Initialize timer for 1ms
Clear timer 1 flag
while(counter
< time)
while(timer 1
flag not expired)
wait for 1 ms to expire
Set timer for
1ms again
Clear flag
counter++;
if GameTimer has expired
Stop Drawbridge motor
Stop Drive motors