#define PID_TARGET 2000

#define PID_P 0.05

#define PID_I 0
#define PID_D 0.015

bool counting
int timeCounter
char *out

=== Milestone 1: LED ===

uint16_t LEDstate
void init_LEDS(void)
-Enable peripheral GPIO for Port F
-Configure pins 1, 2, and 3 of Port F as output

void toggleRedLED(void)
-LEDstate -> ^(GPIO_PIN_1)
-Output LEDstate in PF1

=== Milestone 2 & 3: Command Interpreter & Bluetooth/UART ===

char command[8]
Struct CMD_T
begin
char CmdName[8];
void (*fcntPT)(void);
end

CMD_T Table[]
begin
{ "ledr" , &toggleRedLED },
{ "dd" , &distDisplay },
{ "ddcm" , &distDisplayCM},
{ "sm" , &stopMotor },
{ "mf" , &moveForward },
{ "mb" , &moveBackward },
{ "tr" , &turnRight },
{ "tl" , &turnLeft },
{ "a" , &motionON },
{ "s" , &motionOFF },
end

void init_UART()
Enable peripheral GPIO for Port B
 Enable peripheral UART1
Configure PB0 -> U1RX (receiver)
Configure PB1 -> U1TX (transmitter)
Configure PB0 and PB1
Configure PB0 and PB1 for use by the UART peripheral

Set UART configuration: UART1, UARTclk, Baud, Config

Configure UARTstdio for UARTpritntf and UARTgets functions
Enable UART1

void CommandInterpreter(void)
const int size -> Table size
int i -> 0
while is true
UART gets char input from the user and stores it in command[8]
for each i<size of the Table do
if the comparison between command and CmdName is true
then call the pointer function that corresponds to the i ID
break
end if
end for
if i is more than the size of the table (the command has not been found)
Display in UART "Invalid command! Try again\n"
end if
end while
end

=== Milestone 4: IR Sensors ===

uint32_t distF, distR

void init_ADC(void)
distR -> 0
distF -> 0

Enable peripheral ADC0

Enable peripheral ADC1
Enable peripheral GPIO for Port E

Configure pins 2 and 3 of Port E for ADC

Configures the trigger source and priority of the sample sequence for ADC0 (SS3)
Configure a step of the sample sequencer: ADC0 (SS3)
Enable sample sequence 3 for ADC0
 Enable the interrupt for sample sequence 3 of module ADC0

Configure ADC1 with sample sequencer 3, a processor trigger, and a priority of 0

Configure step 0 of the sample sequencer
Enable sample sequence 3 for ADC1
Enable the interrupt for sample sequence 3 of module ADC1
end

void hwiDistF(void)
Clear sample sequence 3 interrupt source of ADC0
Post software interrupt swiDistT
end

void hwiDistR(void)
Clear sample sequence 3 interrupt source of ADC1
Post software interrupt swiDistT
end

void swiDistTrigger(void)
Get the captured data of ADC0 SS3, and store it in variable distF
Get the captured data of ADC1 SS3, and store it in variable distR
end

void clkDistTrigger(void)
Cause a processor trigger on SS3 of ADC0
Cause a processor trigger on SS3 of ADC1
end

uint16_t Dist_GetF(void)
return distF
end

uint16_t Dist_GetR(void)
return distR
end

void distDisplay(void)
print Right and Left distances in ADC (12-bit)
end

void distDisplayCM(void)
float distF_CM -> 4543.5 * distF^(-0.827)
float distR_CM -> 4543.5 * distR^(-0.827)
 Create dynamic char array out
Convert the float right and left distances in CM into a string and store it out
Print out
end

=== Milestone 5: Motors/PWM ===

uint32_t loadPWM, clockPWM, motorDuty

void init_PWM(void)
Configure the rate of the clock provided to the PWM module
Enable peripheral PWM0

Enable peripheral GPIO for Port C

Configures PC6 as output for the left motor
Configures PC7 as output for the right motor

Write 0 into PC6

Write 0 into PC7

Configures pin PC4 for use by the PWM peripheral (used for left motor)
Configures pin PC5 for use by the PWM peripheral (used for right motor)

Configures the M0PWM6 function for PC4

Configures the M0PWM7 function for PC5

clockPWM -> Processor clock rate / 64;

loadPWM -> (clockPWM / 100) - 1;
motorDuty -> 10;

Configures PWM generator 3 for PWM0 with DOWN counting mode

Sets the period of PWM generator 3 to loadPWM.

Sets the pulse width for PWM 6 output (PC4) to motorDuty*loadPWM/100)

Sets the pulse width for PWM 7 output (PC5) to motorDuty*loadPWM/100)

Enables the timer/counter for PWM generator 3

end

void stopMotor(void)
Write 0 into PC6
Write 0 into PC7
Disables the PWM output for PC4
Disables the PWM output for PC5
end

void moveForward(void)
Write 0 into PC6 into PC6 to set forward direction in left motor
Write 0 into PC7 into PC7 to set forward direction in right motor
Enable the PWM output for PC4 (left motor)
Enable the PWM output for PC5 (right motor)
end

void moveBackward(void)
Write GPIO_PIN_6 into PC6 to set reverse direction in left motor
Write GPIO_PIN_7 into PC7 to set reverse direction in right motor
Enable the PWM output for PC4
Enable the PWM output for PC5
Enable the PWM output for PC4 (left motor)
Enable the PWM output for PC5 (right motor)
end

void turnLeft(void)
Write GPIO_PIN_6 into PC6 to set reverse direction in left motor
Write 0 into PC7 to set forward direction in right motor
Enable the PWM output for PC4 (left motor)
Enable the PWM output for PC5 (right motor)
end

void turnRight(void)
Write 0 into PC6 to set forward direction in left motor
Write GPIO_PIN_7 into PC7 to set reverse direction in right motor
Enable the PWM output for PC4 (left motor)
Enable the PWM output for PC5 (right motor)
end

void setLeftSpeed(uint32_t motorDutyL)

if motorDutyL > 90 then motorDutyL -> 90
if motorDutyL < 0 then motorDutyL -> 0
Sets the pulse width (motorDutyL*loadPWM/100) to M0PWM6 (PC4)
end

void setRightSpeed(uint32_t motorDutyR)

if motorDutyR > 90 then motorDutyR -> 90
if motorDutyR < 0 then motorDutyR -> 0
Sets the pulse width (motorDutyR*loadPWM/100) to M0PWM7 (PC5)
end
void delayS(float timeS)
Causes a delay in timeS seconds
end

=== Milestone 6 & 7: PID Control and Navigation ===

bool GO -> false
int32_t PIDvalue
uint32_t frontD
bool doTurnLeft -> false

int16_t errorPrev -> 0

int16_t errorCurr
int16_t sumPID -> 0
int16_t diffPID
uint16_t PIDspeed -> 100

void checkFrontWall(void)
frontD = Dist_GetF();

if frontD >= 3000

doTurnLeft -> true;
end if
end

void prepPID(void)
Clears timer interrupt for TIMER0
if GO is true
Post software interrupt swiPID (PID)
end if
else
Call stopMotor function
return
end else

void PID(void) //swiPID

if doTurnLeft is true then
frontD -> get front distance
Call turnLeft function
Call setLeftSpeed with parameter 10
Call setRightSpeed with parameter 90

if frontD <= 1000 then

Call moveForward function
doTurnLeft -> false
 end if
end if
else do
Call checkFrontWall function

float proportionalRight, integralRight, derivativeRight

errorCurr -> PID_TARGET- get right distance (distR)

proportionalRight -> PID_P * errorCurr;

sumPID -> sumPID + errorCurr

integralRight -> PID_I * sumPID;

diffPID -> errorCurr - errorPrev;

errorPrev -> errorCurr;
derivativeRight -> PID_D * diffPID;

PIDvalue -> proportionalRight + integralRight + derivativeRight

int16_t rightSpeed -> PIDspeed - PIDvalue;

int16_t leftSpeed -> PIDspeed - (rightSpeed - PIDspeed);

if rightSpeed < 0 then

rightSpeed -> 1
end if
else if rightSpeed > 100 then
rightSpeed -> 100;
end else if

