Code PID điều khiển động cơ dùng LM629, help me!!!!

[123lazyboy]

Bác nào từng làm rồi xem đoạn code này giúp em với, không có mô phỏng nên em vừa viết code vừa test trên mạch, tình hình là vẫn chưa xuất được xung ra. Sau khi viết lệnh xong bit busy = 1 liên tục không xuống.Nó bị chỗ hàm LM629_CheckBusy() ngay sau hàm LM629_WriteComment(LTRJ), trong hàm LM629_LoadTRAJ(....) ak. Hix
Không biết là do phần cứng hay phần mềm nữa. Ai biết vì sao chỉ em với.
yahoo em: tranducnguyen_ks@yahoo.com.vn

/************************************************** ***
This program was produced by the
CodeWizardAVR V2.03.9 Standard
Automatic Program Generator
© Copyright 1998-2008 Pavel Haiduc, HP InfoTech s.r.l.
HP InfoTech, Development Tools for Microcontrollers, C Compilers, In-System Programmers

Project :
Version :
Date : 7/6/2012
Author : Tran Nguyen
Company : CC
Comments:


Chip type : ATmega32
Program type : Application
AVR Core Clock frequency: 8.000000 MHz
Memory model : Small
External RAM size : 0
Data Stack size : 512
************************************************** ***/

#include <mega32.h>
#include <delay.h>
#include <stdlib.h>
#asm
.equ __lcd_port=0x15 ;PORTC
#endasm
#include <lcd.h>

#define clearbit(reg, pbit) (reg &= ~(1<<pbit)) //set ve 0 bit duoc chon trong PORT/thanh ghi
#define setbit(reg, pbit) (reg |= (1<<pbit))

//define the data port (A7:0)
#define LM629_DATA_O PORTA
#define LM629_DATA_I PINA
#define LM629_DATA_DDR DDRA

//define the control port (PS,RD,WR,CS,RST)
#define LM629_CTRL_O PORTD
#define LM629_CTRL_I PIND
#define LM629_CTRL_DDR DDRD

#define LM629_HI 2
#define LM629_CS 3
#define LM629_WR 4
#define LM629_RST 5
#define LM629_RD 6
#define LM629_PS 7

#define RESET 0x00 //Reset LM628
#define PORT8 0x05 //Select 8-Bit Output
#define DFH 0x02 //Define Home

#define SIP 0x03 //Set Index Position
#define LPEI 0x1B //Interrupt on Error
#define LPES 0x1A //Stop on Error
#define SBPA 0x20 //Set Breakpoint, Absolute
#define SBPR 0x21 //Set Breakpoint, Relative
#define MSKI 0x1C //Mask Interrupts
#define RSTI 0x1D //Reset Interrupts
#define LFIL 0x1E //Load Filter Parameters
#define UDF 0x04 //Update Filter
#define LTRJ 0x1F //Load Trajectory
#define STT 0x01 //Start Motion
#define RDSIGS 0x0C //Read Signals Register
#define RDIP 0x09 //Read Index Position
#define RDDP 0x08 //Read Desired Position
#define RDRP 0x0A //Read Real Position
#define RDDV 0x07 //Read Desired Velocity
#define RDRV 0x0B //Read Real Velocity
#define RDSUM 0x0D //Read Integration Sum

char status_byte =0;
unsigned char hightbyte,lowbyte;
unsigned int pidmode;
unsigned char *dis;
unsigned long VEL_VAL, ACC_VAL, POS_VAL;
unsigned int ACCHI,ACCLO,VELHI,VELLO,POSHI,POSLO;
//void LM629_Init();
//void LM629_Reset();
void LM629_CheckBusy(void);
unsigned char LM629_ReadStatus(void);
unsigned char LM629_ReadData(void);
void LM629_WriteData(unsigned char dt);
void LM629_WriteComment(unsigned char com);
void LM629_OUT_Set(void);
void LM629_IN_Set(void);
void LM629_ByteChangeData(unsigned int data);
void LM629_DataChangeByte(unsigned int data);
void LM629_LoadPID(unsigned int kp,unsigned int kd,unsigned int ki,unsigned int il);
void LM629_LoadTRAJ(unsigned int NLINE,unsigned int VEL_FIN,unsigned int ACCL,unsigned int FIN_POS);
void LM629_Star(void);


void LM629_OUT_Set(void)
{
LM629_CTRL_DDR |=(1<<LM629_HI)|(1<<LM629_CS)|(1<<LM629_WR)|(1<<LM 629_RST)|(1<<LM629_RD)|(1<<LM629_PS); //DDR=1 la xuat
LM629_CTRL_O=0xFF;
LM629_DATA_DDR=0xFF; //xuat du lieu
LM629_DATA_O=0x00;
}

void LM629_IN_Set(void)
{
LM629_CTRL_DDR |=(1<<LM629_HI)|(1<<LM629_CS)|(1<<LM629_WR)|(1<<LM 629_RST)|(1<<LM629_RD)|(1<<LM629_PS); //DDR=1 la xuat
LM629_CTRL_O =0xFF;
LM629_DATA_DDR=0x00; //nhan du lieu
LM629_DATA_I=0xFF;
}

void lcd_hienthiso(unsigned long n)
{
char t,trtr,chtr,tr,trng,chng,ng,trm,chu,dv;
t = n / 1000000000;
trtr = (n % 1000000000)/100000000;
chtr = (n % 100000000) /10000000;
tr = (n % 10000000) /1000000;
trng = (n % 1000000) /100000;
chng = (n % 100000) /10000;
ng = (n % 10000) /1000;
trm = (n % 1000) /100;
chu = (n % 100) /10;
dv = (n % 10);
lcd_putchar(0x30+t);
lcd_putchar(0x30+trtr);
lcd_putchar(0x30+chtr);
lcd_putchar(0x30+tr);
lcd_putchar(0x30+trng);
lcd_putchar(0x30+chng);
lcd_putchar(0x30+ng);
lcd_putchar(0x30+trm);
lcd_putchar(0x30+chu);
lcd_putchar(0x30+dv);
}

/************************
* Check the BUSY bit *
************************/
void LM629_CheckBusy(void) //OK
{
char ST;
ST=LM629_ReadStatus();
while(ST & 0x01)
{
ST=LM629_ReadStatus();
lcd_clear();
delay_ms(200);
lcd_gotoxy(0,0);
lcd_putsf("busy");
lcd_hienthiso(ST);
delay_ms(700);

} // bit busy = 1 la ban
}
/*
void LM629_CheckBusy(void) // bit busy = 1 la ban
{
char ST;
ST=LM629_ReadStatus();
if((ST & 0x01 )== 0x01)
{
delay_us(100);
}
}
*/
/*********************************
* Read status byte from LM629 *
*********************************/
unsigned char LM629_ReadStatus(void) //CS=0,RD=0
{
clearbit(LM629_CTRL_O, LM629_CS);
clearbit(LM629_CTRL_O, LM629_PS);
clearbit(LM629_CTRL_O, LM629_RD);
status_byte = LM629_DATA_I;
delay_us(10);
setbit(LM629_CTRL_O, LM629_RD);
setbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_PS);
return(status_byte);
}

/**************************** //CS=0
* Comment write to LM629 * //RD=1
****************************/
void LM629_WriteComment(unsigned char com) // com la so hex cua lenh tuong ung
{
lcd_gotoxy(0,0);
lcd_putsf("Writing COM...");
clearbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_RD); /////////////////////////add
clearbit(LM629_CTRL_O, LM629_PS);
clearbit(LM629_CTRL_O, LM629_WR);
LM629_DATA_O = com;
delay_us(1);
setbit(LM629_CTRL_O, LM629_WR);
setbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_PS);
delay_ms(500);
lcd_clear();
}

/***************************
* Data read from LM629 *
* one byte of pairs *
***************************/
unsigned char LM629_ReadData(void)
{
char tmp;
clearbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_PS);
clearbit(LM629_CTRL_O, LM629_RD);
tmp = LM629_DATA_I;
delay_us(1);
setbit(LM629_CTRL_O, LM629_RD);
setbit(LM629_CTRL_O, LM629_CS);
clearbit(LM629_CTRL_O, LM629_PS);
return(tmp);


