Scribd
Upload
28 views13 pages

Pseudo Code

The document outlines the pseudo code for a robotics project developed by Team 5, detailing the use of various pins for sensors and motors, along with the implementation of functionalities like LED control, command interpretation via UART, and PID control for navigation. It includes initialization functions for LEDs, UART, ADC, PWM, and light sensors, as well as motor control commands for movement and turning. Additionally, it features data acquisition and transmission protocols using a MODBUS frame structure.

Uploaded by

Ivan Torres
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
28 views13 pages

Pseudo Code

The document outlines the pseudo code for a robotics project developed by Team 5, detailing the use of various pins for sensors and motors, along with the implementation of functionalities like LED control, command interpretation via UART, and PID control for navigation. It includes initialization functions for LEDs, UART, ADC, PWM, and light sensors, as well as motor control commands for movement and turning. Additionally, it features data acquisition and transmission protocols using a MODBUS frame structure.

Uploaded by

Ivan Torres
Copyright
© © All Rights Reserved
We take content rights seriously. If you suspect this is your content, claim it here.
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd

/*

* ============PSEUDO CODE================
* ===============TEAM 5===================
* Members: Jorge Martinez, Luong Pham, Thien Tao
*
* Pins Used:
* PB0: UART RX -> BT TX
* PB1: UART TX -> BT RX
* PE2: AIN7 -> Right Sensor
* PE3: AIN6 -> Front Sensor
* PC4 (PWM6) -> Left Motor Enable
* PC5 (PWM7) -> Right Motor Enable
* PC6: -> Left Motor Phase
* PC7: -> Right Motor Phase
* PF4: -> Reflectancer Sensor
*/

#include all required driver and utilities libraries

#define PID_TARGET 2000


#define PID_P 0.05
#define PID_I 0
#define PID_D 0.015

// *** Variables ***


bool counting;
int timeCounter;
char *out;

// *** Milestone 1: LED *** //


uint16_t LEDstate;

void init_LEDS(void){
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|
GPIO_PIN_3);
}

void toggleRedLED(void) {
LEDstate ^= GPIO_PIN_1;
GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1, LEDstate);
}

//////////////////////////////////////////////////////////////////
// *** Milestone 2 & 3: Command Interpreter & Bluetooth/UART*** //
//////////////////////////////////////////////////////////////////
char command[8];
typedef struct {
char CmdName[8];
void (*fcntPT)(void);
} CMD_T;

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

void init_UART(){
//SysCtlClockSet(SYSCTL_SYSDIV_4 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN |
SYSCTL_XTAL_16MHZ);

SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_UART1);
GPIOPinConfigure(GPIO_PB0_U1RX);
GPIOPinConfigure(GPIO_PB1_U1TX);
GPIOPinTypeUART(GPIO_PORTB_BASE, GPIO_PIN_0 | GPIO_PIN_1);

UARTConfigSetExpClk(UART1_BASE, SysCtlClockGet(), 115200,


(UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE |
UART_CONFIG_PAR_NONE));

UARTStdioConfig(1, 115200, SysCtlClockGet()); //To use UARTstdio functions


(UARTprintf, UARTgets, etc)

UARTEnable(UART1_BASE);
}

void CommandInterpreter(void){ //Task


const int size = sizeof(Table);
int i = 0;
while (true) {
UARTgets(command, 8);
for (i = 0; i < size; i++) {
if(strcmp(command, Table[i].CmdName) == 0 && command[0] != NULL) {
(*Table[i].fcntPT)();
break;
}
}
if(i>=sizeof(Table))
UARTprintf("Invalid command! Try again\n");
}
}

////////////////////////////////////
// *** Milestone 4: IR Sensors*** //
////////////////////////////////////
uint32_t distF, distR;

void init_ADC(void){
distR = 0;
distF = 0;

SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC0);
SysCtlPeripheralEnable(SYSCTL_PERIPH_ADC1);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);

// Pin2 = front sensor , Pin3 = right sensor


GPIOPinTypeADC(GPIO_PORTE_BASE, GPIO_PIN_2 | GPIO_PIN_3);

// SS3 ADC0- to sample front sensor | priority = 0


ADCSequenceConfigure(ADC0_BASE, 3, ADC_TRIGGER_PROCESSOR,0);
ADCSequenceStepConfigure(ADC0_BASE, 3, 0, ADC_CTL_CH1|ADC_CTL_IE|
ADC_CTL_END); //PE2 = CH1
ADCSequenceEnable(ADC0_BASE, 3);
ADCIntEnable(ADC0_BASE, 3);

// SS3 ADC1- to sample right sensor | priority = 0


ADCSequenceConfigure(ADC1_BASE, 3, ADC_TRIGGER_PROCESSOR,0);
ADCSequenceStepConfigure(ADC1_BASE, 3, 0, ADC_CTL_CH0|ADC_CTL_IE|
ADC_CTL_END); //PE3 = CH0
ADCSequenceEnable(ADC1_BASE, 3);
ADCIntEnable(ADC1_BASE, 3);
}
void hwiDistF(void) {
ADCIntClear(ADC0_BASE, 3);
Swi_post(swiDistT);
}

void hwiDistR(void) {
ADCIntClear(ADC1_BASE, 3);
Swi_post(swiDistT);
}

void swiDistTrigger(void) {
ADCSequenceDataGet(ADC0_BASE, 3, &distF);
ADCSequenceDataGet(ADC1_BASE, 3, &distR);
}

void clkDistTrigger(void) {
ADCProcessorTrigger(ADC0_BASE, 3);
ADCProcessorTrigger(ADC1_BASE, 3);
}

uint16_t Dist_GetF(void) {
return distF;
}

uint16_t Dist_GetR(void) {
return distR;
}

void distDisplay(void){
UARTprintf("Right distance: %u Front Distance: %u", distR, distF);
}

void distDisplayCM(void) {
float distR_CM = 4543.5 * pow((float)distR,-0.827);
float distF_CM = 4543.5 * pow((float)distR,-0.827);

//SysCtlDelay(3000);
char *out = (char*) malloc(16*sizeof(char));
sprintf(out, "Right distance: %0.1f cm Front Distance: %0.1f cm", distR_CM,
distF_CM);
UARTprintf("%s", out);
}

/////////////////////////////////////
// *** Milestone 5: Motors/PWM *** //
/////////////////////////////////////
uint32_t loadPWM, clockPWM, motorDuty;

void init_PWM(void){
SysCtlPWMClockSet(SYSCTL_PWMDIV_64);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM0);

SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_6); //Left
GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_7); //Right
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_6, 0);
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0);

GPIOPinTypePWM(GPIO_PORTC_BASE, GPIO_PIN_4); //Left


GPIOPinTypePWM(GPIO_PORTC_BASE, GPIO_PIN_5); //Right
GPIOPinConfigure(GPIO_PC4_M0PWM6);
GPIOPinConfigure(GPIO_PC5_M0PWM7);

clockPWM = SysCtlClockGet() / 64;


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

PWMGenConfigure(PWM0_BASE, PWM_GEN_3, PWM_GEN_MODE_DOWN);


PWMGenPeriodSet(PWM0_BASE, PWM_GEN_3, loadPWM);

PWMPulseWidthSet(PWM0_BASE, PWM_OUT_6, motorDuty*loadPWM/100);


PWMPulseWidthSet(PWM0_BASE, PWM_OUT_7, motorDuty*loadPWM/100);

PWMGenEnable(PWM0_BASE, PWM_GEN_3);
}

void stopMotor(void){
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_6, 0);
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0);
PWMOutputState(PWM0_BASE, PWM_OUT_6_BIT, false);
PWMOutputState(PWM0_BASE, PWM_OUT_7_BIT, false);
}