if leftSpeed < 0 then

leftSpeed -> 1;
end if
else if leftSpeed > 100 then
leftSpeed -> 100;
end else if

if leftSpeed > rightSpeed then

If rightSpeed > 40 then
rightSpeed = rightSpeed - 40
else
rightSpeed=5;
end if
 else
if leftSpeed > 40 then
leftSpeed = leftSpeed - 40
else
leftSpeed=5;
end outer else

Call setLeftSpeed function with parameter leftSpeed

Call setRightSpeed function with parameter rightSpeed
end else
end

void motionON(void)
GO -> true
counting -> true
end

void motionOFF(void)
GO -> false
counting -> false
end

=== Milestone 8: Light Sensor ===

int blkLineCounter -> 0;
bool readData
bool sendData

void init_LightSensor()
Enable GPIO peripheral for Port F
Configure PF4 as output
readData-> false
sendData-> false
end

void LightSensorCalculation()
uint32_t lightSensorValue -> 0
uint32_t lightCounter -> 0

Configure PF4 as output

Write GPIO_PIN_4 into PF4 to output voltage to the reflectance sensor for charging

Give time to charge with a small delay

 Configure PF4 as input to read changes in voltage

while input PF4 is not 0 do

Increase lightCounter
end while
lightSensorValue -> lightCounter;

if lightSensorValue > 4000 then

Increase blkLineCounter
end if

else do

if blkLineCounter >= 40 and blkLineCounter <= 100 and readData == 0 then

Turn on Blue LED
Reset blkLineCounter to 0
GO -> true;
sendData -> true;
end if

else if blkLineCounter >= 40 and blkLineCounter <= 100 and readData == 1 then

Turn off LED

Reset blkLineCounter to 0
GO -> false;
sendData -> false;
end else if

else if blkLineCounter > 100 then

Turn Red LED

Reset blkLineCounter to 0;
Call motionOFF function to reset GO and counting flags
Call stopMotor to stop the robot motors
float totalTime -> (float)timeCounter/4
Store a string that displays totalTime in out
Print out in UART
end else if
end else

Reset lightCounter to 0
end
=== Milestone 9: Acquire Data & Send Data ===

struct MODBUSFRAME
begin
char colon
char addr
char data[20]
char checksum
char cr
char lf
end struct

Create MODBUSFRAME struct dataPing

Create MODBUSFRAME struct dataPong;
Create pointer to MODBUSFRAME struct dataCurr
int dataCount -> 0
int j

void init_PingPong()
Point dataCurr to dataPing

dataPing.colon = ':';
dataPing.addr = '5';
dataPing.checksum = '5';
dataPing.cr = '\r';
dataPing.lf = '\n';

dataPong.colon = ':';
dataPong.addr = '5';
dataPong.checksum = '5';
dataPong.cr = '\r';
dataPong.lf = '\n';
end

void PingPong(void)
Clears timer interrupt for TIMER0

if sendData is false then

return;
end if

int x -> absolute value of errorCurr

if dataCount < 20 then

 if (x > 255) then x -> 255;
dataCurr->data[dataCount] -> (char) x
Increment dataCount
end if

else do
Turn Green LED
Print dataCurr->colon
Print dataCurr->addr

for j=0; j<20; j++ do

If dataCurr->data[j] <= 16 then Print “0”
Print dataCurr->data[j]
end for

Print dataCurr->checksum
Print dataCurr->cr
Print dataCurr->lf

if dataCurr currently points to dataPong then

dataCurr now points to dataPing;
end if
else do
dataCurr now points to dataPong
end else

Reset dataCount to 0
end else
end

=== Milestone 10: StopWatch ===

void init_StopWatch()
counting -> false;
timeCounter -> 0
Create dynamic char array out
end

void StopWatch()
if counting is true then
timeCounter -> timeCounter + 1;
end if
end
=== Main ===
int main(void)
Call init_LEDS function
Call init_UART function
Call init_ADC function
Call init_PWM function
Call init_LightSensor function
Call init_PingPong function
Print “Bluetooth Connected” to verify that Bluetooth/UART works properly

Start the RTO’S scheduler

end