/*delay_us(1);
clearbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_PS);
clearbit(LM629_CTRL_O, LM629_RD);
delay_us(1);
lowbyte = LM629_DATA_I;

setbit(LM629_CTRL_O, LM629_RD);
setbit(LM629_CTRL_O, LM629_CS);
clearbit(LM629_CTRL_O, LM629_PS); */

}


void LM629_ByteChangeData(unsigned int data)
{
data=hightbyte;
data= (data<<8)| lowbyte;
}

void LM629_DataChangeByte(unsigned int data)
{
lowbyte = data & 0xFF;
hightbyte = (data>>8) & 0xFF;
}

/*************************
* Data write to LM629 *
* one byte of pairs *
*************************/
void LM629_WriteData(unsigned char dt)
{
//LM629_DataChangeByte(dt);
//LM629_CheckBusy();
//LM629_OUT_Set();

clearbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_RD); ////////////
setbit(LM629_CTRL_O, LM629_PS);
clearbit(LM629_CTRL_O, LM629_WR);
LM629_DATA_O = dt;
delay_us(1);
setbit(LM629_CTRL_O, LM629_WR);
setbit(LM629_CTRL_O, LM629_CS);
clearbit(LM629_CTRL_O, LM629_PS);

/*delay_us(1);
clearbit(LM629_CTRL_O, LM629_CS);
setbit(LM629_CTRL_O, LM629_PS);
clearbit(LM629_CTRL_O, LM629_WR);
delay_us(1);
LM629_DATA_O = lowbyte;
setbit(LM629_CTRL_O, LM629_WR);
setbit(LM629_CTRL_O, LM629_CS);
clearbit(LM629_CTRL_O, LM629_PS);*/

}

