Wall Avoiding Robot Code

#include <stdint.
h>
#include <stdbool.h>
#include <stdlib.h>
#include "inc/hw_memmap.h"
#include "inc/hw_types.h"
#include "driverlib/sysctl.h"
#include "driverlib/gpio.h"
#include "driverlib/pin_map.h"
#include "driverlib/pwm.h"
#include "driverlib/interrupt.h"
#include "inc/hw_ints.h"
#include "driverlib/timer.h"
volatile int Centimeters1 = 0, Centimeters2 = 0, Microseconds1, Microseconds2, I
nitCentimeters2;
volatile int CYCLE_PER_US, PERIOD, START_TIMER_VALUE, MAX_TIMER_VALUE, SENSOR_LI
MIT_TIMER_VALUE, ENDTIME1, ENDTIME2;
volatile uint8_t ECHO1_RECEIVED= 0, ECHO2_RECEIVED = 0;
volatile uint8_t REVERSE = 0; //Determines position of motor. '0' for FORWARD, '
1' for REVERSE
void TriggerPulse1()
{
//Send TRIGGER pulse
GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_2, 0xFF);
SysCtlDelay(10 * CYCLE_PER_US /3);
GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_2, 0x00);
//Reset Timer and enable interrupt
TimerLoadSet(TIMER0_BASE,TIMER_A,PERIOD-1); //Reset Timer value
TimerIntEnable(TIMER0_BASE, TIMER_TIMA_TIMEOUT); //Enable Timer interrup
t
TimerEnable(TIMER0_BASE,TIMER_A); //Start Timer
GPIOIntClear(GPIO_PORTD_BASE, GPIO_INT_PIN_1); //Clear GPIO interrupt be
fore enable
GPIOIntEnable(GPIO_PORTD_BASE, GPIO_INT_PIN_1); //Enable interrupt
while(ECHO1_RECEIVED == 0){} //Wait for pulse
}
void TriggerPulse2()
{
//Send TRIGGER2 pulse
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0xFF);
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0x00);
//Reset Timer and enable interrupt
TimerLoadSet(TIMER1_BASE,TIMER_A,PERIOD-1); //Reset Timer value
TimerIntEnable(TIMER1_BASE, TIMER_TIMA_TIMEOUT); //Enable Timer interrup
t
TimerEnable(TIMER1_BASE,TIMER_A); //Start Timer
GPIOIntClear(GPIO_PORTC_BASE, GPIO_INT_PIN_6); //Clear GPIO interrupt be
fore enable
GPIOIntEnable(GPIO_PORTC_BASE, GPIO_INT_PIN_6); //Enable interrupt
while(ECHO2_RECEIVED == 0){}//Wait for pulse
}
void TurnRight()
{
GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_1 , 0x02); //left
GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_6 , 0); //right
SysCtlDelay(SysCtlClockGet()/3*0.2);
GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_6 , 0x40); //right
GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_1 , 0); //left
}
void TurnRightMore()
{
}
void TurnLeft()
{
}
void TurnLeftMore()
{
}
void GoStraight()
{
}
int main (void)
{
float ResponseParameter = 1.3; //Determines range which motor will react
faster outside this parameter
float Tolerance = 1; //"Virtual track" width = Tolerance x2
double InnerBoundary = 0;
double OuterBoundary = 0;
double OffInnerBoundary = 0;
double OffOuterBoundary = 0;
SysCtlClockSet(SYSCTL_SYSDIV_1|SYSCTL_USE_OSC|SYSCTL_XTAL_16MHZ|SYSCTL_O
SC_MAIN);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD); //Enable GPIO Port D for Ul
trasonic Sensor 1
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC); //Enable GPIO Port C for Ul
trasonic Sensor 2
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB); //Enable GPIO Port B for mo
tor 1
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE); //Enable GPIO Port E for mo
tor 2
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA); //Enable GPIO Port A for PW
M
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1); //Enable PWM Module 1
SysCtlPWMClockSet(SYSCTL_PWMDIV_16);
IntMasterEnable(); //Enable processor interrupts
//Enable TRIGGER1 and ECHO1 pins
GPIOPinTypeGPIOInput(GPIO_PORTD_BASE, GPIO_PIN_1); //PORTD PIN 1 as ECHO
GPIOPinTypeGPIOOutput(GPIO_PORTD_BASE, GPIO_PIN_2); //PORTD PIN 2 as TRI
GGER
GPIOPinWrite(GPIO_PORTD_BASE, GPIO_PIN_2, 0);
//Set ECHO1 as interrupt
IntEnable(INT_GPIOD_BLIZZARD);
GPIOIntClear(GPIO_PORTD_BASE, GPIO_INT_PIN_1); //clear as general rule o
f thumb
GPIOIntTypeSet(GPIO_PORTD_BASE, GPIO_INT_PIN_1, GPIO_FALLING_EDGE); //wa
tch end of sensor delay
GPIOIntDisable(GPIO_PORTD_BASE, GPIO_INT_PIN_1);
//2nd Ultrasonic sensor
//Enable TRIGGER2 and ECHO2 pins
GPIOPinTypeGPIOInput(GPIO_PORTC_BASE, GPIO_PIN_6); //PORTc PIN 6 as ECHO
GPIOPinTypeGPIOOutput(GPIO_PORTC_BASE, GPIO_PIN_7 ); //PORTc PIN 7 as TR
IGGER
GPIOPinWrite(GPIO_PORTC_BASE, GPIO_PIN_7, 0);
//Set ECHO2 as interrupt
IntEnable(INT_GPIOC_BLIZZARD);
GPIOIntClear(GPIO_PORTC_BASE, GPIO_INT_PIN_6); //clear as general rule o
f thumb
GPIOIntTypeSet(GPIO_PORTC_BASE, GPIO_INT_PIN_6, GPIO_FALLING_EDGE); //wa
tch end of sensor delay
GPIOIntDisable(GPIO_PORTC_BASE, GPIO_INT_PIN_6);
//Calculate no. of cycle per microsecond based on 16MHz clock
CYCLE_PER_US = SysCtlClockGet()/1000000; //16 for 16MHz
//Calculate no. of cycle in one measurement interval (min. 50ms)
PERIOD = 50000 * CYCLE_PER_US; //800k
//Timer value when ECHO pulse starts with 450us after the trigger
START_TIMER_VALUE = PERIOD - (450 * CYCLE_PER_US); //792800
//Timer value when delay is more than 36ms (no ECHO due to sensor limit)
SENSOR_LIMIT_TIMER_VALUE = START_TIMER_VALUE - (36000 * CYCLE_PER_US); /
/216800
//Timer value when distance is maximum (350cm)
MAX_TIMER_VALUE = START_TIMER_VALUE - (350 * 58 * CYCLE_PER_US); //46800
0
//Enable Timer0 to schedule TRIGGER1 and measure ECHO1
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
TimerConfigure(TIMER0_BASE, TIMER_CFG_PERIODIC); //count down
TimerLoadSet(TIMER0_BASE,TIMER_A,PERIOD-1);
TimerDisable(TIMER0_BASE,TIMER_A);
//Setup interrupt for Timer0 timeouts
TimerIntClear(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
TimerIntDisable(TIMER0_BASE, TIMER_TIMA_TIMEOUT);
IntEnable(INT_TIMER0A);
//Enable Timer1 to schedule TRIGGER2 and measure ECHO2
SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER1);
TimerConfigure(TIMER1_BASE, TIMER_CFG_PERIODIC); //count down
TimerLoadSet(TIMER1_BASE,TIMER_A,PERIOD-1);
//Setup interrupt for Timer1 timeouts
TimerIntDisable(TIMER1_BASE, TIMER_TIMA_TIMEOUT);
IntEnable(INT_TIMER1A);
//Servo motor PWM
GPIOPinConfigure(GPIO_PA7_M1PWM3);
GPIOPinTypePWM(GPIO_PORTA_BASE, GPIO_PIN_7);
PWMGenConfigure(PWM1_BASE, PWM_GEN_1, PWM_GEN_MODE_UP_DOWN|PWM_GEN_MODE_
NO_SYNC);
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_1, 20000);
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, 350); //2250 and 500
PWMGenEnable(PWM1_BASE, PWM_GEN_1);
PWMOutputState(PWM1_BASE, PWM_OUT_3_BIT, true);
//Calibrate sensing distance from wall
TriggerPulse2();
InitCentimeters2 = Centimeters2;
InnerBoundary = InitCentimeters2 - Tolerance;
OuterBoundary = InitCentimeters2 + Tolerance;
//Configure ports for motor 1 & 2
GPIOPinTypeGPIOOutput(GPIO_PORTE_BASE, GPIO_PIN_1 | GPIO_PIN_2 );
GPIOPinTypeGPIOOutput(GPIO_PORTB_BASE, GPIO_PIN_6 );
while(1)
{
ECHO1_RECEIVED = 0;
ECHO2_RECEIVED = 0;
TriggerPulse1(); //Measure for front sensor
TriggerPulse2(); //Measure for side sensor
OffInnerBoundary = InnerBoundary - Centimeters2;
OffOuterBoundary = Centimeters2 - OuterBoundary;
if((Centimeters2 <= InnerBoundary) && (REVERSE == 0))
{
if(OffInnerBoundary <= ResponseParameter)
TurnRight();
else
TurnRightMore();
}
if((Centimeters2 >= OuterBoundary) && (REVERSE ==0))
{
//OffOuterBoundary = Centimeters2 - OuterBoundary;
if(OffOuterBoundary <= ResponseParameter)
TurnLeft();
if(OffOuterBoundary > ResponseParameter)
TurnLeftMore();
}
if((Centimeters2 <= InnerBoundary) && (REVERSE == 1))
{
//OffInnerBoundary = InnerBoundary - Centimeters2;
if(OffInnerBoundary <= ResponseParameter)
TurnLeft();
if(OffInnerBoundary > ResponseParameter)
TurnLeftMore();
}
if((Centimeters2 >= OuterBoundary) && (REVERSE == 1))
{
//OffOuterBoundary = Centimeters2 - OuterBoundary;
if(OffOuterBoundary <= ResponseParameter)
TurnRight();
if(OffOuterBoundary > ResponseParameter)
TurnRightMore();
}
if((Centimeters2 > InnerBoundary) && (Centimeters2 < OuterBounda
ry))
GoStraight();
}
}
void GPIOIntHandler1(void) //Measure pulse return time for front sensor
{
GPIOIntClear(GPIO_PORTD_BASE, GPIO_INT_PIN_1);
ENDTIME1 = TimerValueGet(TIMER0_BASE,TIMER_A);
}
void TimerIntHandler1(void) //Compute distance for front sensor
{
GPIOIntClear(GPIO_PORTD_BASE, GPIO_INT_PIN_1); //In case there i
s no ECHO1 signal
ECHO1_RECEIVED = 1;
if(ENDTIME1 <= MAX_TIMER_VALUE);
Centimeters1 = 350;
if(ENDTIME1 < SENSOR_LIMIT_TIMER_VALUE)
Centimeters1 = 350;
else
Microseconds1 = (START_TIMER_VALUE - ENDTIME1) / CYCLE_PER_US;
Centimeters1 = Microseconds1 / 58;
IntEnable(INT_GPIOA); //Trigger interrupt to detect obstacle
IntTrigger(INT_GPIOA);
}
void GPIOIntHandler2(void) //Measure pulse return time for side sensor
{
GPIOIntClear(GPIO_PORTC_BASE, GPIO_INT_PIN_6);
ENDTIME2 = TimerValueGet(TIMER1_BASE,TIMER_A);
}
void TimerIntHandler2(void) //Compute distance for side sensor
{
GPIOIntClear(GPIO_PORTC_BASE, GPIO_INT_PIN_6); //In case there i
s no ECHO2 signal
ECHO2_RECEIVED = 1;
if(ENDTIME2 <= MAX_TIMER_VALUE);
Centimeters2 = 350;
if(ENDTIME2 < SENSOR_LIMIT_TIMER_VALUE)
Centimeters2 = 350;
else
Microseconds2 = (START_TIMER_VALUE - ENDTIME2) / CYCLE_PER_US;
Centimeters2 = Microseconds2 / 58;
}
void Obstacle(void)
{
IntDisable(INT_GPIOA_BLIZZARD);
if((Centimeters1 < 35) && (REVERSE == 0))
{
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_3, 2100); //Turn servo 180 d
egrees
GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_6 , 0);
GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_1 , 0x02); //left wheel t
urn only
SysCtlDelay(SysCtlClockGet()/3*2.18); //U-turn for X number of s
econds
GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_6 , 0x40);
REVERSE = 1; //Set position as reverse
}
else if((Centimeters1 < 7) && (REVERSE == 1))
{
GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_1 , 0); //Stop both motor
s
GPIOPinWrite(GPIO_PORTB_BASE, GPIO_PIN_6 , 0);
GPIOPinWrite(GPIO_PORTE_BASE, GPIO_PIN_2 , 0x04); //Beep continu
ously
while(1){} //Enter infinite loop to END
}
}

Wall Avoiding Robot Code

Încărcat de

Informații document

Drepturi de autor

Formate disponibile

Partajați acest document

Partajați sau inserați document

Opțiuni de partajare

Vi se pare util acest document?

Este necorespunzător acest conținut?

Drepturi de autor:

Formate disponibile

Wall Avoiding Robot Code

Încărcat de

Drepturi de autor:

Formate disponibile

#include <stdint.

S-ar putea să vă placă și