Due to the relatively linear sequence of tasks needed to be accomplished by the robot, it was decided not to use a formal state machine architecture for the software. Each task the robot had to accomplish usually only had to be done once. Therefore, it made sense to use a purely procedural architecture with nested if statements.
The tape module contains low level functions for handling inputs from the tape sensors, ball sensor, and bump sensors.
Initialize Tape
Initialize the A/D port for 6 analog inputs (4 tape sensors and 2 parts of the ball sensor)
Initialize Buttons
Initialize the bump sensors by setting two pins to inputs
The rest of the module contains low level functions that return true or false (1 or 0) depending on certain conditions (i.e. right tape sensor over black tape, black ball in ball sorter, front bump sensor activated, etc.)
The motor module contains low level functions for controlling drive motors and auxiliary motors.
Initialize Motor
Initialize PWM
8 bit mode
left aligned
high polarity
set clock and scale/prescale
enable PWM for two pins
set MODRR for two pins
LeftMotorEnc / RightMotorEnc
Takes a speed and direction input
Stores the direction as a static module variable and sends the speed to the encoder module
PIDLeftMotor / PIDRightMotor
Is called by the encoder module, NOT by the user
Takes as input a requested duty cycle
Depending on direction of the motor, set the motor direction bit high or low
Depending on direction, the requested duty cycle may have to be inverted
Sets the duty cycle of the motor to the (possibly inverted) requested duty cycle
The rest of the module contains low level functions that control the duty cycle of the drive motors directly, without going through PID control, and low level functions that control the auxiliary motors. The door-lock motors (hopper and shooter) can be opened and closed. The sorter motor can be set to spin in both directions and set to stop.
OFF: Stay at the same position so that all the balls fall in to the hopper (goal 3 ramp)
ON: Sort yellow balls to shooter, black balls to hopper, also limit the shooter at 4 balls max
The sorter is implemented in a non blocking code fashion, it is then to be called frequently in main or by a time interrupt
The figure below describe the sorter when it is activated
The sorter has 4 states:
WAIT FOR BALL: sorter at rest, ready for a new ball
TURN CW: turning CW (deliver ball to shooter)
TURN CCW: turning CCW (deliver ball to hopper)
GO BACK: ball sorted, back to initial position
Pseudo Code
Module variable:
char SorterMode,flag=0
Enable Sorter
Set SorterMode=1
Disable Sorter
Clear SorterMode
Sorter Run
if SorterMode is 0
If flag =1 clear flag and return
else if sorter reaches the default position stop and raise the flag
if not turn CW
if SorterMode=1
Check events and follow the appropriate state stransition in figure above
-Report speed and mileage of each wheels
-Close the PID control loop
A WORD ABOUT CONTROL STRATEGY
Each execution (trigger by a 20MS output compare), the program check to see if the desired speed and current speed is far away from each other (difference > ramprate)
If they are, then the PID controller set a target speed (call rampRPM below) to be a certain number between current speed and desired speed.
This is to avoid the robot from underwent a too brutal change in speed.
Example, let's say at t=0, speed=0 and desiredspeed=100,ramprate=10,
at the t= 20MS, the controller set a Target speed at 10RPM
at the t= 40MS, the controller set a Target speed at 20RPM
at the t= 60MS, the controller set a Target speed at 30RPM
....
from t= 200MS, the controller set a Target speed at 100RPM
The rest of the control algorithm is the same as normal PID control
Pseudo Code
Module static variables: Mileage1 and 2, RPM1 and 2, RPM Desired 1 and 2 and PIDMode
Elementary functions
These functions are pretty straight forward, their names are also explicit: MileageLeft report Mileage of Left wheel, RPMLeft return the value of RPM1 ...
The Module also has a Enable and Disable Function for the main to choose if it wants PID or not.
PID Control
Every 20MS, check if PIDMode is activated if yes:
check current RPM,
check desired RPM,
determine an intermidiate target RPM if neccessary (avoid shock)
compute error, and sum error
compute requested PWM, do anti windup if necessary
