#include "XSS_Main.h"
//AD 控制位
#define ADC1_EM0 10UL //ADC1_SE10 电感0
#define ADC1_EM1 11UL //ADC1_SE11 电感1
#define ADC1_EM2 12UL //ADC1_SE12 电感2
#define ADC1_EM3 13UL //ADC1_SE13 电感3
#define ADC1_EM4 14UL //ADC1_SE14 电感4
#define ADC0_GYRO1 12UL //ADC0_SE12 陀螺仪1
#define ADC0_GYRO2 13UL //ADC0_SE13 陀螺仪2
#define ADC_DISABLE 0X1FUL
uint8_t PWM_ENABLE_FLAG=1; //该变量只在PWM_ENABLE()和PWM_DISABLE()函数中调用
#define em_adc_size 10
volatile uint16_t em_adc[5][em_adc_size] = {0,0,0,0,0}; //电磁传感器adc直接采样的值
//volatile uint16_t em_adc_adjust[5][em_adc_size] = {0,0,0,0,0}; //电磁传感器adc直接采样的值的处理值
double em[5] = {0,0,0,0,0}; //处理后的5个电感数据
/****************************************************/
/********************修改以下的值********************/
volatile double em_adc_standard[5] = {11500,10500,11300,11000,12000};//要改的！！！！！！！！！！！
/*********************2013-12-05*********************/
/****************************************************/
#define gyro_adc_size 10
volatile uint16_t gyro_adc[gyro_adc_size] = {0}; //陀螺仪adc采样值
int32_t row,row_ave,row_vari; //5个电磁传感器定位值,0~5000，最中间2500,相邻电感1000,(是不是与传感器距离有关)
volatile uint16_t temp;
volatile int32_t speed_set;
int32_t er,d_er_ave,d_er,d_er_vari; //误差微分
uint32_t po_er;
//row储存值
#define row_save_size 99 // 储存个数
#define d_er_save_size 99 // 储存个数
volatile int32_t row_save[row_save_size+1] = {2500}; // 存储传感器的值
volatile int32_t d_er_save[d_er_save_size+1] = {2500};// 存储误差
uint16_t ROW_DELT=5;
uint16_t ROW_FLAG=0;
uint16_t XS_FLAG;
uint16_t DW_FLAG;
//舵机参数
/**********************调整以下的值*********************/
#define steer_left_adjust 700 //最左
#define steer_midd_adjust 600 //中间
#define steer_rigt_adjust 500 //最右
/***********************2013-12-05**********************/
//注意归一化的算法
#define steer_left_bound -1000 //归一化之后最左端
#define steer_rigt_bound 1000 //归一化之后最右端
volatile int16_t steer_set;
int32_t steer_gyh_abs_ave;
int32_t steer_gyh_ave;
//方向控制参数
/*********************通过实验调整*************************/
float P_DIRECTION_RATIO = 0.03; //方向控制p0.03
float I_DIRECTION_RATIO = 0.0; //方向控制i
float D_DIRECTION_RATIO = 0.05; //方向控制d0.032
/***********************未定日期***************************/
//every+0.02 0.015better
float p_5=0.015;
float p_10=0.025;
float p_15=0.045;
float p_20=0.058;
//float d=0.053;
/*float p_5=0.01;
float p_10=0.02;
float p_15=0.04;
float p_20=0.053;*///值从哪里来
float d=0.068; //
int32_t s_z1=400;//666 750
int32_t s_z2=400;//620 580
int32_t s_w1=385;//540 590
int32_t s_w2=385;//540 480
int32_t s_s1=360;//520 570
int32_t s_s2=360;//520 460
float xs_p=0.58;
float dw_p=0.7;
float P_SPEED_RATIO = 1 ;
/*********************/
/*****是不是得改啊*****/
uint16_t em_bound=4000; // 最大电感值，超过认为错误。减小会降低赛车适应性。
uint16_t em_out_of_bound=500; // 此时认为冲出赛道
/**********************/
uint16_t pwm_motor ;//电机输出pwm
volatile int32_t speed,speed_set=400; //origin 400
/*方向控制*/
uint16_t pwm_steer ,pwm_steer_gyh_abs,d_pwm_steer,steer_direction_old=600;//波pwm
int pwm_steer_gyh;
uint16_t pwm_steer_gyh_abs_save[50];
int32_t pwm_steer_gyh_save[50];
int steer_gyh_fangcha; //竟然是汉语拼音
#define speed_save_size 10
volatile int32_t speed_save[speed_save_size+1]={400};
volatile uint16_t pwm_motor_old=2800;
uint16_t adc0_head = 0;
uint16_t adc1_head = 0;
uint16_t pwm_steer_t=700;
int i_stop;
uint16_t CAR_STOP = 0;
uint16_t START_LINE = 0;
double start_line;
#define START_ON 1;
#define START_OFF 0;
int start=0;
uint32_t wotext=0;
/*----------------------------------------------------------------------------
void em_adc_fliter(void) //传感器ad采样信号滤波
*----------------------------------------------------------------------------*/
void em_adc_filter(void)
{
volatile int i_pos = 0;
int32_t fir_i,fir_head;
float em_adc_sum[5] = {0,0,0,0,0};
float fir[em_adc_size] = {0.2,0.15,0.15,0.1,0.1,0.1,0.05,0.05,0.05,0.05}; //每组传感器取用10组数据，根据所取顺序依次加权求em值
for(i_pos = 0;i_pos < 5;i_pos++)
{
for(fir_i = 0;fir_i<em_adc_size;fir_i++)
{
fir_head = adc1_head - fir_i;
if (fir_head < 0) fir_head += 10;
em_adc_sum[i_pos] += fir[fir_i] * em_adc[i_pos][fir_head];
}
// em[i_pos] = em_adc_sum[i_pos];
em[i_pos]=(double)(em_adc_sum[i_pos]/em_adc_standard[i_pos])*10000;
}
}
/*----------------------------------------------------------------------------
void row_filter(void) //计算信号row滤波
*----------------------------------------------------------------------------*/
void row_filter(void)
{
volatile int i_pos = 0;
int32_t row_sum=0,d_er_sum,row_vari_sum,d_er_vari_sum;
int32_t row_max=2500,row_min=2500;
if (row < -2000 ) row = -2000; //row值限幅
if (row > 7000 ) row = 7000;
if ((row - row_save[row_save_size]) > 280) //row增值限幅
row = row_save[row_save_size] + 280;
if ((row - row_save[row_save_size] + 280) < 0 )
row = row_save[row_save_size] - 280;
i_pos = 0;
row_sum=0;
while(i_pos < row_save_size)
{
row_save[i_pos] = row_save[i_pos+1];
row_max=row_max>row_save[i_pos]?row_max:row_save[i_pos]; //取最大值，同时往中间靠
row_min=row_max<row_save[i_pos]?row_min:row_save[i_pos]; //取最小值，同时往中间靠
row_sum+=row_save[i_pos];
i_pos++;
}
row_save[row_save_size] = row;
d_er=row_save[row_save_size]-row_save[row_save_size-1]; //取差值
row_ave=0.01*(row_sum+row); //row平均值
if(row_max>3500&&row_min<1500)
{
XS_FLAG=1;
}
else
{
XS_FLAG=0;
}
// if((row_max<2500&&row_min>2000)||(row_max<3000&&row_min>2500))
// {
// DW_FLAG=1;
// }
// else
// {
// DW_FLAG=0;
// }
i_pos=0;
d_er_sum=0;
row_vari_sum=0;
while(i_pos <row_save_size)
{
d_er_save[i_pos] = d_er_save[i_pos+1];
d_er_sum+=d_er_save[i_pos];//误差求和
row_vari_sum+=(row_save[i_pos]-row_ave)*(row_save[i_pos]-row_ave); //row的方差
i_pos++;
}
d_er_save[row_save_size]=d_er;
row_vari=0.01*row_vari_sum; //row方差
d_er_ave=0.01*(d_er_sum+d_er); //误差平均值
d_er_save[row_save_size]=d_er;
i_pos=0;
d_er_vari_sum=0;
while(i_pos <row_save_size+1)
{
d_er_vari_sum+=(d_er_save[i_pos]-d_er_ave)*(d_er_save[i_pos]-d_er_ave);
i_pos++;
}
d_er_vari=0.01*d_er_vari_sum; //误差方差
er=2500-row;
po_er=er>0?er:-er;
}
/*----------------------------------------------------------------------------
void get_position(void) //传感器赛道定位
*----------------------------------------------------------------------------*/
void get_row(void)
{
static int max = 0;
volatile int i_max;
//
max=0;
for(i_max=0;i_max<5;i_max++) //找出em[5]中最大的一个
if(em[i_max] > em[max])
max = i_max;
if (em[max] <= em_bound) //6000，这个数还是会使整个系统的适应性不够好，所以能取得大一点就尽量大。（本文取的是3558）
{
if(row<2500)
{
row=row_save[row_save_size]-280;
}
else if(row>2500)
{
row=row_save[row_save_size]+280;
}
ROW_FLAG=1;
if (em[max] <= em_out_of_bound)
{
PWM_ENABLE_FLAG=0;
}
}
else
{
if(max == 0)
{
row = 1000 + (int32_t)(10000*(em[1] - em[0])/(em[0] + em[1] + em[0]*em[1]/431));//这个算法的依据？2720
}
else if(max == 4)
{
row = 4000 + (int32_t)(10000*(em[4] - em[3])/(em[3] + em[4] + em[3]*em[4]/424));
}
else
{
if(em[max-1] > em[max+1])
row = max*1000 + 500.0*((em[max]-em[max-1]))/((em[max]-em[max+1])); //控制转向
else
row = (max+1)*1000 - 500.0*((em[max]-em[max+1]))/((em[max]-em[max-1])); //控制转向
}
}
row_filter();
}
float get_kp(int po_er) //kp自适应？
{
float kp_l;
if(po_er<500)
{
kp_l=p_5;
//D_DIRECTION_RATIO=0.032;
D_DIRECTION_RATIO=0.037;
}
else if(po_er<1000)
{
kp_l=p_5+0.002*(po_er-500)*(p_10-p_5);//算法的依据？
//D_DIRECTION_RATIO=0.032;
D_DIRECTION_RATIO=0.047;
}
else if(po_er<1500)
{
kp_l=p_10+0.002*(po_er-1000)*(p_15-p_10);//算法的依据？
//D_DIRECTION_RATIO=0.032;
D_DIRECTION_RATIO=0.057;
}
else if(po_er<2000)
{
kp_l=p_15+0.002*(po_er-1500)*(p_20-p_15);
D_DIRECTION_RATIO=d;
}
else
{
kp_l=p_20;
D_DIRECTION_RATIO=d;
}
// kp_l*=1+po_er*0.0001;
if(XS_FLAG==1)
{
kp_l*=xs_p;
}
// if(DW_FLAG==1)
// {
// kp_l*=dw_p;
// }
return kp_l;
}
//void get_KP_KD(void)
//{
// if(d_er_ave<10&&d_er_ave>-10&&po_er<1000)
// {
// P_DIRECTION_RATIO=0.02;
// D_DIRECTION_RATIO=0.02;
//
// }
// else
// {
// P_DIRECTION_RATIO=0.045;
// D_DIRECTION_RATIO=0.04;
// }
//}
/*----------------------------------------------------------------------------
void steer_guiyihua(void) //舵机归一化
#define steer_left_adjust = 800; //最左端
#define steer_midd_adjust = 860; //中间
#define steer_rigt_adjust = 915; //最右端
*--------------------------------------------------------*/
// void steer_guiyihua(void)
// {
// steer_midd = steer_midd_adjust;
// steer_left_adjust_x = (steer_left_adjust - steer_midd_adjust) / 1000;
// steer_rigt_adjust_x = (steer_rigt_adjust - steer_midd_adjust) / 1000;
// }
/*----------------------------------------------------------------------------
???ú1éò??ˉ??oóμ?pwm_steer?????μμ?±￡′?
*----------------------------------------------------------------------------*/
void steer_gyh_abs_save(void)
{
volatile int i_steer = 0,i_sum=0,i_fangcha;
int32_t steer_gyh_abs_sum_l=0;
int32_t steer_gyh_sum_l=0;
int32_t steer_gyh_abs_ave_l;
int32_t steer_gyh_ave_l;
int32_t steer_gyh_fangcha_l;
i_steer = 0;
while(i_steer < 49)
{
pwm_steer_gyh_abs_save[i_steer] = pwm_steer_gyh_abs_save[i_steer+1];
pwm_steer_gyh_save[i_steer]=pwm_steer_gyh_save[i_steer+1];
i_steer++;
}
pwm_steer_gyh_abs_save[49] =pwm_steer_gyh_abs;
pwm_steer_gyh_save[49]=pwm_steer_gyh;
steer_gyh_sum_l=0;
steer_gyh_abs_sum_l=0;
for(i_sum=40;i_sum<50;i_sum++)
{
steer_gyh_abs_sum_l+=pwm_steer_gyh_abs_save[i_sum];
steer_gyh_sum_l+=pwm_steer_gyh_save[i_sum];
}
steer_gyh_ave_l=0.1*steer_gyh_sum_l;
steer_gyh_abs_ave_l=0.1*steer_gyh_abs_sum_l;
steer_gyh_sum_l=0;
steer_gyh_abs_sum_l=0;
steer_gyh_abs_ave=steer_gyh_abs_ave_l;
steer_gyh_ave=steer_gyh_ave_l;
steer_gyh_fangcha_l=pwm_steer_gyh_abs-steer_gyh_abs_ave_l;
steer_gyh_fangcha=steer_gyh_fangcha_l;
}
/*----------------------------------------------------------------------------
void steer_guiyihua(void) //归一化函数
*--------------------------------------------------------*/
void steer_guiyihua(void)
{
if(pwm_steer<steer_midd_adjust)
{
pwm_steer_gyh=(pwm_steer-steer_midd_adjust)*1000/(steer_midd_adjust-steer_rigt_adjust) ;
pwm_steer_gyh_abs=-pwm_steer_gyh;
}
else
{
pwm_steer_gyh=(pwm_steer-steer_midd_adjust)*1000/(steer_left_adjust-steer_midd_adjust);
pwm_steer_gyh_abs=pwm_steer_gyh;
}
steer_gyh_abs_save();
}
/*----------------------------------------------------------------------------
void direction_control(void) //方向控制
*--------------------------------------------------------*/
void direction_control(void)
{
float P_DIRECTION,D_DIRECTION,I_DIRECTION;
int32_t steer_direction;
int32_t row_sub;
P_DIRECTION_RATIO=get_kp(po_er);
// if(ROW_FLAG == 2)
// {
// P_DIRECTION_RATIO=0.025;
// }
// P_DIRECTION_RATIO=0.046;
D_DIRECTION_RATIO=d;
P_DIRECTION = P_DIRECTION_RATIO;
D_DIRECTION = D_DIRECTION_RATIO;
I_DIRECTION = I_DIRECTION_RATIO;
row_sub = row_save[row_save_size-6] - row_save[row_save_size];
steer_direction = (int32_t)(P_DIRECTION * (2500 - row) + D_DIRECTION * row_sub + I_DIRECTION) + steer_midd_adjust ;//算法依据？
if(row_save[99]==row_save[98]&&row_save[98]==row_save[97]&&row_save[97]==row_save[96])
{
// if(steer_direction>steer_midd_adjust)
// steer_direction=steer_left_adjust;
// else if(steer_direction<steer_midd_adjust)
// steer_direction=steer_rigt_adju++st;
if(i_stop<400)
{i_stop++;
}
}
else
{
i_stop=0;
}
if( steer_direction > steer_left_adjust)
steer_direction = steer_left_adjust;
else if(steer_direction < steer_rigt_adjust)
steer_direction = steer_rigt_adjust;
// if(steer_direction-pwm_steer>10)
// {
// pwm_steer+=10;
// }
// else if(steer_direction-pwm_steer<-10)
// {
// pwm_steer-=10;
// }
// else
// {
pwm_steer = steer_direction;
// }
steer_guiyihua();
// if(steer_direction > 0)//右转
// steer_set = steer_direction * steer_rigt_adjust_x + steer_midd_adjust;
// else //左转
// steer_set = steer_direction * steer_left_adjust_x + steer_midd_adjust;
}
/*----------------------------------------------------------------------------
void motor_control(void) //速度控制
*--------------------------------------------------------*/
double get_k_motor(double speed_er)
{
double k_motor_l;
int speed_er_abs_l;
if(speed_er<0) //取绝对值
{
speed_er_abs_l=-speed_er;
}
else
{
speed_er_abs_l=speed_er;
}
if(speed_er_abs_l<50)
{
k_motor_l=2*speed_er_abs_l*0.01*speed_er_abs_l*0.01;
}
else if(speed_er_abs_l<100)
{
k_motor_l=1-2*(speed_er_abs_l*0.01-1)*(speed_er_abs_l*0.01-1);
}
else
{
k_motor_l=1;
}
return k_motor_l;
}
void speedsave(void)
{
volatile int i_sp = 0;
i_sp = 0;
while(i_sp < speed_save_size)
{
speed_save[i_sp] = speed_save[i_sp+1];
i_sp++;
}
speed_save[speed_save_size] = speed;
}
int get_speed_set()
{
static int speed_set_l;
// int speed_l;
static int zd_flag,zd2_flag;
start++ ;
if(start<500)
{
speed_set_l=0;
ROW_FLAG=1;
}
else if(start<600)
{
speed_set_l+=5;
ROW_FLAG=1;
}
else
{
//
start=1201;
ROW_FLAG=0;
if(steer_gyh_fangcha<50&&steer_gyh_fangcha>-50)
{
if(pwm_steer_gyh_abs<200)
{
zd_flag++;
zd2_flag++;
if(zd_flag>10)
{
speed_set_l=s_z1;
zd_flag=21;
}
}
else if(pwm_steer_gyh_abs<400)
{
zd2_flag++ ;
zd_flag=0;
if(zd2_flag>10)
{
speed_set_l=s_z1-0.001*pwm_steer_gyh_abs*(s_z1-s_z2);
zd2_flag=21;
}
}
else
{
speed_set_l=s_w1-0.001*pwm_steer_gyh_abs*(s_w1-s_w2);
zd_flag=0;
zd2_flag=0;
}
}
else if(steer_gyh_fangcha>50)
{
speed_set_l=s_s1-0.001*pwm_steer_gyh_abs*(s_s1-s_s2);
zd2_flag=0 ;
zd_flag=0;
}
else
{
speed_set_l=s_w1;
}
if(speed_set_l>= s_z1)
{
speed_set_l = s_z1;
}
}
if(XS_FLAG==1)
{
speed_set_l=s_w1;
}
// else
// {
// zd_flag=0;
// speed_set_l=700-0.1*pwm_steer_gyh_abs;
// if(pwm_steer_gyh_abs>900&&steer_gyh_fangcha<100&&steer_gyh_fangcha>-100)
// speed_set_l=450;
// }
//// speed_set_l=530-0.1*pwm_steer_gyh_abs;
//// if(pwm_steer_gyh_abs>800&&steer_gyh_fangcha<100&&steer_gyh_fangcha>-100)
//// speed_set_l=300;
// }
if(i_stop>=400||CAR_STOP == 1)
{
speed_set_l=0;
}
return speed_set_l;
}
void motor_control(void)
{
int speed_er_l,speed_d_er_l;
// int speed_set_ls;
float kp_motor,kd_motor;
speed=MDL_ACDdecod_Function2(MODE_FIRSTDECODE_FALSE);
/***************************************************************************************/
/***************************************************************************************/
speed_set=get_speed_set();//可以改成恒定值
/***************************************************************************************/
/***************************************************************************************/
// speed_set=430;
// if(speed_set_ls>speed_set)
// {
// speed_set++;
// }
// else if( speed_set_ls<speed_set)
// {
// speed_set-=5;
// }
speedsave();
speed_d_er_l=speed_save[8]-speed_save[9];
kp_motor=0.5;
kd_motor=get_k_motor(speed_er_l);
pwm_motor_old = 2500;
//pwm_motor = pwm_motor_old-kp_motor*(speed_set-speed)-kd_motor*speed_d_er_l;//
pwm_motor = pwm_motor_old-kp_motor*(speed_set+speed)+kd_motor*speed_d_er_l;//
pwm_motor_old=pwm_motor;
if(PWM_ENABLE_FLAG==0)
{
pwm_motor = 5000;
// pwm_motor=3500;
}
}
void PWM_DISABLE(void)
{
PWM_ENABLE_FLAG=0;
// MK60_PORT_GPIO_IO_SET(PORTA,12,1,0); //初始化不让PWM输出
// MK60_PORT_GPIO_IO_SET(PORTA,13,1,0);
}
void PWM_ENABLE(void)
{
PWM_ENABLE_FLAG=1;
// PORTA->PCR[12] = PORT_PCR_MUX(4); // FTM1 CH0
// PORTA->PCR[13] = PORT_PCR_MUX(4); // FTM1 CH1
}
void put_send_buffer(void)
{
int32_t temp;
int32_t write_time = 1000; //向send_buffer[]写数据时间周期 单位ms 最快10ms
static uint32_t count_1s=4;
count_1s++;
if(count_1s>=(write_time*1000/PIT0_TIME)){//PIT0_TIME = 5000；
count_1s=0;
// LED_REVERSE(LED1);
if(UART_send_length==0){
temp=row;
UART_send_buffer[0] ='r';
UART_send_buffer[1] ='o';
UART_send_buffer[2] ='w';
if(temp<0)UART_send_buffer[3]='-',temp=-temp;else UART_send_buffer[3]='+';
UART_send_buffer[4]=temp%10000/1000+'0';
UART_send_buffer[5]=temp%1000/100+'0';
UART_send_buffer[6]=temp%100/10+'0';
UART_send_buffer[7]=temp%10+'0';
temp=em[1];
UART_send_buffer[8] ='e';
UART_send_buffer[9] ='m';
UART_send_buffer[10] ='a';
if(temp<0)UART_send_buffer[11]='-',temp=-temp;else UART_send_buffer[11]='+';
UART_send_buffer[12]=temp%100000/10000+'0';
UART_send_buffer[13]=temp%10000/1000+'0';
UART_send_buffer[14]=temp%1000/100+'0';
UART_send_buffer[15]=temp%100/10+'0';
UART_send_buffer[16]=temp%10+'0';
UART_send_buffer[17]=' ';
UART_send_buffer[18]='\r';
UART_send_buffer[19]='\n';
UART_send_length=20;
}
}
}
// void put_send_buffer(void)
//{
// int32_t temp;
// int32_t write_time = 1000; //向send_buffer[]写数据时间周期 单位ms
// static uint32_t count_1s=4;
//
// count_1s++;
// if(count_1s>=(write_time*1000/PIT0_TIME)){//PIT0_TIME = 5000；
// count_1s=0;
// LED_REVERSE(LED1);
// if(UART_send_length==0){
// temp=row;
// if(temp<0)UART_send_buffer[0]='-',temp=-temp;else UART_send_buffer[0]='+';
// UART_send_buffer[1]=temp%1000/100+'0';
// UART_send_buffer[2]=temp%100/10+'0';
// UART_send_buffer[3]=temp%10+'0';
// UART_send_buffer[4]=' ';
// UART_send_buffer[5]='\r';
// UART_send_buffer[6]='\n';
// UART_send_length=7;
// }
// }
//}
void startline_init(void)
{
MK60_PORT_GPIO_IO_pulldown(PORTA,17,0);
MK60_PORT_GPIO_IO_pulldown(PORTA,9,0);
// MK60_PORT_GPIO_IO_SET(PORTA,9,0,0);
// MK60_PORT_GPIO_IO_SET(PORTA,17,0,0);
}
//改成PORTA17和PORTA9
void startline(void)
{
static uint32_t start_i=0,start_state = 1;
if(start_state == 1)
{
if(MK60_PORT_GPIO_READ(PORTA,17)||MK60_PORT_GPIO_READ(PORTA,9))
{
start_state = START_OFF;
START_LINE++ ;
start_line = START_LINE;
}
}
else
{
start_i++;
if(start_i >= 1000)
{
start_state = START_ON;
start_i = 0;
}
}
if(START_LINE>=2)
{
CAR_STOP = 1;
}
}
void KEY_init(void)
{
MK60_PORT_GPIO_IO_SET(PORTA,14,0,0); //按键接口
MK60_PORT_GPIO_IO_SET(PORTA,15,0,0);
MK60_PORT_GPIO_IO_SET(PORTA,16,0,0);
MK60_PORT_GPIO_IO_SET(PORTA,5,0,0);
MK60_PORT_GPIO_IO_SET(PORTA,6,0,0);
MK60_PORT_GPIO_IO_SET(PORTA,7,0,0);
MK60_PORT_GPIO_IO_SET(PORTA,8,0,0);
PORTA->PCR[0]=(0|PORT_PCR_MUX(7));
PORTA->PCR[1]=(0|PORT_PCR_MUX(7));
PORTA->PCR[2]=(0|PORT_PCR_MUX(7));
PORTA->PCR[3]=(0|PORT_PCR_MUX(7));
}
int main(void)
{
DisableInterrupts(); //禁止总中断
//系统正式开始工作前的启动代码，全部迁移至以下函数中进行
//系统时钟设置-->配置中毒向量表-->允许GPIO时钟-->系统运行频率信息更新
//-->配置并启动SYSTICK时钟-->如有必要，开启TRACE_CLKOUT方便调试
MK60_SYSTEM_STARTWORK();
MK60_FTFL_INITIALIZE_();
MDL_ACDdecod_Initializ(); //增量式编码器的初始化函数
UART_INIT(); //用于串行通信端口的初始化函数，可作调试功能/数据交互
ADC_INIT(); //初始化AD转换
MDL_PITCNTRL_Initialize(); //用于PIT端口的初始化函数，外环控制器运行使用
// LED_INIT();
MDL_GPIO_Initialize(); //用于DLED的GPIO初始化函数
startline_init();
KEY_init();
LCD_Init();
LCD_Fill(0x00);//亮屏
LCD_Print(5,3,"Welcome!");
EnablesInterrupts(); //开总中断
MDL_PWMtimer_Initializ(); //用于PWM输出的初始化函数
MK60_PITS_Enables_IRQ(MK60_PIT_CH0);
//LED_ON(LED1+LED2+LED3+LED4);
while(1)
{
key_task();
send_task();
}
}
/*----------------------------------------------------------------------------
ADC0_IRQHandler ，获取两个陀螺仪的信号
*----------------------------------------------------------------------------*/
// void ADC0_IRQHandler(void)
// {
//
// }
/*----------------------------------------------------------------------------
ADC1_IRQHandler ,获取五个电磁传感器的值，并且记录放入数组em_adc[x][]中。x=0,1,2,3,4
*----------------------------------------------------------------------------*/
void ADC1_IRQHandler(void)
{
static uint32_t adc_cnt = 0;
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC1_EM0;
switch (adc_cnt)
{
case 0:
{
adc1_head++;
if(adc1_head >= 10)
adc1_head = 0;
em_adc[0][adc1_head] = ADC1->R[0];
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC1_EM1;
adc_cnt = 1;
break;
}
case 1:
{
em_adc[1][adc1_head] = ADC1->R[0];
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC1_EM2;
adc_cnt = 2;
break;
}
case 2:
{
em_adc[2][adc1_head] = ADC1->R[0];
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC1_EM3;
adc_cnt = 3;
break;
}
case 3:
{
em_adc[3][adc1_head] = ADC1->R[0];
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC1_EM4;
adc_cnt = 4;
break;
}
case 4:
{
em_adc[4][adc1_head] = ADC1->R[0];
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC_DISABLE;
adc_cnt = 0;
break;
}
}
em_adc_filter();
}
uint32_t msTicks = 0; /* counts 1ms timeTicks */
/*----------------------------------------------------------------------------
PIT0中断服务程序 5ms
内部填补控制算法
PIT_CHx_IRQHandler中断函数，根据所定义的PIT_CHx端口进行设置。
注意：中断处理函数名称要和所响应的中断源一致
*----------------------------------------------------------------------------*/
void PIT0_IRQHandler(void)
{
static uint32_t i = 0;
PIT->CHANNEL[0].TCTRL &= ~PIT_TCTRL_TIE_MASK;
PIT->CHANNEL[0].TFLG |= PIT_TFLG_TIF_MASK;
PIT->CHANNEL[0].TCTRL |= (PIT_TCTRL_TIE_MASK | PIT_TCTRL_TEN_MASK);
if(i>=199)
{
i=0;
// LED_REVERSE(LED3);
}
i++;
//算法开始
//1、通过em[][]数组中的值计算赛道位置
get_row();
//2、通过位置确定舵机pwm_steer值
direction_control();
//3、通过编码器获得车轮速度，根据现有速度得出pwm_motor值
motor_control();
if(ROW_FLAG==1)
{
pwm_motor=3000;
}
// else if(ROW_FLAG==2)
// {
// pwm_steer=steer_midd_adjust;
// }
startline();
// pwm_steer = 490;//left:800 midd:860 rigt 915
// pwm_motor = 9000;
//算法结束
// pwm_steer=pwm_steer_t;
//
MDL_SteerDriveNM01Duty(pwm_steer);//
MDL_MotorDriveModiDuty(10000-pwm_motor); //电机给定占空比 不是浮点数 0~10000对应0~100%
//MDL_MotorDriveModiDuty(7500); //电机给定占空比 不是浮点数 0~10000对应0~100%
in_stream(row,pwm_steer,speed,speed_set);
}
/**************************************************************************
PIT1中断服务程序 0.5ms
PIT_CH1_IRQHandler中断函数
500us 触发一次AD采样 即采样频率为2khz
****************************************************************************/
void PIT1_IRQHandler(void)
{
PIT->CHANNEL[1].TCTRL &= ~PIT_TCTRL_TIE_MASK;
PIT->CHANNEL[1].TFLG |= PIT_TFLG_TIF_MASK;
PIT->CHANNEL[1].TCTRL |= (PIT_TCTRL_TIE_MASK | PIT_TCTRL_TEN_MASK);
// ADC0->SC1[0] = (ADC0->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC0_GYRO1;
ADC1->SC1[0] = (ADC1->SC1[0] & (~ADC_SC1_ADCH_MASK)) + ADC1_EM0;
}
//File .End. Here.
