十四届恩智浦变形金刚三轮组资料 -

/**
* @file 采集电感值
* @author 电子信息工程学院李科磊
* @date 2019 3月22
*/
void Get_AD(void)
{
uint8 i=0,j=0,k=0;
int L_AD=0,R_AD=0,M_AD=0,B_AD=0;
for(i=0;i<5;i++)//每个电感取5次值
{
B_AD+=adc_once(ADC0_SE11,ADC_10bit);//采集ADC0_SE0电压，精度10位
}
Standar_AD=B_AD*0.2;
for(i=0;i<10;i++)//每个电感取5次值
{
Left_org[i]=adc_once(ADC0_SE10,ADC_10bit);//采集ADC0_SE0电压，精度10位
Left_org[i]=(i>0&&(Left_org[i]-Left_org[i-1])>5?Left_org[i-1]:Left_org[i]);
Right_org[i]=adc_once(ADC0_SE17,ADC_10bit);//采集ADC0_SE1电压，精度10位
Right_org[i]=(i>0&&(Right_org[i]-Right_org[i-1])>5?Right_org[i-1]:Right_org[i]);
Mid_org[i]=adc_once(ADC0_SE18,ADC_12bit);
Mid_org[i]=(i>0&&(Mid_org[i]-Mid_org[i-1])>5?Mid_org[i-1]:Mid_org[i]);
}
/*===========================冒泡降序排序=================================*/
for(j=0;j<10;j++) //10次数据
{
for(k=0;k<10-j;k++)
{
if(Left_org[k]<Left_org[k+1])
{
L_AD = Left_org[k+1];
Left_org[k+1]=Left_org[k];
Left_org[k]=L_AD;
}
if(Right_org[k]<Right_org[k+1])
{
R_AD = Right_org[k+1];
Right_org[k+1]=Right_org[k];
Right_org[k]=R_AD;
}
if(Mid_org[k]<Mid_org[k+1])
{
M_AD = Mid_org[k+1];
Mid_org[k+1]=Mid_org[k];
Mid_org[k]=M_AD;
}
}
}
/*===========================中值滤波=================================*/
g_fLeft_AD=(Left_org[3]+Left_org[4]+Left_org[5]+Left_org[6])*0.25f;
g_fRight_AD=(Right_org[3]+Right_org[4]+Right_org[5]+Right_org[6])*0.25f;
g_fCenter_AD=(Mid_org[3]+Mid_org[4]+Mid_org[5]+Mid_org[6])*0.25f;
//**********最小二乘法求斜率*********//
for(i=0;i<7;i++)
{
AD_right_buff[i]=AD_right_buff[i+1];
AD_left_buff[i]=AD_left_buff[i+1];
}
AD_right_buff[7]=g_fRight_AD;
AD_left_buff[7]=g_fLeft_AD;
float xsum=0.216,ysum=0,xysum=0,x2sum=0.00734;//0.01116
for(i=0;i<8;i++)
{
ysum+=AD_right_buff[i];
xysum+=(i+1)*(AD_right_buff[i])*0.006;
}
AD_right_slope=(8*xysum-xsum*ysum)/(8*x2sum-xsum*xsum)*0.01;
ysum = xysum = 0;
for(i=0;i<8;i++)
{
ysum+=AD_left_buff[i];
xysum+=(i+1)*(AD_left_buff[i])*0.006;
}
AD_left_slope=(8*xysum-xsum*ysum)/(8*x2sum-xsum*xsum)*0.01;
Exp=500/(g_fCenter_AD+0.5*abs((int)(g_fLeft_AD-g_fRight_AD)));
Exp=Exp>2?2:Exp;
Exp=Exp<1?1:Exp;
error_AD=(g_fLeft_AD-g_fRight_AD)*100*Exp/(g_fRight_AD+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
if(fall_flag==0)
{
if(obstacl_flag==1)
{
error_AD=40*g_fRealSpeed/140;//进弯道吗慢速30 //中弯道中速28 40 23
}
else if(obstacl_flag==2)
{
error_AD=-32*g_fRealSpeed/140;// 三轮中速30//40
}
else if(obstacl_flag==3)
{
error_AD=40*g_fRealSpeed/140;
}
}
else
{
if(obstacl_flag==1)
{
error_AD=40*g_fRealSpeed/140;//进弯道吗慢速30 //中弯道中速28 40 23
}
else if(obstacl_flag==2)
{
error_AD=-30*g_fRealSpeed/140;// 三轮中速30//40
}
else if(obstacl_flag==3)
{
error_AD=40*g_fRealSpeed/140;
}
}
}
/**********环岛判断**********/
void Round_Judge()
{
if(QZAngle<-95&&(fall_flag==0||fall_flag==4))//直立状态下环岛检测
{
if(Deraction%3==1)
{
if((g_fRight_AD+g_fCenter_AD+g_fLeft_AD>1600)&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&AD_right_slope>60&&g_fCenter_AD>300)//预判断
{
Round_flag_L=1;
if(Deraction%3==1)
{
Deraction=2;
}
else
{
Deraction=0;
}
}
}
else if(Deraction%3==0)
{
if((g_fRight_AD+g_fCenter_AD+g_fLeft_AD>1800)&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&AD_right_slope<0&&g_fCenter_AD>350)//预判断
{
Round_flag_R=1;
Deraction=1;
}
}
else if(Deraction%3==2)
{
if((g_fRight_AD+g_fCenter_AD+g_fLeft_AD>1800)&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&AD_right_slope==0&&g_fCenter_AD>350&&AD_left_slope>50)//预判断
{
Round_flag_L=1;
Deraction=0;
}
}
if(AD_right_slope>40&&Round_flag_R==1)//环岛入口+(QZAngle+91)*10 Standar_AD>220 60 40
{
Round_flag_R=2;
Counter=-1;
}
if(AD_left_slope>0&&Round_flag_L==1&&AD_right_slope==0&&Deraction%3==2)////环岛入口 200*(130/g_fRealSpeed）
{
Round_flag_L=2;
Counter=-1;
}
if(AD_left_slope>0&&Round_flag_L==1&&Deraction%3==0)//环岛入口 200*(130/g_fRealSpeed）
{
Round_flag_L=2;
Counter=-1;
}
if(Round_flag_R==2)//右环打角
{
error_AD=(g_fLeft_AD*0.24-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.24+0.5*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD>55?55:error_AD;//42
error_AD=error_AD<-55?-55:error_AD;
}
if(Round_flag_L==2)//左环打角
{
error_AD=(g_fLeft_AD-g_fRight_AD*0.24)*300/(g_fRight_AD*0.24+g_fLeft_AD+0.5*g_fCenter_AD);//差比和获取偏差
if(Deraction%3==0)
{
//error_AD=error_AD<-40?-40:error_AD;//42
error_AD=25;//error_AD>40?40:error_AD;
}
else
{
error_AD=error_AD<-50?-50:error_AD;//42
error_AD=error_AD>50?50:error_AD;
}
}
if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
{
Counter--;
}
if(Round_flag_R==0&&Round_L==1) //进入环岛电感处理&&Counter>0
{
error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
else if(Round_flag_R==0&&Round_L==2)
{
error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
if(Deraction%3==0)
{
if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
{
g_fLeft_AD=400-Counter*1;
error_AD=(g_fLeft_AD-g_fRight_AD)*100/(g_fLeft_AD+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
}
else if(Round_flag_L==0&&Round_R==1) //进入环岛电感处理&&Counter>0
{
error_AD=(350-g_fRight_AD)*100/(g_fRight_AD+350+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
else if(Round_flag_L==0&&Round_R==2)
{
error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
}
if(fall_flag==2)//三轮状态下环岛检测
{
if(((g_fRight_AD>850&&g_fLeft_AD>850))&&Round_flag_L==0&&Counter==0&&Round_flag_R==0&&g_fCenter_AD<700)//预判断||(g_fRight_AD>900||g_fLeft_AD>900)
{
if(Deraction%3==0)
{
Round_flag_R=1;
Deraction=1;
}
else if(Deraction==1)
{
Round_flag_L=1;
Deraction=2;
}
else if(Deraction==2)
{
Round_flag_L=1;
Deraction=0;
}
}
if(Standar_AD>550&&Round_flag_R==1)//环岛入口
{
Round_flag_R=2;
Counter=-1;
}
if(Standar_AD>535*(150.0/g_fRealSpeed)&&Round_flag_L==1&&Deraction==2)////环岛入口
{
Round_flag_L=2;
Counter=-1;
}
if(Standar_AD>600&&Round_flag_L==1&&Deraction==0)////环岛入口
{
Round_flag_L=2;
Counter=-1;
}
if(Round_flag_R==2)//右环打角
{
error_AD=(g_fLeft_AD*0.25-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.25+0.5*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD>(42*g_fRealSpeed/140)?(42*g_fRealSpeed/140):error_AD;//42
error_AD=error_AD<(-42*g_fRealSpeed/140)?(-42*g_fRealSpeed/140):error_AD;
}
if(Round_flag_L==2)//左环打角
{
//差比和获取偏差
if(Deraction%3==0)
{
error_AD=17*(g_fRealSpeed/135.0);//error_AD>40?40:error_AD;
}
else
{
error_AD=(g_fLeft_AD-g_fRight_AD*0.25)*300/(g_fRight_AD*0.25+g_fLeft_AD+0.5*g_fCenter_AD);
error_AD=error_AD<-42?-42:error_AD;//42
error_AD=error_AD>42?42:error_AD;
}
}
if(Round_flag_L==0&&Round_R==1) //进入环岛电感处理&&Counter>0
{
error_AD=(200-g_fRight_AD)*100/(250+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-42?-42:error_AD;//42
error_AD=error_AD>42?42:error_AD;
}
else if(Round_flag_L==0&&Round_R==2)
{
error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-42?-42:error_AD;//42
error_AD=error_AD>42?42:error_AD;
}
if(Round_flag_R==0&&Round_L==1) //进入环岛电感处理//(1+(Counter-300)/500)
{
if(Deraction%3==2)
{
error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
else
{
error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
}
if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
{
error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
error_AD=error_AD<-45?-45:error_AD;//42
error_AD=error_AD>45?45:error_AD;
}
// if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
// {
// error_AD=(200-g_fRight_AD)*100/(200+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
// error_AD=error_AD<-45?-45:error_AD;//42
// error_AD=error_AD>45?45:error_AD;
// }
}
if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
{
Counter--;
}
if((Round_R==1||Round_L==1||Round_L==2||Round_R==2)&&Counter==0)//变量清零
{
Round_R=0;
Round_L=0;
}
}
/**
* @file 方向外环
* @note 三电感
* @brief 山外K60平台
* @author 电子信息工程学院李科磊
* @date 2019年3月22日
*/
void DerectError_Get(void) //外环
{
Get_AD();
if(obstacl_Delay<300)
{
Round_Judge();
}
if(fall_flag==2)
{
target_vel_z=(5.7)*error_AD+4.5*(error_AD-last_error_AD);
target_vel_z=target_vel_z>240?240:target_vel_z;
target_vel_z=target_vel_z<-240?-240:target_vel_z;
}
else
{
target_vel_z=(5.0)*error_AD+3.0*(error_AD-last_error_AD);
target_vel_z=target_vel_z>250?250:target_vel_z;
target_vel_z=target_vel_z<-250?-250:target_vel_z;
}
last_error_AD=error_AD;
}
/**
* @file 方向内环
* @note 串级
* @brief 山外K60
* @author 沈阳航空航天大学天牛座
* @date 2019年3月22日
*/
void Direction_out(void)
{
float error;
static float last_error;
float price_Differention;
float price_Proportion;
static float Pre1_Error[4];
//error = target_vel_z*13*(-89/QZAngle) - GYRO_Speed_Z*0.25*(-100/QZAngle);//转向角度串方向
error = target_vel_z*13.0 - GYRO_Speed_Z*0.25;
price_Proportion=g_priceControl_P_in * error;
price_Differention=g_priceControl_D_in * (error - last_error);
if(fall_flag==2)
{
error = target_vel_z*13.1 - GYRO_Speed_Z*0.245;//转向角度串方向
price_Proportion=g_priceControl_P_in * error;
price_Differention=g_priceControl_D_in * (error - last_error)*1.1;
}
Direct_Parameter =price_Proportion + price_Differention ;//(1+error*error*0.000000001f)
last_error = error;
Pre1_Error[3]=Pre1_Error[2];
Pre1_Error[2]=Pre1_Error[1];
Pre1_Error[1]=Pre1_Error[0];
Pre1_Error[0]=Direct_Parameter;
Direct_Parameter=Pre1_Error[0]*0.8+Pre1_Error[1]*0.1+Pre1_Error[2]*0.06+Pre1_Error[3]*0.02 ;
price_PWM=Direct_Parameter;
price_PWM=price_PWM>550?550:price_PWM;
price_PWM=price_PWM<-550?-550:price_PWM;
}

复制代码