void moveForward(void){
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_6, 0);
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0);
PWMOutputState(PWM0_BASE, PWM_OUT_6_BIT, true);
PWMOutputState(PWM0_BASE, PWM_OUT_7_BIT, true);
}

void moveBackward(void){
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_6, GPIO_PIN_6);
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, GPIO_PIN_7);
PWMOutputState(PWM0_BASE, PWM_OUT_6_BIT, true);
PWMOutputState(PWM0_BASE, PWM_OUT_7_BIT, true);
}

void turnLeft(void){
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_6, GPIO_PIN_6); //Left Forward
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0); //Right Backward
PWMOutputState(PWM0_BASE, PWM_OUT_6_BIT, true);
PWMOutputState(PWM0_BASE, PWM_OUT_7_BIT, true);
}

void turnRight(void){
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_6, 0); //Left Backward
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, GPIO_PIN_7); //Right Forward
PWMOutputState(PWM0_BASE, PWM_OUT_6_BIT, true);
PWMOutputState(PWM0_BASE, PWM_OUT_7_BIT, true);
}

void setLeftSpeed(uint32_t motorDutyL){


if (motorDutyL > 90) {motorDutyL = 90;}
if ((uint32_t*)motorDutyL < 0) {motorDutyL = 0;}
PWMPulseWidthSet(PWM0_BASE, PWM_OUT_6, motorDutyL*loadPWM/100);
}

void setRightSpeed(uint32_t motorDutyR){


if (motorDutyR > 90) {motorDutyR = 90;}
if ((uint32_t*)motorDutyR < 0) {motorDutyR = 0;}
PWMPulseWidthSet(PWM0_BASE, PWM_OUT_7, motorDutyR*loadPWM/100);
}

void delayS(float timeS){


SysCtlDelay(timeS*(SysCtlClockGet()/3));
}

/////////////////////////////////////////////////////////
// *** 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;
}
}

void prepPID(void){ //TimerFunc


TimerIntClear(TIMER0_BASE, TimerIntStatus(TIMER0_BASE, false));
if (GO) {
Swi_post(swiPID);
}
else
stopMotor();
return;
}

void PID(void) {
if (doTurnLeft) {

frontD = Dist_GetF();
GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3, 6);
// Turn left until distance sensor sees opening
turnLeft();
setLeftSpeed(10);
setRightSpeed(90);

if (frontD <= 1000) {


moveForward();
doTurnLeft = false;
}
}
else {
checkFrontWall();

float proportionalRight, integralRight, derivativeRight;

errorCurr = PID_TARGET- Dist_GetR();

proportionalRight = PID_P * errorCurr;

sumPID += errorCurr;
integralRight = PID_I * sumPID;

diffPID = errorCurr - errorPrev;


errorPrev = errorCurr;
derivativeRight = PID_D * diffPID;

PIDvalue = proportionalRight + integralRight + derivativeRight;


//UARTprintf("%i ", PIDvalue);

int16_t rightSpeed = PIDspeed - PIDvalue;


int16_t leftSpeed = PIDspeed - (rightSpeed - PIDspeed);

if (rightSpeed < 0){


rightSpeed = 1;
}
else if(rightSpeed > 100){
rightSpeed = 100;
}

if(leftSpeed < 0){


leftSpeed = 1;
}
else if(leftSpeed > 100){
leftSpeed = 100;
}

if(leftSpeed > rightSpeed){


if(rightSpeed > 40)
rightSpeed = rightSpeed - 40;
else
rightSpeed=5;
}

else{
if(leftSpeed > 40)
leftSpeed = leftSpeed - 40;
else
leftSpeed=5;
}

setLeftSpeed(leftSpeed);
setRightSpeed(rightSpeed);
}
}

void motionON(void){
GO = true;
counting = true;
}

void motionOFF(void){
GO = false;
counting = false;
}

///////////////////////////////////////
// *** Milestone 8: Light Sensor *** //
///////////////////////////////////////
int blkLineCounter=0;
bool readData;
bool sendData;

