📄 intelligence_vehicle_ccd.c
字号:
#include "Cpu.h"
#include "Events.h"
#include "PWM8.h"
#include "Bit1.h"
#include "Bit2.h"
#include "AS1.h"
#include "Puls1.h"
#include "Bit3.h"
#include "PWM9.h"
#include "EInt1.h"
#include "AD2.h"
#include "Bit4.h"
#include "TI1.h"
/* Include shared modules, which are used for whole project */
#include "PE_Types.h"
#include "PE_Error.h"
#include "PE_Const.h"
#include "IO_Map.h"
#include "ccd.h"
#include "test_speed.h"
extern void fuzzy(void);
extern byte CURRENT_INS1,CURRENT_INS2,COG_OUT;
void main(void)
{
PE_low_level_init();
status=WAIT;
image_ready = FALSE;
first_int=TRUE; row_cur=0;
del_x_buf=27;
EInt1_Enable(); //允许外部中断
runing=FALSE;
speed=5;
measure_mode = MEASURE_SPEED;
Bit1_PutVal(1); // stop the motor , PA6=1
w=1500; //
v_set=125;
v_e1=0;
v_e2=0;
__EI(); //开中断
ut=80;
for(;;)
{
while(!image_ready);
if (status!=WAIT)
{
EInt1_Disable(); //关闭外部中断
if (status==SAMPLE)
{
stop();
send_data(); //传采样数据
}
if (status==STOP) stop(); //关闭电机
if (status==START) start(); //启动电机
if(status==SEND) send();
if(status==MEASURE_SPEED) measure_mode = MEASURE_SPEED;
if(status==MEASURE_DX) measure_mode = MEASURE_DX;
if(status==MEASURE_DL) measure_mode = MEASURE_DL;
if(status==MEASURE_DQ) measure_mode = MEASURE_DQ;
if(status==MEASURE_DR) measure_mode = MEASURE_DR;
status=WAIT;
image_ready = FALSE;
first_int=TRUE;
EInt1_Enable();
continue;
}
Bit4_PutVal(1);
control();
speed_ctrl();// AS1_SendChar((byte)v_cur);
Bit4_PutVal(0);
image_ready = FALSE;
}
}
外部中断服务程序：
void EInt1_OnInterrupt(void)
{
/* place your EInt1 interrupt procedure body here */
if (first_int)
{
first_int=FALSE;
row_started=FALSE;
pre_odd_even=Bit3_GetVal();
return;
}
else if(row_started)
{
if(row_x<3)
{
row_x +=1;
return;
}
row_x=0;
if (pre_odd_even==0)
{
ATD0CTL5 = 144;
for(col_cur=-COL_START;col_cur<COL;col_cur++) {
while(!ATD0STAT0_SCF); ATD0CTL5 = 144;
if (col_cur<0) continue;
data1[row_cur][col_cur]= ATD0DR0L; }
}
else
{
ATD0CTL5 = 144;
for(col_cur=-COL_START;col_cur<COL;col_cur++)
{
while(!ATD0STAT0_SCF);
ATD0CTL5 = 144;
if (col_cur<0) continue;
data2[row_cur][col_cur]= ATD0DR0L;
}
}
row_cur+=1;
if (row_cur==ROW)
{
odd_even_flag = pre_odd_even;
first_int=TRUE;
image_ready=TRUE;
ATD0CTL5 = 145;
while(!ATD0STAT0_SCF);
angle=ATD0DR0L;
return;
}
}
else if (pre_odd_even!=Bit3_GetVal())
{
row_started=TRUE;
pre_odd_even=Bit3_GetVal();
row_x=0;
row_cur=0;
}
}
控制算法程序：CCD.C
void control(void)
{
n=0;
if (odd_even_flag==0)
{
for(x=0;x<VALID_COL;x++)
{
rx=0;
nx=0;
for(y=ROW_START;y<VALID_ROW+ROW_START;y++)
{
if(data1[y][x]<LEVEL)
{
rx=rx+y;
nx +=1;
}
}
if(nx!=0)
{
dx[n]=rx/nx;
dl[n]=x+1;
n+=1;
}
}
}
else
{
for(x=0;x<VALID_COL;x++)
{
rx=0;
nx=0;
for(y=ROW_START;y<VALID_ROW+ROW_START;y++)
{
if(data2[y][x]<LEVEL)
{
rx=rx+y;
nx +=1;
}
}
if(nx!=0)
{
dx[n]=rx/nx;
dl[n]=x+1;
n+=1;
}
}
}
if (n==0) return;
del_x=dx[0]-ROW_START+1;
v_cur=Puls1_GetHoldValue();
CURRENT_INS1=(byte)del_x;
CURRENT_INS2=(byte)(del_x+VALID_ROW-del_x_buf);
fuzzy();
wx=COG_OUT-128;
del_x_buf=del_x;
CURRENT_INS12=(byte)dl[0];
CURRENT_INS22=(byte)(dl[0]+VALID_COL-del_l_buf);
fuzzy2();
wl=COG_OUT2;
if (dx[0]<MID) wl=-wl;
del_l_buf=dl[0];
wr=30*(dx[0]-dx[n/2]*2+dx[n-1])/(dl[0]-dl[n/2])/5;
w=1500+wx+wl+wr;
if (w<1150) w=1150;
if(w>1850) w=1850;
PWM9_SetDutyUS(w);
if(measure==ON && measure_mode!=MEASURE_SPEED )
{
switch (measure_mode)
{
case MEASURE_DX: speed_data[num]=del_x; break;
case MEASURE_DL: speed_data[num]=dl[n-1]-dl[0]; break;
case MEASURE_DQ: speed_data[num]=del_l+300; break;
case MEASURE_DR: speed_data[num]=del_r+200; break;
}
num++;
if(num==1000)
{
measure=OFF;
}
}
}
模糊控制算法一： Fuzzy.asm
XDEF fuzzy
XDEF CURRENT_INS1
XDEF CURRENT_INS2
XDEF COG_OUT
INCLUDE 'mc9s12dg128.inc'
MY_EXTENDED_RAM: SECTION
FUZ_INS DS.B 10
FUZ_OUT DS.B 7
CURRENT_INS1 DS.B 1
CURRENT_INS2 DS.B 1
COG_OUT DS.B 1
MyCode: SECTION
INPUT_MFS DC.B 0,25,255,36
DC.B 15,31,51,42
DC.B 28,37,64,64
DC.B 34,50,42,51
DC.B 40,65,36,255
DC.B 0,48,255,45
DC.B 38,63,25,36
DC.B 60,68,64,64
DC.B 65,90,36,25
DC.B 80,128,45,255
RULE_START DC.B 0,5,$FE,10,$FE
DC.B 0,6,$FE,10,$FE
DC.B 0,7,$FE,11,$FE
DC.B 0,8,$FE,12,$FE
DC.B 0,9,$FE,13,$FE
DC.B 1,5,$FE,10,$FE
DC.B 1,6,$FE,11,$FE
DC.B 1,7,$FE,12,$FE
DC.B 1,8,$FE,13,$FE
DC.B 1,9,$FE,14,$FE
DC.B 2,5,$FE,11,$FE
DC.B 2,6,$FE,12,$FE
DC.B 2,7,$FE,13,$FE
DC.B 2,8,$FE,14,$FE
DC.B 2,9,$FE,15,$FE
DC.B 3,5,$FE,12,$FE
DC.B 3,6,$FE,13,$FE
DC.B 3,7,$FE,14,$FE
DC.B 3,8,$FE,15,$FE
DC.B 3,9,$FE,16,$FE
DC.B 4,5,$FE,13,$FE
DC.B 4,6,$FE,14,$FE
DC.B 4,7,$FE,15,$FE
DC.B 4,8,$FE,16,$FE
DC.B 4,9,$FE,16,$FF
SGLTN_POS DC.B 0,43,121,128,135,213,255
fuzzy:
LDX #INPUT_MFS
LDY #FUZ_INS
LDAA CURRENT_INS1
LDAB #5
GRAD_LOOP MEM
DBNE B,GRAD_LOOP
LDAA CURRENT_INS2
LDAB #5
GRAD_LOOP1 MEM
DBNE B,GRAD_LOOP1
LDAB #7
RULE_EVAL CLR 1,Y+
DBNE b,RULE_EVAL
LDX #RULE_START
LDY #FUZ_INS
LDAA #$FF
REV
DEFUZ LDY #FUZ_OUT
LDX #SGLTN_POS
LDAB #7
WAV
EDIV
TFR Y,D
STAB COG_OUT
RTC
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -