자세한 의뢰는 ▶링크 클릭해주세요
평행주차 졸업작품(=졸작) PIC32MX460기반
Design Contest Report_06_mai_2011.doc
AutomaticTrafficMovementsControl.pdf
AutomaticTrafficMovementsControl.ppt
Project.egg▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼▼
#include <plib.h>
#include "standardtypes.h"
#include "configuration.h"
#ifndef OVERRIDE_CONFIG_BITS
#pragma config ICESEL = ICS_PGx2 // ICE/ICD Comm Channel Select
#pragma config BWP = OFF // Boot Flash Write Protect
#pragma config CP = OFF // Code Protect
#pragma config FNOSC = PRIPLL // Oscillator Selection
#pragma config FSOSCEN = OFF // Secondary Oscillator Enable
#pragma config IESO = OFF // Internal/External Switch-over
#pragma config POSCMOD = HS // Primary Oscillator
#pragma config OSCIOFNC = OFF // CLKO Enable
#pragma config FPBDIV = DIV_8 // Peripheral Clock divisor
#pragma config FCKSM = CSDCMD // Clock Switching & Fail Safe Clock Monitor
#pragma config WDTPS = PS1 // Watchdog Timer Postscale
#pragma config FWDTEN = OFF // Watchdog Timer
#pragma config FPLLIDIV = DIV_2 // PLL Input Divider
#pragma config FPLLMUL = MUL_16 // PLL Multiplier
#pragma config UPLLIDIV = DIV_2 // USB PLL Input Divider
#pragma config UPLLEN = OFF // USB PLL Enabled
#pragma config FPLLODIV = DIV_1 // PLL Output Divider
#pragma config PWP = OFF // Program Flash Write Protect
#pragma config DEBUG = OFF // Debugger Enable/Disable
#endif
/* ------------------------------------------------------------ */
/* Global Variables */
/* ------------------------------------------------------------ */
//***FLAGS***//
char done_parking;
char parck;
char object2;
char object1;
char right_flag=0;
char left_flag=0;
char btn_flag=0;
//*** Counters***//
char few_reads;
int i=0;
int tur=0;
int rot=0;
unsigned int rightSensorA_impulseCount=0;
//The Hall sensors on both motors provide electrical pulses.
unsigned int leftSensorA_impulseCount=0;
//all pulses are counted, and used to calculate distances
//★The permanent magnets placed on the rotating shaft will interact with the Hall sensor, and square wave will be generated.
unsigned int old_rightCount=0;
unsigned int old_leftCount=0;
//these variabiles are used for the motor syncronize algorithm
//***Distances***//
unsigned int dist=0; //will hold the value of the distance between two objects, in order to decide if the car fits or not
int dmin_1; //will hold the minimum distance between the car and the object
int dmin_2; //(this helps when the surface of the object is irregular)
int d; // "d"[cm]-> is the distance obtained with a math function : get_function()
int a; // "a" -> holds the value provided by the Analog_To_Digital_Converter
int get_distance(void) //transform the result from ADC in a value that represents centimeters
{
d=((9500/(3*a+80))*(5/3))-3; // d=[cm]
return d;
}
void Delay(int dly) //1ms if dly=1
{
TMR4=0;
T4CON=0x8070;
PR4=0xffff;
while(TMR4 < dly*16);
}
void angle(int ang) //used when steering
{ tur=0;
rot=2*(3*ang)/7.78;
while(tur<=rot);
}
void distance(int dis) //based on the wheel diameter, this function indicates the distance that the car has to make
{ tur=0;
rot=dis*2.69;
while(tur<=rot);
}
//*** Movement commands***//
void speed(int sp) //sets the PWM Duty Cycle.
{ //The range of sp is: 0...9999
OC2R=sp;
OC2RS=sp;
OC3R=sp;
OC3RS=sp;
}
void speed_and_direction(int x,int dir)
{
if(dir==1)
{
prtMtrLeftDirSet = (1<<bnMtrLeftDir); // Left Motor direction
prtMtrRightDirClr = (1<< bnMtrRightDir); //Right Motor direction
}else if(dir==0)
{
prtMtrLeftDirClr = (1<<bnMtrLeftDir); //Left Motor direction
prtMtrRightDirSet = (1<< bnMtrRightDir); //Right Motor direction
}
speed(x);
}
void go_forward(int dx)
{
// }
prtMtrLeftDirSet = (1<<bnMtrLeftDir); // }=> car goes forward (the DC motors are antiparallel)
prtMtrRightDirClr = (1<< bnMtrRightDir); // }
speed(5000);
right_flag=1;
distance(dx);
right_flag=0;
speed(0);
}
void go_back(int dy)
{
prtMtrLeftDirClr = (1<<bnMtrLeftDir);
prtMtrRightDirSet = (1<< bnMtrRightDir);
speed(5000);
right_flag=1;
distance(dy);
right_flag=0;
speed(0);
}
void stop(int brk) //used to slow down, until the car stops
{
for(i=brk;i>=0;i--)
speed_and_direction(i,1);
}
void go_right(int d_right)
{
// Left Motor direction
prtMtrLeftDirClr = (1<<bnMtrLeftDir);
//Right Motor direction
prtMtrRightDirSet = (1<<bnMtrRightDir);
OC2R=0;
OC2RS=0;
OC3R=5000;
OC3RS=5000;
right_flag=1;
angle(d_right);
right_flag=0;
speed(0);
}
void go_left(int d_left)
{
// Left Motor direction
prtMtrLeftDirClr = (1<<bnMtrLeftDir);
//Right Motor direction
prtMtrRightDirClr = (1<<bnMtrRightDir);
OC2R=5000;
OC2RS=5000;
OC3R=0;
OC3RS=0;
left_flag=1;
angle(d_left);
left_flag=0;
speed(0);
}
//*** Interrupt Service Routines***//
void __ISR(_INPUT_CAPTURE_3_VECTOR,ipl1) IC3_Interrupt_handler(void)
{
asm("ei");
rightSensorA_impulseCount++;
if(right_flag==1)
{
tur++;
}
mIC3ClearIntFlag();
}//end IC3 handler
void __ISR(_INPUT_CAPTURE_2_VECTOR,ipl1) IC2_Interrupt_handler(void)
{
asm("ei");
leftSensorA_impulseCount++;
if(left_flag==1)
{
tur++;
}
if(dist>0)
{
dist++;
}
if(dist >= 95 && btn_flag==1)
{
parck=fTrue;
}
if(dist >=140 && btn_flag==2)
{
parck=fTrue;
}
mIC2ClearIntFlag();
}//end IC2 handler
void __ISR(_TIMER_1_VECTOR, ipl1) Timer1_Interrupt_handler(void)
{
asm("ei");
if( right_flag==0 && left_flag==0 )
{
if( (leftSensorA_impulseCount-old_leftCount) < (rightSensorA_impulseCount-old_rightCount) )
{
OC3R-=50;
OC3RS-=50;
}
else if( (leftSensorA_impulseCount-old_leftCount) > (rightSensorA_impulseCount-old_rightCount) )
{
OC3R+=50;
OC3RS+=50;
}
}
old_rightCount=rightSensorA_impulseCount;
old_leftCount=leftSensorA_impulseCount;
mT1ClearIntFlag();
}//end T1 handler
//***All stars here***//
main(void)
{
//---INITIALIZATIONS---//
init_all();
parck=fFalse;
done_parking=fFalse;
object1=fFalse;
object2=fFalse;
a=0;
d=0;
dmin_1=0;
dmin_2=0;
// Enable multi-vector interrupts.
INTEnableSystemMultiVectoredInt();
initADC( 0xfffe);
TRISB=0x00ff;
IC2CON=0x8081;
IC3CON=0x8081;
mIC3ClearIntFlag();
mIC3SetIntPriority( 1);
mIC3IntEnable( 1);
mIC2ClearIntFlag();
mIC2SetIntPriority( 1);
mIC2IntEnable( 1);
mT1ClearIntFlag();
mT1SetIntPriority(1);
mT1IntEnable(1);
INTEnableInterrupts();
LATBbits.LATB10=0;
LATBbits.LATB11=0;
LATBbits.LATB12=0;
LATBbits.LATB13=0;
//starts searching for a parking space
while (1)
{
if(button1==1)
{
Delay(20);
if(button1==1)
{
btn_flag=1;
Delay(2000);
speed_and_direction(4000,1);
T1CON=(1<<15);
}
}
if(button2==1)
{
Delay(20);
if(button2==1)
{
btn_flag=2;
Delay(2000);
speed_and_direction(4000,1);
T1CON=(1<<15);
}
}
while( !done_parking && btn_flag )
{
a=readADC();
d=get_distance();
if(d>0 && d<=15)
{
dmin_1=d;
while(d>0 && d<=15)
{
LATBbits.LATB10=1;
if(d<dmin_1) dmin_1=d;
a=readADC();
d=get_distance();
}
object1=fTrue;
LATBbits.LATB11=1;
speed(2500);
dist=1;
}//end if
a=readADC();
a=readADC();
a=readADC();
d=get_distance();
if(d>0 && d<=15 && object1)
{
few_reads=3;
dmin_2=d;
while(few_reads>=0)
{
a=readADC();
d=get_distance();
if(dmin_2<d)dmin_2=d;
few_reads--;
}
LATBbits.LATB12=1;
object2=fTrue;
}
if (parck && object2)
{
dist=0;
parck=fFalse;
T1CON&=0x7fff;
speed(0);
Delay(500);
if(btn_flag==1)
{
go_back(9); //cm
Delay(500);
go_left(140);
Delay(500);
go_back(dmin_2);//cm
Delay(500);
go_right(170);
Delay(500);
go_forward(14);//cm
}else if(btn_flag==2)
{
go_back(4);
Delay(800);
go_left(70);
Delay(800);
go_back(1.41*dmin_2+16);//cm
Delay(800);
go_right(100);
Delay(800);
go_forward(20);//cm
}
done_parking=1;
}else if(object2)
{
dist=0;
object2=0;
dmin_1=dmin_2;
speed(4000);
}
}// end while(!oparcat)
}//end while(1);
}//end main
