标题:
MC9S12XS128智能小车测试源程序
[打印本页]
作者:
lxw15385989805
时间:
2018-4-12 15:58
标题:
MC9S12XS128智能小车测试源程序
s128智能小车测试程序源码
0.jpg
(36.24 KB, 下载次数: 28)
下载附件
2018-4-12 23:56 上传
单片机源程序如下:
#include <hidef.h> /* common defines and macros */
#include <MC9S12XS128.h> /* derivative information */
#include "math.h"
#pragma LINK_INFO DERIVATIVE "mc9s12xs128"
/*********************DEFINE************************/
#define WHITE 50
#define BLACK 40
#define KD 0
#define EnableSCIReInt SCI0CR2 |= 0x20
#define DisableSCIReInt SCI0CR2 &= 0xDF
#define RTI_SWITCH_ON CRGINT_RTIE=1
#define RTI_SWITCH_OFF CRGINT_RTIE=0
#define IC_SWITCH_ON TSCR1_TEN=1
#define IC_SWITCH_OFF TSCR1_TEN=0
//-----------------define key---------------------//
#define KEY_RUN_OR_LCD PORTK_PK0
#define KEY_START_RUN PORTA_PA1
#define KEY_UP PORTA_PA2
#define KEY_DOWN PORTA_PA3
#define KEY_ENTER PORTA_PA4
//-----------------part of lcd---------------------//
#define LINE1 0
#define LINE2 1
#define LINE1_HEAD 0x80
#define LINE2_HEAD 0xC0
#define DATA_MODE 0x38 // 8 bit data transmission mode, double line
#define CLR 0x01
#define BUSY 0x80 // LCD Busy Tag
#define CURSOR_RESET 0x02
#define INPUTMODE_CUR_R 0x06
#define INPUTMODE_CUR_L 0x04
#define INPUTMODE_ALL_E 0x05
#define INPUTMODE_ALL_D 0x04
#define SCREEN_OPEN 0x0C
#define SCREEN_OPEN_CUR 0x02
#define SCREEN_OPEN_FLASH 0x01
#define SCSHIFT_CUR_L 0x10 // Screen or Cursor shift
#define SCSHIFT_CUR_R 0x14
#define SCSHIFT_SCR_L 0x18
#define SCSHIFT_SCR_R 0x1C
//-----------------define lcd pins--------------------//
#define RS PORTA_PA5 //Data Command Pin 1 data 0 command
#define RW PORTA_PA6 //Read Write Pin 1 read 0 write
#define EN PORTA_PA7 //LCD Enable Signal
#define LCD_DATA PTH
#define LCD_DATA_DIR DDRH
#define LCD_DIR_IN 0x00
#define LCD_DIR_OUT 0xff
/*********************END************************/
/***************HARDWARE INIT FUN DECLARE******************/
void uart_init(void);
extern void uart_PutChar(unsigned char);
unsigned char TERMIO_GetChar(void);
void printp( char * , ...);
void CPU_Init(void);
void IO_Init(void);
void SciTx(unsigned char text);
char SciRx(void);
void RTI_Init(void);
void IC_Init(void);
void OC7_Init(void);
void PTI0_Init(void);
void PWM_Init(void);
void AD_Init(void);
//-----------------part of lcd---------------------//
void LcdCommand(unsigned char command,unsigned char BusyC);
void LcdData(unsigned char temp,unsigned char BusyC);
void LcdInit(void);
unsigned char ReadStatus(void);
void GotoXY( unsigned char x, unsigned char y );
void Print(unsigned char *str);
void Lcd_Print_Str(unsigned char x,unsigned char y,char *s);
void Lcd_Print_Int( unsigned char x,unsigned char y,unsigned int data);
void Lcd_Print_Char( unsigned char x,unsigned char y,unsigned char data);
void Lcd_Print_uChar( unsigned char x,unsigned char y,char data);
/*********************END************************/
/***************SOFTWARE FUN DECLARE******************/
void light_flip(void);
void AD_DATA_HIGHT(void);
void AD_DATA_LOW(void);
void VOL_Normaliz(void);
void err_calculate_hight(void);
void err_calculate_low(void);
signed int PID(void);
void key_scan_deal(void);
void LCD_DISPLAY(void);
uint err_Std(void);
void check_line(void);
void LcdDelay(byte k);
void Dly_ms(int ms);
void delay(unsigned int ref);
/*********************END************************/
/*********************DEFINE VAR************************/
//---------------sensor data var-----------------//
uchar THRESHOLD_black=40;
uchar max_vol[9],min_vol[9]={255,255,255,255,255,255,255,255,255};
uchar WHITE_MIN=240,WHITE_MAX=250;
uchar percent[9],data[9],det_vol[9],flip_data[9];
schar err[100],err_e[3];
uchar L_times=130;
byte flip=0,max_num_p=0;
//---------------sensor_2 data var-----------------//
uchar data_2[5],THRESHOLD_2=200,flip_data_2[5],err_e2[3];
//-----------------speed var---------------------//
byte load_bit=0;
uchar pluse_1=0,pluse_2=0,get_speed=0,set_speed=30,now_speed=0;
uchar MAX_SPEED=30;
//-----------------check var-------------------- //
uint check_pluse=0;
uchar start_line=0,stop=0,stop_delay=0,slope_time=0,slope_timing=0;
byte check=0,check_bit=0,line_num=0,triangle_status=0,slope_bit=0,black_bit=0;
//------------------key&lcd var----------------------//
byte key_r_o_l=0,lcd_print=1;
uchar menu=1,menu_option=1,bits=0;
//----------------- calc var---------------------//
uchar KP=8,period_1=0,period_2=0;
char std_err[10];
int temp_std;
/*********************END************************/
void main()
{
DisableInterrupts;
CPU_Init();
IO_Init();
uart_init();
//EnableSCIReInt;
LcdInit();
PWM_Init();
AD_Init();
VOL_Normaliz();
Dly_ms(3000);
Dly_ms(2000);
RTI_Init();
IC_Init();
for(;;)
{
EnableInterrupts;
//err_calculate_hight();
key_scan_deal();
}
}
//--------------------------------------------------------------
// HARDWARE FUN START
//--------------------------------------------------------------
void CPU_Init()
{
CLKSEL=0X00;
PLLCTL_PLLON=1;
REFDV=0x01; //BUS_CLOCK = 40M
SYNR=0x04;
_asm(nop);
_asm(nop);
while(!(CRGFLG_LOCK==1));
CLKSEL_PLLSEL=0x01;
FCLKDIV=0x0F;//Flash Clock Divide Factor 40M/16=?M
}
void SciTx(unsigned char text)
{
unsigned char temp;
temp=SCI0SR1; /*clear flag*/
while (!(SCI0SR1&0x80)); /* wait for output buffer empty */
SCI0DRH=0;
SCI0DRL=text;
}
char SciRx(void)
{
unsigned char result,temp;
temp=SCI0SR1; /*clear flag*/
while(!(SCI0SR1&0x20));
result=SCI0DRL;
return result;
}
void RTI_Init()
{
RTICTL=0b10010011; //0010000=2*10^3,10010011=8*10^3=0.5ms,10001111=16*10^3=1ms,11000111=160x10^3=10ms ,11101111=1.6*10^6=100ms
//CRGINT_RTIE=1;
}
void IC_Init(){
DisableInterrupts;
TIOS_IOS0=0; //channel 0 input capture
TCTL4_EDG0B=1;
TCTL4_EDG0A=1;
TIE_C0I=1; //channel 0 interrupt enable
TSCR2=0x03; //TIMEclock = 40M / 8 =0.2us
TFLG1 = 0xFF;
TFLG2 = 0x80;
//TSCR1_TEN=1; //Timer enable
EnableInterrupts;
}
void OC7_Init()
{
TSCR2_TCRE=1;
TSCR2_PR=0;
TIOS_IOS7=1;
TC7=0x5dc0; //24000,1ms
TCTL1=0x00;
TIE_C7I=1;
TSCR1_TEN = 1;
}
/*
void PTI0_Init()
{
PITCFLMT=0x00;
PITCE_PCE0=1;
PITMUX_PMUX0=0;
PITMTLD0=0x01;
PITLD0=0x0001; //2x2x25ns=1us;
PITINTE_PINTE0=1;
PITCFLMT_PITE=1;
}
*/
void PWM_Init()
{
PWME=0xaa; //enable c_1,c_3,c_5,c_7
PWMPOL=0xaa;
PWMCAE=0X00;
PWMCTL=0xf0; //com 0&1 2&3 4&5
PWMPRCLK=0x00;
PWMCLK_PCLK1=1; //0&1---SA
PWMSCLA=0x04; //SA = 5M
PWMPER01=50000; //舵机: 5000000/50000=100Hz ;
PWMDTY01=7100;
PWMPER23=5000; //电机: 24000000/12000=2000Hz ;24000000/2400=10000Hz ;40000000/5000=8000Hz
PWMDTY23=2500;
PWMPER45=5000;
PWMDTY45=2500;
}
void AD_Init()
{
//[3:0]---WRAP
//ATD0CTL0=0x00;
//7---ETRIGSEL, [6:5]---Resolution Select, 4---Discharge, [3:0]---External Trigger Channel Select
ATD0CTL1=0x00;
//6---AFFC;5---ICLKSTP; [4:3]---External Trigger Edge/Polarity; 2---ETRIGE;
//1---ASCIE,Sequence Complete Interrupt Enable; 0---ACMPIE,Compare Interrupt Enable
ATD0CTL2=0x40;
//7---DJM,Data Justification; [6:3]---S8C,S4C,S2C,S1C,Sequence Length;2---FIFO;
//[1:0]---Background Debug Freeze Enable;
ATD0CTL3=0x98;
//[7:5]---SMP,Sample Time Select; [4:0]---PRS,fATDCLK=fBUS/2/(PRS + 1)
ATD0CTL4=0x01;
//6---SC5,Special Channel Conversion; 5---SCAN,Continuous Conversion Sequence Mode
//4---MULT,Multi-Channel Sample Mode; [3:0]---Analog Input Channel Select Code CD,CC,CB,CA
//ATD0CTL5=0x01;
//ATD0CMPE_CMPE0=1;
//ATD0CMPHT_CMPHT0=0; //>AD0
//ATD0DR0L=0x000f;
ATD0DIEN=0x00;
}
void IO_Init(){
// SENSOR
DDRB=0xff;
PORTB=0x00;
DDRA=0x01;
PORTA_PA0=0;
// KEY DEFINE
DDRK_DDRK0 = 0;
DDRA_DDRA1 = 0;
DDRA_DDRA2 = 0;
DDRA_DDRA3 = 0;
DDRA_DDRA4 = 0;
DDRK_DDRK1 = 0;
DDRK_DDRK2 = 0;
DDRK_DDRK3 = 0;
// LCD CONTORL
DDRA_DDRA5 = 1;
DDRA_DDRA6 = 1;
DDRA_DDRA7 = 1;
//LED
DDRK_DDRK4 = 1;
DDRK_DDRK5 = 1;
PORTK_PK4 = 0;
PORTK_PK5 = 0;
//sensor_2
DDRK_DDRK7 = 1;
PORTK_PK7 = 0;
}
void Dly_ms(int ms)
{
int ii,jj;
if (ms<1) ms=1;
for(ii=0;ii<ms;ii++)
for(jj=0;jj<2670;jj++); //busclk:16MHz--1ms
}
void delay(unsigned int ref){
uint kk;
for(kk=0;kk<ref;kk++){
asm nop;
}
}
/************************LCD drive start*************************/
void LcdInit(){
LCD_DATA_DIR = LCD_DIR_OUT;
LcdDelay(21);
LcdCommand(DATA_MODE,1); //set 8 bit data transmission mode
LcdDelay(7);
LcdCommand(DATA_MODE,1); //set 8 bit data transmission mode
LcdDelay(7);
LcdCommand(DATA_MODE,1); //set 8 bit data transmission mode
LcdCommand(CLR,0); //clear screen
LcdCommand(DATA_MODE,1); //set 8 bit data transmission mode
LcdCommand(INPUTMODE_CUR_R | INPUTMODE_ALL_D, 1); // cursor right, disable moving
LcdCommand(SCREEN_OPEN, 1); //open display (enable lcd display)
LcdCommand(LINE1_HEAD,1); //set lcd first display address
LcdCommand(CLR,1); //clear screen
}
unsigned char ReadStatus(void)
{
unsigned char cRtn;
LCD_DATA_DIR = LCD_DIR_IN;
RS = 0;
RW = 1;
EN = 0;
delay(500);
EN = 1;
while (LCD_DATA & BUSY); //Test Busy State
cRtn = LCD_DATA_DIR; // if Not save the port value, it should be change
LCD_DATA_DIR = LCD_DIR_OUT;
return(cRtn);
}
void LcdCommand(unsigned char command,unsigned char BusyC){
if (BusyC)
ReadStatus(); //Test it busy or not
LCD_DATA=command;
RS=0;
RW=0;
EN=0;
delay(500);
EN=1;
}
void LcdData(unsigned char dat,unsigned char BusyC)
{
if (BusyC)
ReadStatus(); //Test it busy or not
LCD_DATA=dat;
RS=1;
RW=0;
EN=0;
delay(500);
EN=1;
}
void LcdDelay(byte k){ //k=7=5ms
uint i, j;
for (i = 0; i < k; i++)
for(j = 0; j < 4780; j++);
}
void GotoXY( unsigned char x, unsigned char y ){
unsigned char address;
if (y == LINE1)
address = LINE1_HEAD + x;
else
address = LINE2_HEAD + x;
LcdCommand(address,1);
}
void Lcd_Print_Int( unsigned char x,unsigned char y,unsigned int data){
char i=0;
unsigned char a[5];
a[0]=(data/10000)%10+'0';
a[1]=(data/1000)%10+'0';
a[2]=(data/100)%10+'0';
a[3]=(data/10)%10+'0';
a[4]=data%10+'0';
for(i=0; i<5; i++)
{
GotoXY(i+x, y);
LcdData(a[i],1);
}
}
void Lcd_Print_Char( unsigned char x,unsigned char y,unsigned char data)
{
char j=0;
unsigned char a[3];
a[0]=(data/100)%10+'0';
a[1]=(data/10)%10+'0';
a[2]=data%10+'0';
for(j=0; j<3; j++)
{
GotoXY(j+x, y);
LcdData(a[j],1);
}
}
void Lcd_Print_uChar( unsigned char x,unsigned char y,char data)
{
char j=0,k=0;
unsigned char a[3];
if(data>=0){
a[0]=(data/100)%10+'0';
a[1]=(data/10)%10+'0';
a[2]=data%10+'0';
for(j=0; j<3; j++)
{
GotoXY(j+x, y);
LcdData(a[j],1);
}
}
else{
data=~data;
data++;
a[0]=(data/100)%10+'0';
a[1]=(data/10)%10+'0';
a[2]=data%10+'0';
Lcd_Print_Str(x,y,"-");
for(k=0; k<3; k++)
{
GotoXY(k+x+1, y);
LcdData(a[k],1);
}
}
}
void Lcd_Print_Str(unsigned char x,unsigned char y,char *s)
{
GotoXY(x,y); //set address
while(*s!='\0') // write character
{
LcdData(*s, 1);
s++;
}
}
/************************LCD drive end***************************/
//--------------------------------------------------------------
// HARDWARE FUN END
//--------------------------------------------------------------
//--------------------------------------------------------------
// STARTEGY FUN START
//--------------------------------------------------------------
void light_flip(){
ATD0CTL3=0xc8;
ATD0CTL5=0x10;
while(!ATD0STAT0_SCF);
flip_data[0]=ATD0DR0L;
flip_data[1]=ATD0DR1L;
flip_data[2]=ATD0DR2L;
flip_data[3]=ATD0DR3L;
flip_data[4]=ATD0DR4L;
flip_data[5]=ATD0DR5L;
flip_data[6]=ATD0DR6L;
flip_data[7]=ATD0DR7L;
flip_data[8]=ATD0DR8L;
}
void AD_DATA_HIGHT()
{
//light_flip();
/*
ATD0CTL3=0x98;
ATD0CTL5=0x13;
while(!ATD0STAT0_SCF);
flip_data[3]=ATD0DR0L;
flip_data[4]=ATD0DR1L;
flip_data[5]=ATD0DR2L;
*/
PORTB = 0x08|0x10|0x20;
delay(L_times);
ATD0CTL3=0x98;
ATD0CTL5=0x13;
while(!ATD0STAT0_SCF);
data[3]= ATD0DR0L;
data[4]= ATD0DR1L;
data[5]= ATD0DR2L;
PORTB = 0x00;
/*
ATD0CTL3=0x98;
ATD0CTL5=0x10;
while(!ATD0STAT0_SCF);
flip_data[0]=ATD0DR0L;
flip_data[1]=ATD0DR1L;
flip_data[2]=ATD0DR2L;
*/
PORTB = 0x01|0x02|0x04;
delay(L_times);
ATD0CTL3=0x98;
ATD0CTL5=0x10;
while(!ATD0STAT0_SCF);
data[0]= ATD0DR0L;
data[1]= ATD0DR1L;
data[2]= ATD0DR2L;
PORTB = 0x00;
/*
ATD0CTL3=0x98;
ATD0CTL5=0x16;
while(!ATD0STAT0_SCF);
flip_data[6]=ATD0DR0L;
flip_data[7]=ATD0DR1L;
flip_data[8]=ATD0DR2L;
*/
PORTB = 0x40|0x80;
PORTA_PA0=1;
delay(L_times);
ATD0CTL3=0x98;
ATD0CTL5=0x16;
while(!ATD0STAT0_SCF);
data[6]= ATD0DR0L;
data[7]= ATD0DR1L;
data[8]= ATD0DR2L;
PORTB = 0x00;
PORTA_PA0=0;
//printp("\n %D,%D,%D,%D,%D,%D,%D,%D,%D \n",flip_data[0],flip_data[1],flip_data[2],flip_data[3],flip_data[4],flip_data[5],flip_data[6],flip_data[7],flip_data[8]);
}
void AD_DATA_LOW(){
ATD0CTL3=0xa8;
ATD0CTL5=0x19;
while(!ATD0STAT0_SCF);
data_2[0]=ATD0DR0L;
data_2[1]=ATD0DR1L;
data_2[2]=ATD0DR2L;
data_2[3]=ATD0DR3L;
data_2[4]=ATD0DR4L;
}
//----------------------------压差归一------------------------------//
void VOL_Normaliz(){
uchar aa;
char show_vol=0;
if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==1) line_num=0;
else if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==0) line_num=6;
else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==1) line_num=7;
else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==0) line_num=8;
else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==1) line_num=9;
else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==0) line_num=10;
else if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==1) line_num=11;
else if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==0) line_num=12;
while(KEY_RUN_OR_LCD==1) {
LCD_DISPLAY();
}
key_r_o_l=0xff;
LcdCommand(CLR,1);
while(KEY_START_RUN ==1){
AD_DATA_HIGHT();
for(aa=0;aa<9;aa++){
if(data[aa]>max_vol[aa]) max_vol[aa]=data[aa];
if(data[aa]<min_vol[aa]) min_vol[aa]=data[aa];
if((max_vol[aa]-min_vol[aa])>det_vol[aa]){ //det_vol impossible zero
det_vol[aa]=max_vol[aa]-min_vol[aa];
}
}
if(lcd_print==1) {
if(KEY_DOWN==0)
{
Dly_ms(1);
if(KEY_DOWN==0){
show_vol++;
if(show_vol>10) show_vol=0;
while(!KEY_DOWN);
}
}
if(KEY_UP==0){
Dly_ms(1);
if(KEY_UP==0){
show_vol--;
if(show_vol<0) show_vol=10;
while(!KEY_UP);
}
}
if(show_vol==0){
Lcd_Print_Str(0,0,"data[0]-data[2]:");
Lcd_Print_Char(0,1,data[0]);
Lcd_Print_Char(6,1,data[1]);
Lcd_Print_Char(12,1,data[2]);
}
if(show_vol==1){
Lcd_Print_Str(0,0,"data[3]-data[5]:");
Lcd_Print_Char(0,1,data[3]);
Lcd_Print_Char(6,1,data[4]);
Lcd_Print_Char(12,1,data[5]);
}
if(show_vol==2){
Lcd_Print_Str(0,0,"data[6]-data[8]:");
Lcd_Print_Char(0,1,data[6]);
Lcd_Print_Char(6,1,data[7]);
Lcd_Print_Char(12,1,data[8]);
}
if(show_vol==3){
Lcd_Print_Str(0,0," det[0]-det[2]: ");
Lcd_Print_Char(0,1,det_vol[0]);
Lcd_Print_Char(6,1,det_vol[1]);
Lcd_Print_Char(12,1,det_vol[2]);
}
if(show_vol==4){
Lcd_Print_Str(0,0," det[3]-det[5]: ");
Lcd_Print_Char(0,1,det_vol[3]);
Lcd_Print_Char(6,1,det_vol[4]);
Lcd_Print_Char(12,1,det_vol[5]);
}
if(show_vol==5){
Lcd_Print_Str(0,0," det[6]-det[8]: ");
Lcd_Print_Char(0,1,det_vol[6]);
Lcd_Print_Char(6,1,det_vol[7]);
Lcd_Print_Char(12,1,det_vol[8]);
}
if(show_vol==6){
Lcd_Print_Str(0,0," flip[0]---[2]: ");
Lcd_Print_Char(0,1,flip_data[0]);
Lcd_Print_Char(6,1,flip_data[1]);
Lcd_Print_Char(12,1,flip_data[2]);
}
if(show_vol==7){
Lcd_Print_Str(0,0," flip[3]---[5]: ");
Lcd_Print_Char(0,1,flip_data[3]);
Lcd_Print_Char(6,1,flip_data[4]);
Lcd_Print_Char(12,1,flip_data[5]);
}
if(show_vol==8){
Lcd_Print_Str(0,0," flip[6]---[8]: ");
Lcd_Print_Char(0,1,flip_data[6]);
Lcd_Print_Char(6,1,flip_data[7]);
Lcd_Print_Char(12,1,flip_data[8]);
}
if(show_vol==9)
{
Lcd_Print_Str(0,0,"data_2[0--3]");
Lcd_Print_Char(0,1,data_2[0]);
Lcd_Print_Char(6,1,data_2[1]);
Lcd_Print_Char(12,1,data_2[2]);
}
if(show_vol==10)
{
Lcd_Print_Str(0,0,"data_2[4--5]");
Lcd_Print_Char(0,1,data_2[3]);
Lcd_Print_Char(6,1,data_2[4]);
}
}
if(lcd_print==2){
printp("\n %D %D %D %D %D %D %D %D %D \n",data[0],data[1],data[2],data[3],data[4],data[5],data[6],data[7],data[8]);
printp("\n det :%D %D %D %D %D %D %D %D %D \n",det_vol[0],det_vol[1],det_vol[2],det_vol[3],det_vol[4],det_vol[5],det_vol[6],det_vol[7],det_vol[8]);
}
}
}
void err_calculate_low(){
uchar jj,b_num=0;
AD_DATA_LOW();
for(jj=0;jj<5;jj++){
if(data_2[jj]<THRESHOLD_2) b_num++;
}
if(b_num==1||b_num==2)
{
if(data_2[0]<THRESHOLD_2&&data_2[1]<THRESHOLD_2) err_e2[0]=37;
else if(data_2[1]<THRESHOLD_2&&data_2[2]<THRESHOLD_2) err_e2[0]=15;
else if(data_2[2]<THRESHOLD_2&&data_2[3]<THRESHOLD_2) err_e2[0]=-15;
else if(data_2[3]<THRESHOLD_2&&data_2[4]<THRESHOLD_2) err_e2[0]=-37;
else if(data_2[2]<THRESHOLD_2) err_e2[0]=0;
else if(data_2[0]<THRESHOLD_2) err_e2[0]=45;
else if(data_2[1]<THRESHOLD_2) err_e2[0]=30;
else if(data_2[3]<THRESHOLD_2) err_e2[0]=-30;
else if(data_2[4]<THRESHOLD_2) err_e2[0]=-45;
err[0]=err_e2[1];
err_e2[1]=err_e2[0];
}
}
void err_calculate_hight()
{
byte black_l=0,black_r=8,slope_num=0,min_slope=0,black=0;
uchar ii=0,max_num=0,max_black=100,n=0;
schar det_spacing=0,spacing=0;
uchar up_edge=0,down_edge=0;
byte black_num=0,white_num=0,zero_num=0;
//flip=0;
AD_DATA_HIGHT();
for(ii=0;ii<9;ii++)
{
if(data[ii]<=min_vol[ii]) percent[ii]=1;
else if(data[ii]>=max_vol[ii]) percent[ii]=100;
else percent[ii]=(data[ii]-min_vol[ii])*100/det_vol[ii];
if(percent[ii]<max_black&&percent[ii]<=40)
{
max_black=percent[ii];
max_num=ii;
}
if(percent[ii]<=40) {
black_num++;
}
if(data[ii]-max_vol[ii]>25) slope_num++;
if(percent[ii]<=20) zero_num++;
if(percent[ii]>=50) white_num++;
}
for(ii=0;ii<9;ii++){
if(percent[ii]<40) {
black_l=ii;
break;
}
}
for(ii=8;ii>0;ii--){
if(percent[ii]<40){
black_r=ii;
break;
}
}
//----------------------flip----------------------//
if(black_r-black_l==0||black_r-black_l==1) {
flip=1; //黑线连续,个数规定,数据可用?
PORTK_PK5=1;
}
else if((black_r-black_l)==2&&(black_r+black_l)/2<40) {
flip=1;
PORTK_PK5=1;
}
else {
flip=0;
PORTK_PK5=0;
}
//------------------------slope------------------------//
//前排为上坡
if(check==0&&slope_num>=7&&slope_bit==0){
slope_bit=1; //连续检测到正常时清除标志
PORTK_PK7=1;
//PORTK_PK4=1;
KP=13;
check=0;
}
//------------------------start-line------------------------//
else if(slope_bit==0&&zero_num>=2&&black_num>=6&&check==0&&abs(err_e[0]-err_e[1])<=120){
slope_bit=0;
check=1;
PORTK_PK7 = 1; //打开下排
//PORTK_PK4=1;
}
//----------------------前排检测到跑道-------------------------//
if(flip==1&&abs(max_num-max_num_p)<=4)
{
if(slope_bit==1){ //连续地看到了数值
slope_time=1;
}
else if(slope_bit==0){
if(max_num==0) err_e[0]=120-30*percent[max_num]/(percent[max_num]+percent[max_num+1]);
else if(max_num==8) err_e[0]=-120+30*percent[max_num]/(percent[max_num]+percent[max_num-1]);
else
{
switch(max_num)
{
case 1:spacing=90; break;
case 2:spacing=60; break;
case 3:spacing=30; break;
case 4:spacing=0; break;
case 5:spacing=-30; break;
case 6:spacing=-60; break;
case 7:spacing=-90; break;
default:
break;
}
if(percent[max_num+1]==percent[max_num-1]) err_e[0]=spacing;
else if(percent[max_num+1]<percent[max_num-1])
{
err_e[0]=spacing-30*percent[max_num]/(percent[max_num+1]+percent[max_num]);
}
else if(percent[max_num+1]>percent[max_num-1])
{
err_e[0]=spacing+30*percent[max_num]/(percent[max_num]+percent[max_num-1]);
}
}
err[0]=err_e[0];
err_e[1]=err_e[0];
max_num_p=max_num;
}
}
else if(slope_bit==1){
err_calculate_low();
}
/*
if(slope_bit==0&&white_num==9&&flip==0&&abs(max_num-max_num_p)<=4){
if(err_e[0]>=50) {
err_e[0]=120;
err[0]=err_e[0];
}
else if(err_e[0]<=-50){
err_e[0]=-120;
err[0]=err_e[0];
}
}
*/
if(check==1)
{
AD_DATA_LOW();
for(ii=0;ii<5;ii++){
if(data_2[ii]<THRESHOLD_2) black++;
}
if(black>=3){
black_bit=1;
//check_pluse=0;
//check=0;
//PORTK_PK7=0;
}
}
}
//-------------------------------------------------------------------------//
signed int PID(){
uchar kk=0,low=0;
sint u;
if(err[0]<=45&&err[0]>=-45) {
KP=5;
u=7000+KP*err[0];
}
else if(err[0]>45){
KP=9;
u=6920+KP*err[0];
}
else if(err[0]<-45) {
KP=9;
u=7180+KP*err[0];
}
//u=7000+KP*err[0]+KD*(err[0]-2*err[1]+err[2]);
if(u>=8050) u=8050;
if(u<=6100) u=6100;
for(kk=100;kk>0;kk--) {
err[kk]=err[kk-1];
}
return(u);
}
void key_scan_deal(void){
/*
if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==0) line_num=6;
else if(PORTK_PK3==0&&PORTK_PK2==0&&PORTK_PK1==1) line_num=7;
else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==0) line_num=8;
else if(PORTK_PK3==0&&PORTK_PK2==1&&PORTK_PK1==1) line_num=9;
else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==0) line_num=10;
else if(PORTK_PK3==1&&PORTK_PK2==0&&PORTK_PK1==1) line_num=11;
else if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==0) line_num=12;
else if(PORTK_PK3==1&&PORTK_PK2==1&&PORTK_PK1==1) line_num=13;
*/
if(KEY_RUN_OR_LCD==0)
{
LcdDelay(14);
if(KEY_RUN_OR_LCD==0)
{
key_r_o_l=~key_r_o_l;
while(!KEY_RUN_OR_LCD);
}
}
if(key_r_o_l==0){
//DisableInterrupts;
RTI_SWITCH_OFF;
IC_SWITCH_OFF;
LCD_DISPLAY();
PWMDTY45=PWMDTY23;
}
else{
//EnableInterrupts;
LcdCommand(CLR,1);
RTI_SWITCH_ON;
IC_SWITCH_ON;
}
}
/*************************LCD show start************************/
void LCD_DISPLAY(void)
{
/************************KEY_DOWN*************************/
if(KEY_DOWN==0)
{
LcdDelay(7);
if(KEY_DOWN==0)
{
//控制主菜单向下 1---9
……………………
…………限于本文篇幅 余下代码请从51黑下载附件…………
复制代码
所有资料51hei提供下载:
7.15(2).rar
(583.2 KB, 下载次数: 14)
2018-4-12 15:58 上传
点击文件名下载附件
下载积分: 黑币 -5
欢迎光临 (http://www.51hei.com/bbs/)
Powered by Discuz! X3.1