void LM629_LoadPID(unsigned int kp,unsigned int kd,unsigned int ki,unsigned int il)
{
lcd_clear();
lcd_gotoxy(0,0);
lcd_putsf("Loading PID");
delay_ms(500);

LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteComment(LFIL);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData(0x01);
LM629_WriteData(0x0F);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((kp>>8) & 0x00FF);
LM629_WriteData(kp & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((kd>>8) & 0x00FF);
LM629_WriteData(kd & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((ki>>8) & 0x00FF);
LM629_WriteData(ki & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((il>>8) & 0x00FF);
LM629_WriteData(il & 0x00FF);

LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteComment(UDF);

lcd_clear();
lcd_gotoxy(0,0);
lcd_putsf("PID complete");
delay_ms(500);
/*LM629_WriteData(0xFF01);
LM629_WriteData(kp);
LM629_WriteData(ki);
LM629_WriteData(kd);
LM629_WriteData(il);
LM629_WriteComment(UDF);*/
}

void LM629_LoadTRAJ(unsigned int NLINE,unsigned int VEL_FIN,unsigned int ACCL,unsigned int FIN_POS) //traj la lenh set che do van toc or vi tri
{ // trong traj : bit acc vel pos set len 1
//NLINE = so xung en // bao gom ca lenh dung.
//VEL_FIN = rpm
//FIN_POS = revs
//ACCL = revs/s*s
POS_VAL = NLINE * 4 * FIN_POS;
lcd_gotoxy(0,0);
lcd_putsf("POS_VAL load: ");
lcd_gotoxy(0,1);
lcd_hienthiso(POS_VAL);
delay_ms(1500);
lcd_clear();
//VEL_VAL = NLINE * 4 * 512 / 1000000 * VEL_FIN /60 *65536; //512 la sample time chom theo bit traj control
VEL_VAL = NLINE * VEL_FIN *2.237;
lcd_gotoxy(0,0);
lcd_putsf("VEL_VAL load: ");
lcd_gotoxy(0,1);
lcd_hienthiso(VEL_VAL);
delay_ms(1500);
lcd_clear();
//ACC_VAL = NLINE * 4 * 512 * 512 / 1000000000000 * ACCL * 65536 ;
ACC_VAL = NLINE * ACCL *0.0687;
lcd_gotoxy(0,0);
lcd_putsf("ACC_VAL load: ");
lcd_gotoxy(0,1);
lcd_hienthiso(ACC_VAL);
delay_ms(1500);
lcd_clear();

ACCHI = (ACC_VAL >> 16)& 0x0000FFFF;
ACCLO = ACC_VAL & 0x0000FFFF;
VELHI = (VEL_VAL >> 16)& 0x0000FFFF;
VELLO = VEL_VAL & 0x0000FFFF;
POSHI = (POS_VAL >> 16)& 0x0000FFFF;
POSLO = POS_VAL & 0x0000FFFF;

lcd_gotoxy(0,0);
lcd_putsf("Loading TRAJ...");
delay_ms(500);

LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteComment(LTRJ);
delay_us(10);
LM629_IN_Set();
LM629_CheckBusy(); /////////////// sau lenh write command bit busy set lien tuc
LM629_OUT_Set();
LM629_WriteData(0x00); // chon che do TRAJ
LM629_WriteData(0x2A);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((ACCHI>>8) & 0x00FF);
LM629_WriteData(ACCHI & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((ACCLO>>8) & 0x00FF);
LM629_WriteData(ACCLO & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((VELHI>>8) & 0x00FF);
LM629_WriteData(VELHI & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((VELLO>>8) & 0x00FF);
LM629_WriteData(VELLO & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((POSHI>>8) & 0x00FF);
LM629_WriteData(POSHI & 0x00FF);
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteData((POSLO>>8) & 0x00FF);
LM629_WriteData(POSLO & 0x00FF);

lcd_clear();
lcd_gotoxy(0,0);
lcd_putsf("COMPLETE");
delay_ms(1000);

/*unsigned int acc1,acc2,vel1,vel2,pos1,pos2;
LM629_WriteComment(LTRJ);
LM629_WriteData(traj);

//LM629_WriteData(traj&0xFF20);
acc2=(acc>>16)&0x0000FFFF;
acc1= acc & 0x0000FFFF;
LM629_WriteData(acc2);
LM629_WriteData(acc1);
delay_us(1);

//LM629_WriteData(traj&0xFF08);
vel2=(vel>>16)&0x0000FFFF;
vel1=vel & 0x0000FFFF;
LM629_WriteData(vel2);
LM629_WriteData(vel1);
delay_us(1);

//LM629_WriteData(traj&0xFF02);
pos2=(pos>>16)&0x0000FFFF;
pos1=pos & 0x0000FFFF;
LM629_WriteData(pos2);
LM629_WriteData(pos1);
delay_us(1); */
}

void LM629_Start(void)
{
LM629_IN_Set();
LM629_CheckBusy();
LM629_OUT_Set();
LM629_WriteComment(STT);
lcd_clear();
lcd_gotoxy(0,0);
lcd_putsf("RUN MOTOR");
delay_ms(500);
}


void main(void)
{

PORTA=0x00;
DDRA=0x00;

PORTB=0x00;
DDRB=0x00;

PORTC=0x00;
DDRC=0xFF;

PORTD=0x00;
DDRD=0x00;

// LCD module initialization
lcd_init(16);
PORTC.3=1;

while (1)
{

/*
LM629_WriteComment(RDSIGS);
LM629_ReadData();
lcd_gotoxy(0,0);
lcd_hienthiso(hightbyte);
lcd_putsf(" ");
lcd_hienthiso(lowbyte);
delay_ms(3000);
*/
/*
LM629_LoadTRAJ(100,200,50,100);
LM629_LoadPID(10,5,5,1);
LM629_Start();
while(1); */
LM629_OUT_Set();
setbit(LM629_CTRL_O, LM629_RD);
clearbit(LM629_CTRL_O, LM629_CS);
LM629_DATA_O=0x0F;
while(1);
};
}

[sonthientao]

Bạn đã xử lý được chưa?

Bác nào từng làm rồi xem đoạn code này giúp em với, không có mô phỏng nên em vừa viết code vừa test trên mạch, tình hình là vẫn chưa xuất được xung ra. Sau khi viết lệnh xong bit busy = 1 liên tục không xuống.Nó bị chỗ hàm LM629_CheckBusy() ngay sau hàm LM629_WriteComment(LTRJ), trong hàm LM629_LoadTRAJ(....) ak. Hix
Không biết là do phần cứng hay phần mềm nữa. Ai biết vì sao chỉ em với.
yahoo em: tranducnguyen_ks@yahoo.com.vn

[ATTACH=CONFIG]48770[/ATTACH]

Hi Nguyên
Bạn đã xử lý được chưa?