📄 rebuilt_for_l297_main.c
字号:
/******************************************************************************
PROJECT: control step_motor
MCU: dsPIC30F4011. Xtal = 12Mhz
Programmer: ATATIN
IDE: MPLAB 8.0
Complier: Microchip C30 v3.0
Description: control step_motor
Start: 11/12/2008
Describe: Nang cap' chay. noi. suy duong` cheo' + cung tron`
******************************************************************************/
#include "p30f4011.h"
#include "RS232.h"
#include "math.h"
//***************************************************************************//
// CONFIG SYSTEM
//***************************************************************************//
_FOSC(CSW_FSCM_OFF & XT); //Fail-Safe Clock Monitor Off, XT=12M, FCY=12M/4=3M
_FWDT(WDT_OFF); //WatchDog Off
_FBORPOR(MCLR_EN & PBOR_ON & BORV_27);
//MCLR Enable, Power on Reset On, Brown-out Reset On = 2.7V.
_FGS(CODE_PROT_OFF); //Code Protection Off
//***************************************************************************//
#define direction1 LATBbits.LATB0
#define clock1 LATBbits.LATB1
#define enable1 LATBbits.LATB2
#define direction2 LATDbits.LATD0
#define clock2 LATDbits.LATD1
#define enable2 LATBbits.LATB7
#define clock3 LATBbits.LATB3
//#define enable3 LATDbits.LATD2
//#define chieu1 direction1
//#define chieu2 direction2
#define thuan 1
#define nghich 0
#define chay 1
#define dung 0
//****************************************************************************//
//unsigned char nstep1=1; //vi. tri' buoc'
unsigned char chieu1=thuan;// quay thuan
unsigned int tuongdoi1=0; //toa. do. vi. tri' tuong doi'
unsigned int ss1=0;
unsigned int tuyetdoi1=0; //toa. do. vi. tri' tuyet. doi'
//****************************************************************************//
unsigned char nstep2=1; //vi. tri' buoc'
unsigned char chieu2=thuan;// quay thuan
unsigned int tuongdoi2=0; //toa. do. vi. tri' tuong doi'
unsigned int ss2=0;
unsigned int tuyetdoi2=0; //toa. do. vi. tri' tuyet. doi'
float deltax;
float deltay;
float tananfa;
float bankinh;
unsigned char runflag=0;
//*******************Define functions*****************************************//
void Init_Timer1();//OK
void Init_Timer2();//OK
void quay1();//OK
void quay2();
void quaytuongdoi1(unsigned int i);
void quaytuongdoi2(unsigned int i);
void quaytuyetdoi1(unsigned int i);
void quaytuyetdoi2(unsigned int i);
void tuongdoi(unsigned char i,unsigned int x,unsigned char j,unsigned int y);
void tuyetdoi(unsigned int x, unsigned int y);
void nstt_tuyetdoi(unsigned int x, unsigned int y);// ham noi suy tuyen tinh toa do tuyet doi
void nstt_tuongdoi(unsigned char chieux, unsigned int x,unsigned char chieuy, unsigned int y );// ham noi suy tuyen tinh toa do tuong doi
void nsct_tuyetdoi(unsigned int x, unsigned int y, unsigned int tamx, unsigned int tamy, unsigned char chieuquay ) ;
float khoangcach(unsigned int x, unsigned int y); // tinh khoang cach tu vi tri hien tai toi vi. tri' co' toa. do. [x,y]
void delay();
//***************************************************************************//
float khoangcach(unsigned int x, unsigned int y)
{
unsigned int temp1, temp2;
temp1=fabs(tuyetdoi1-x);
temp2=fabs(tuyetdoi2-y);
return(sqrt(temp1*temp1+temp2*temp2));
}
//***************************************************************************//
void nsct_tuyetdoi(unsigned int dich_x, unsigned int dich_y, unsigned int tamx, unsigned int tamy, unsigned char chieuquay )
/*x, y: toa do dich';
tamx, tamy: toa do ta^m cung tron` (tinh theo toa do tuyet doi so voi diem dau` cua cung tron`)
chieuquay: theo kim dong ho */
{
float temp; //so sanh voi bankinh
unsigned int delta_x;
unsigned int delta_y;
unsigned char gocphantu, gocphantu_dich; // xac dinh goc phan tu o vi. tri' hien. tai. va vi. tri' dich' den'
bankinh=khoangcach(tamx, tamy);
temp=bankinh;
//====================== xac dinh goc phan tu cua vi. tri' dich'
if((dich_x<tamx)&&(dich_y>=tamy)) //goc' phan tu thu I
{
gocphantu_dich=1;
}
else if((dich_x>=tamx)&&(dich_y>tamy)) //goc' phan tu thu II
{
gocphantu_dich=2;
}
else if((dich_x<=tamx)&&(dich_y<tamy)) //goc' phan tu thu III
{
gocphantu_dich=3;
}
else if((dich_x>tamx)&&(dich_y<=tamy)) //goc' phan tu thu IV
{
gocphantu_dich=4;
}
//====================== QUAY THUAN
do
{
if(chieuquay==thuan) // chieu quay thuan
{
//====================== xac dinh chieu quay cua dong. co* va gocphantu cua vi. tri' hien. tai.
if((tuyetdoi1<tamx)&&(tuyetdoi2>=tamy)) //goc' phan tu thu I
{
chieu1=thuan;
direction1=chieu1;
chieu2=thuan;
direction2=chieu2;
gocphantu=1;
}
else if((tuyetdoi1>=tamx)&&(tuyetdoi2>tamy)) //goc' phan tu thu II
{
chieu1=thuan;
direction1=chieu1;
chieu2=nghich;
direction2=chieu2;
gocphantu=2;
}
else if((tuyetdoi1<=tamx)&&(tuyetdoi2<tamy)) //goc' phan tu thu III
{
chieu1=nghich;
direction1=chieu1;
chieu2=thuan;
direction2=chieu2;
gocphantu=3;
}
else if((tuyetdoi1>tamx)&&(tuyetdoi2<=tamy)) //goc' phan tu thu IV
{
chieu1=nghich;
direction1=chieu1;
chieu2=nghich;
direction2=chieu2;
gocphantu=4;
}
//======================
if(temp<bankinh) // so sanh' voi' ban kinh' thuc. te'
{
if((gocphantu==1)||(gocphantu==4))
{
quaytuongdoi2(1);
while(_T2IE);
}
else if((gocphantu==2)||(gocphantu==3))
{
quaytuongdoi1(1);
while(_T1IE);
}
}
//==================
else if(temp>=bankinh) // so sanh' voi' ban kinh' thuc. te'
{
if((gocphantu==2)||(gocphantu==3))
{
quaytuongdoi2(1);
while(_T2IE);
}
else if((gocphantu==1)||(gocphantu==4))
{
quaytuongdoi1(1);
while(_T1IE);
}
}
//=================== QUAY NGHICH
}
else if(chieuquay==nghich)//chieu quay nghich
{
//====================== xac dinh chieu quay cua dong. co* va gocphantu cua vi. tri' hien. tai.
if((tuyetdoi1<tamx)&&(tuyetdoi2>=tamy)) //goc' phan tu thu I
{
chieu1=nghich;
direction1=chieu1;
chieu2=nghich;
direction2=chieu2;
gocphantu=1;
}
else if((tuyetdoi1>=tamx)&&(tuyetdoi2>tamy)) //goc' phan tu thu II
{
chieu1=nghich;
direction1=chieu1;
chieu2=thuan;
direction2=chieu2;
gocphantu=2;
}
else if((tuyetdoi1<=tamx)&&(tuyetdoi2<tamy)) //goc' phan tu thu III
{
chieu1=thuan;
direction1=chieu1;
chieu2=nghich;
direction2=chieu2;
gocphantu=3;
}
else if((tuyetdoi1>tamx)&&(tuyetdoi2<=tamy)) //goc' phan tu thu IV
{
chieu1=thuan;
direction1=chieu1;
chieu2=thuan;
direction2=chieu2;
gocphantu=4;
}
//======================
if(temp<bankinh) // so sanh' voi' ban kinh' thuc. te'
{
if((gocphantu==1)||(gocphantu==4))
{
quaytuongdoi1(1);
while(_T1IE);
}
else if((gocphantu==2)||(gocphantu==3))
{
quaytuongdoi2(1);
while(_T2IE);
}
}
//==================
else if(temp>=bankinh) // so sanh' voi' ban kinh' thuc. te'
{
if((gocphantu==1)||(gocphantu==4))
{
quaytuongdoi2(1);
while(_T2IE);
}
else if((gocphantu==2)||(gocphantu==3))
{
quaytuongdoi1(1);
while(_T1IE);
}
}
}
//***************************************
temp=khoangcach(tamx, tamy); //tinh' lai ban kinh thuc te'
} while((gocphantu!=gocphantu_dich)||((tuyetdoi1!=dich_x)&&(tuyetdoi2!=dich_y)));
//=======================
if(tuyetdoi1==dich_x) while(tuyetdoi2!=dich_y)
{
quaytuongdoi2(1);
while(_T2IE);
}
if(tuyetdoi2==dich_y) while(tuyetdoi1!=dich_x)
{
quaytuongdoi1(1);
while(_T1IE);
}
}
//***************************************************************************//
void nstt_tuyetdoi(unsigned int x, unsigned int y)// ham noi suy tuyen tinh
// x, y: tinh theo toa do tuyet doi
{
if(tuyetdoi1<x)
{
x=x-tuyetdoi1;
chieu1=thuan; //quay thuan
direction1=chieu1;
}
else if(tuyetdoi1>x)
{
x=tuyetdoi1-x;
chieu1=nghich;//quay nguoc
direction1=chieu1;
}
else x=0;
//==============================
if(tuyetdoi2<y)
{
y=y-tuyetdoi2;
chieu2=thuan; //quay thuan
direction2=chieu2;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -