|
Program for robot
ในการที่จะควบคุมให้นหุ่นยนต์สามารถเคลื่อนที่ได้นั้น เราจำเป็นจะต้องมีการเขียนโปรแกรมเพื่อที่จะทำให้หุ่นยนต์สามารถที่จะทำงานได้ตามคำสั่งของเรา ดังนั้นการที่จะทำให้หุ่นยนต์สามารถทำงานได้ดี การเขียนโปรแกรมเป็นส่วนสำคัญมากในการสั่งงานหุ่นยนต์
Code For TheRobot
#define starttimer T1CON.F0
#define startinterrupt PIE1.F0
#define sensorfrontleft PortD.F0
#define sensorfrontright PortD.F1
#define sensorbackleft PortD.F2
#define sensorbackright PortD.F3
#define motorleftforward PortB.F0
#define motorleftbackward PortB.F1
#define motorrightforward PortB.F2
#define motorrightbackward PortB.F3
#define NTIMER 3
#define TIME_TRACK 1
#define TIME_TURN 100
#define TIME_OF_SENSOR_TURN 2
#define START 0
#define TRACK 1
#define LEFT 2
#define RIGHT 3
#define turnleft 0
#define turnright 1
// 1 tick as 10 ms
int TmrEnb[NTIMER] = {1,0,1};
int TmrMod[NTIMER] = {1,1,1};
int TmrRel[NTIMER] = {TIME_TRACK,TIME_TURN,TIME_OF_SENSOR_TURN};
int TmrCnt[NTIMER] = {TIME_TRACK,TIME_TURN,TIME_OF_SENSOR_TURN};
int TmrRdy[NTIMER] = {0,0,0};
int TmrErr[NTIMER] = {0,0,0};
int turn[]={turnleft,turnright,turnright,turnright,turnleft,turnleft,turnright,turnright,turnright,turnright};
int state,n,n0;
char i;
void timer1_Init();
void line();
void left();
void right();
void motorleft(char forward,char backward);
void motorright(char forward,char backward);
void main()
{
timer1_Init();
TRISA = 0x00;
TRISB = 0x00;
TRISC = 0x00;
TRISD = 0xFF;
TRISE = 0x00;
starttimer = 1; // start timer
state = START;
n0=0;
n=0;
while (1)
{
startinterrupt = 1; // enable interrupt
switch(state)
{
case START:
startinterrupt=0; // disable interrupt
if (TmrRdy[0] == 1)
{
TmrRdy[0] = 0;
TMR1H = 0XFF;
TMR1L = 0XFF;
startinterrupt=1; // enable interrupt
line();
}
else if (TmrRdy[1] == 1)
{
TmrRdy[1] = 0;
TMR1H = 0XFF;
TMR1L = 0XFF;
startinterrupt=1; // enable interrupt
}
else if (TmrRdy[2] == 1)
{
TmrRdy[2] = 0;
TMR1H = 0XFF;
TMR1L = 0XFF;
startinterrupt=1; // enable interrupt
}
else
{
startinterrupt=1; // enable interrupt
}
if ((sensorbackleft==1)&&(sensorbackright==1))
{
motorleft(1,0);
motorright(1,0);
}
else
{
motorleft(0,0);
motorright(0,0);
state=TRACK;
}
break;
case TRACK:
startinterrupt=0; // disable interrupt
if (TmrRdy[0] == 1)
{
TmrRdy[0] = 0;
TMR1H = 0X27;
TMR1L = 0X10;
if(!((sensorbackleft==1)&&(sensorbackright==1)))
{
line();
}
PortD=PortD&0x0f;
startinterrupt=1; // enable interrupt
}
else if (TmrRdy[1] == 1)
{
TmrRdy[1] = 0;
TmrEnb[1] = 0;
TMR1H = 0X27;
TMR1L = 0X10;
startinterrupt=1; // enable interrupt
n++;
}
else if (TmrRdy[2] == 1)
{
TmrRdy[2] = 0;
TMR1H = 0X27;
TMR1L = 0X10;
if((sensorbackleft==1)&&(sensorbackright==1))
{
if(turn[n]==turnleft)
{
state = LEFT;
}
if(turn[n]==turnright)
{
state = RIGHT;
}
}
startinterrupt=1; // enable interrupt
}
else
{
startinterrupt=1; // enable interrupt
}
if (n0==1)
{
n0=0;
TmrEnb[1]=1;
}
break;
case LEFT:
startinterrupt=0; // disable interrupt
if (TmrRdy[0] == 1)
{
TmrRdy[0] = 0;
TMR1H = 0X27;
TMR1L = 0X10;
startinterrupt=1; // enable interrupt
left();
PortD=PortD&0x0f;
}
if ((sensorfrontleft==0)&&(sensorfrontright==0))
{
motorleft(0,0);
motorright(0,0);
state=TRACK;
n0=1;
}
else
{
startinterrupt=1; // enable interrupt
}
break;
case RIGHT:
startinterrupt=0; // disable interrupt
if (TmrRdy[0] == 1)
{
TmrRdy[0] = 0;
TMR1H = 0X27;
TMR1L = 0X10;
startinterrupt=1; // enable interrupt
right();
PortD=PortD&0x0f;
}
if ((sensorfrontleft==0)&&(sensorfrontright==0))
{
motorleft(0,0);
motorright(0,0);
state=TRACK;
n0=1;
}
else
{
startinterrupt=1; // enable interrupt
}
break;
case 4:
startinterrupt=0; // disable interrupt
if (TmrRdy[0] == 1)
{
TmrRdy[0] = 0;
TMR1H = 0X27;
TMR1L = 0X10;
startinterrupt=1; // enable interrupt
motorleftforward=motorleftbackward=motorrightforward=motorrightbackward=0;
PortD=PortD&0x0f;
}
if ((sensorfrontleft==0)&&(sensorfrontright==0))
{
motorleft(0,0);
motorright(0,0);
state=TRACK;
n0=1;
}
else
{
startinterrupt=1; // enable interrupt
}
break;
}
}
}
void line()
{
if((sensorfrontleft==0)&&(sensorfrontright==0))
{
motorleft(1,0);
motorright(1,0);
}
if ((sensorfrontleft==0)&&(sensorfrontright==1))
{
motorleft(1,0);
motorright(0,0);
}
if ((sensorfrontleft==1)&&(sensorfrontright==0))
{
motorleft(0,0);
motorright(1,0);
}
}
void left()
{
motorleft(0,0);
motorright(1,0);
}
void right()
{
motorleft(1,0);
motorright(0,0);
}
/*
Initial timer1 function
*/
void timer1_Init()
{
/*
CPU Clock 20 MHz Prescaler 4
use timer0 overflow
frequency = 20 MHz / 4 = 5 MHz
period = 5 MHz = 0.2 us
number of clock = 1 ms / 0.2 us = 5000 clock
5000 clock = 0x1388
intitial = 0xFFFF-0x1388 = 0xEC77
*/
//CONFIG1= 0b1110000011010011;
INTCON = 0xC0; // Enable GIE, Peripharal interrupt
startinterrupt = 1; // Enable Timer1 interrupt
starttimer = 1; // Start Timer1
T1CON.F1 = 0; // Select TMR1 Clock Source as Internal instruction cycle clock (FOSC/4)
T1CON.F4 = 0; // Set Timer1 Input Clock Prescale is 1:8
T1CON.F5 = 0;
ANSEL = 0; // Configure AN pins as digital I/O
ANSELH = 0;
OSCCON.OSTS = 0; // Select Internal oscillator is used for system clock
OSCCON.IRCF0=1; // Configure to use internal oscillator
OSCCON.IRCF1=1;
OSCCON.IRCF2=1;
OSCCON.SCS=1;
TMR1H = 0XFF;
TMR1L = 0XFF;
C1ON_BIT = 0; // Disable Comparators
C2ON_BIT = 0;
}
/*
Interrupt function
*/
void interrupt()
{
if(PIR1.F0)
{
int k;
PIR1.F0 = 0;
for(k=0; k {
if(TmrEnb[k] == 1)
{
TmrCnt[k]--;
if(TmrCnt[k] == 0)
{
if(TmrRdy[k] == 1)
{
TmrErr[k] = 0;
}
else
{
TmrRdy[k] = 1;
TmrCnt[k] = TmrRel[k];
if(TmrMod[k] == 0)
{
TmrEnb[k] = 0;
}
}
}
}
}
}
}
/*
Motor left function
*/
void motorleft(char forward,char backward)
{
motorleftforward=forward;
motorleftbackward=backward;
}
/*
Motor right function
*/
void motorright(char forward,char backward)
{
motorrightforward=forward;
motorrightbackward=backward;
}
|
ด้วยความปราถนาดีจาก