void init_LightSensor(){
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF); // Enable PortF Peripheral
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_4); // Make PortF Pin4
Output
readData= false;
sendData= false;
}

void LightSensorCalculation() {
uint32_t lightSensorValue = 0;
uint32_t lightCounter = 0;

GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, GPIO_PIN_4);

GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_4, GPIO_PIN_4);


SysCtlDelay(200); //give time to charge
GPIOPinTypeGPIOInput(GPIO_PORTF_BASE, GPIO_PIN_4);

while (GPIOPinRead(GPIO_PORTF_BASE, GPIO_PIN_4) != 0) {


lightCounter++;
}
lightSensorValue = lightCounter;

if (lightSensorValue > 4000) {

++blkLineCounter;
//UARTprintf("\n%i", blkLineCounter);
}

else {

if ((blkLineCounter >= 40) && (blkLineCounter <= 100) && (readData == 0)) {

GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,
4);
blkLineCounter = 0;
GO = true;
sendData = true;
}

else if ((blkLineCounter >= 40) && (blkLineCounter <= 100) && (readData == 1)) {

GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,
0);
blkLineCounter = 0;
GO = false;
sendData = false;
}

else if (blkLineCounter > 100) {

GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3,
2);
blkLineCounter = 0;
motionOFF();
stopMotor();
float totalTime = (float)timeCounter/4;
sprintf(out, "\r\n%0.2fs", totalTime);
UARTprintf(out);
}
}

lightCounter = 0;
}

///////////////////////////////////////////////////
// *** Milestone 9: Acquire Data & Send Data *** //
///////////////////////////////////////////////////
typedef struct {
char colon;
char addr;
char data[20];
char checksum;
char cr;
char lf;
} MODBUSFRAME;

MODBUSFRAME dataPing;
MODBUSFRAME dataPong;
MODBUSFRAME* dataCurr;
int dataCount = 0;
int j;

void init_PingPong(){
dataCurr = &dataPing;

[Link] = ':';
[Link] = '5';
[Link] = '5';
[Link] = '\r';
[Link] = '\n';

[Link] = ':';
[Link] = '5';
[Link] = '5';
[Link] = '\r';
[Link] = '\n';
}

void PingPong(void) {
TimerIntClear(TIMER1_BASE, TimerIntStatus(TIMER1_BASE, false));

if ((sendData==false) || (GO==false)) {
return;
}

int x = abs(errorCurr);

if (dataCount < 20) {


if (x > 255) x = 255;
dataCurr->data[dataCount] = (char) x;
dataCount++;

} else {
GPIOPinWrite(GPIO_PORTF_BASE, GPIO_PIN_3, 8);

UARTprintf("%c ", dataCurr->colon);


UARTprintf("%c ", dataCurr->addr);

for(j=0; j<20; j++){


if(dataCurr->data[j]<=16) UARTprintf("0");
UARTprintf("%X ", dataCurr->data[j]);
}

UARTprintf("%c ", dataCurr->checksum);


UARTprintf("%c", dataCurr->cr);
UARTprintf("%c", dataCurr->lf);

if (dataCurr == &dataPong) {
dataCurr = &dataPing;
}
else {
dataCurr = &dataPong;
}

dataCount = 0;
}
}

/////////////////////////////////////
// *** Milestone 10: StopWatch *** //
/////////////////////////////////////
//bool counting;
//int timeCounter;

void init_StopWatch() {
counting = false;
timeCounter = 0;
out = (char*)malloc(sizeof(char) * 16);
}

void StopWatch() {
if (counting) {
timeCounter = timeCounter + 1;
}
}

// *** Main ***


int main(void){
init_LEDS();
init_UART();
init_ADC();
init_PWM();
init_LightSensor();
init_PingPong();
UARTprintf("Bluetooth Connected\n");

BIOS_start();
}

You might also like