单片机论坛

 找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 227|回复: 0
收起左侧

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

[复制链接]
狄利克雷 发表于 2019-9-29 09:51 | 显示全部楼层 |阅读模式

结构照片

结构照片

结构照片

结构照片

部分代码
源码太大783056733要源码

  1. /**
  2. * @file                采集电感值
  3. * @author                电子信息工程学院 李科磊
  4. * @date                2019 3月22   
  5. */
  6. void Get_AD(void)
  7. {
  8.     uint8 i=0,j=0,k=0;
  9.     int L_AD=0,R_AD=0,M_AD=0,B_AD=0;
  10.     for(i=0;i<5;i++)//每个电感取5次值
  11.     {
  12.         B_AD+=adc_once(ADC0_SE11,ADC_10bit);//采集ADC0_SE0电压,精度10位
  13.     }
  14.     Standar_AD=B_AD*0.2;
  15.     for(i=0;i<10;i++)//每个电感取5次值
  16.     {
  17.         Left_org[i]=adc_once(ADC0_SE10,ADC_10bit);//采集ADC0_SE0电压,精度10位
  18.         Left_org[i]=(i>0&&(Left_org[i]-Left_org[i-1])>5?Left_org[i-1]:Left_org[i]);
  19.         Right_org[i]=adc_once(ADC0_SE17,ADC_10bit);//采集ADC0_SE1电压,精度10位
  20.         Right_org[i]=(i>0&&(Right_org[i]-Right_org[i-1])>5?Right_org[i-1]:Right_org[i]);
  21.         Mid_org[i]=adc_once(ADC0_SE18,ADC_12bit);
  22.         Mid_org[i]=(i>0&&(Mid_org[i]-Mid_org[i-1])>5?Mid_org[i-1]:Mid_org[i]);
  23.     }
  24. /*===========================冒泡降序排序=================================*/
  25.       for(j=0;j<10;j++)  //10次数据
  26.       {
  27.         for(k=0;k<10-j;k++)
  28.         {
  29.           if(Left_org[k]<Left_org[k+1])
  30.           {
  31.             L_AD = Left_org[k+1];
  32.             Left_org[k+1]=Left_org[k];
  33.             Left_org[k]=L_AD;
  34.           }
  35.           if(Right_org[k]<Right_org[k+1])
  36.           {
  37.             R_AD = Right_org[k+1];
  38.             Right_org[k+1]=Right_org[k];
  39.             Right_org[k]=R_AD;
  40.           }
  41.           if(Mid_org[k]<Mid_org[k+1])
  42.           {
  43.             M_AD = Mid_org[k+1];
  44.             Mid_org[k+1]=Mid_org[k];
  45.             Mid_org[k]=M_AD;
  46.           }

  47.         }
  48.       }
  49. /*===========================中值滤波=================================*/

  50.       g_fLeft_AD=(Left_org[3]+Left_org[4]+Left_org[5]+Left_org[6])*0.25f;
  51.       g_fRight_AD=(Right_org[3]+Right_org[4]+Right_org[5]+Right_org[6])*0.25f;
  52.       g_fCenter_AD=(Mid_org[3]+Mid_org[4]+Mid_org[5]+Mid_org[6])*0.25f;
  53. //**********最小二乘法求斜率*********//
  54.       for(i=0;i<7;i++)
  55.       {
  56.         AD_right_buff[i]=AD_right_buff[i+1];
  57.         AD_left_buff[i]=AD_left_buff[i+1];
  58.       }
  59.       AD_right_buff[7]=g_fRight_AD;
  60.       AD_left_buff[7]=g_fLeft_AD;

  61.       float xsum=0.216,ysum=0,xysum=0,x2sum=0.00734;//0.01116
  62.       for(i=0;i<8;i++)
  63.       {
  64.         ysum+=AD_right_buff[i];
  65.         xysum+=(i+1)*(AD_right_buff[i])*0.006;
  66.       }
  67.       AD_right_slope=(8*xysum-xsum*ysum)/(8*x2sum-xsum*xsum)*0.01;

  68.       ysum = xysum = 0;
  69.       for(i=0;i<8;i++)
  70.       {
  71.         ysum+=AD_left_buff[i];
  72.         xysum+=(i+1)*(AD_left_buff[i])*0.006;
  73.       }
  74.       AD_left_slope=(8*xysum-xsum*ysum)/(8*x2sum-xsum*xsum)*0.01;
  75.       Exp=500/(g_fCenter_AD+0.5*abs((int)(g_fLeft_AD-g_fRight_AD)));
  76.       Exp=Exp>2?2:Exp;
  77.       Exp=Exp<1?1:Exp;
  78.       error_AD=(g_fLeft_AD-g_fRight_AD)*100*Exp/(g_fRight_AD+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
  79.       if(fall_flag==0)
  80.       {
  81.           if(obstacl_flag==1)
  82.           {
  83.              error_AD=40*g_fRealSpeed/140;//进弯道吗慢速30   //中弯道中速28 40 23
  84.           }
  85.           else  if(obstacl_flag==2)
  86.           {
  87.              error_AD=-32*g_fRealSpeed/140;//  三轮中速30//40
  88.           }
  89.           else  if(obstacl_flag==3)
  90.           {
  91.              error_AD=40*g_fRealSpeed/140;
  92.           }
  93.       }
  94.       else
  95.       {
  96.           if(obstacl_flag==1)
  97.           {
  98.              error_AD=40*g_fRealSpeed/140;//进弯道吗慢速30   //中弯道中速28 40 23
  99.           }
  100.           else  if(obstacl_flag==2)
  101.           {
  102.              error_AD=-30*g_fRealSpeed/140;//  三轮中速30//40
  103.           }
  104.           else  if(obstacl_flag==3)
  105.           {
  106.              error_AD=40*g_fRealSpeed/140;
  107.           }
  108.       }


  109. }
  110. /**********环岛判断**********/
  111. void Round_Judge()
  112. {
  113.    if(QZAngle<-95&&(fall_flag==0||fall_flag==4))//直立状态下环岛检测
  114.    {
  115.      if(Deraction%3==1)
  116.      {
  117.        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)//预判断
  118.         {
  119.                Round_flag_L=1;
  120.                if(Deraction%3==1)
  121.                {
  122.                   Deraction=2;
  123.                }
  124.                else
  125.                {
  126.                   Deraction=0;
  127.                }
  128.         }
  129.      }
  130.      else  if(Deraction%3==0)
  131.      {
  132.         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)//预判断
  133.         {
  134.                   Round_flag_R=1;
  135.                   Deraction=1;
  136.           }
  137.       }
  138.       else if(Deraction%3==2)
  139.       {
  140.          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)//预判断
  141.          {
  142.                   Round_flag_L=1;
  143.                   Deraction=0;
  144.           }
  145.       }
  146.       if(AD_right_slope>40&&Round_flag_R==1)//环岛入口+(QZAngle+91)*10   Standar_AD>220  60 40
  147.       {
  148.          Round_flag_R=2;
  149.          Counter=-1;
  150.       }
  151.       if(AD_left_slope>0&&Round_flag_L==1&&AD_right_slope==0&&Deraction%3==2)////环岛入口 200*(130/g_fRealSpeed)
  152.       {
  153.            Round_flag_L=2;
  154.            Counter=-1;
  155.       }
  156.       if(AD_left_slope>0&&Round_flag_L==1&&Deraction%3==0)//环岛入口 200*(130/g_fRealSpeed)
  157.       {
  158.            Round_flag_L=2;
  159.            Counter=-1;
  160.       }
  161.         if(Round_flag_R==2)//右环打角
  162.         {
  163.            error_AD=(g_fLeft_AD*0.24-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.24+0.5*g_fCenter_AD);//差比和获取偏差
  164.            error_AD=error_AD>55?55:error_AD;//42
  165.            error_AD=error_AD<-55?-55:error_AD;

  166.         }
  167.         if(Round_flag_L==2)//左环打角
  168.         {
  169.            error_AD=(g_fLeft_AD-g_fRight_AD*0.24)*300/(g_fRight_AD*0.24+g_fLeft_AD+0.5*g_fCenter_AD);//差比和获取偏差
  170.            if(Deraction%3==0)
  171.            {
  172.               //error_AD=error_AD<-40?-40:error_AD;//42
  173.               error_AD=25;//error_AD>40?40:error_AD;
  174.            }
  175.            else
  176.            {
  177.               error_AD=error_AD<-50?-50:error_AD;//42
  178.               error_AD=error_AD>50?50:error_AD;
  179.            }

  180.         }
  181.         if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
  182.         {
  183.             Counter--;
  184.         }
  185.        if(Round_flag_R==0&&Round_L==1) //进入环岛电感处理&&Counter>0
  186.        {
  187.          error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
  188.          error_AD=error_AD<-45?-45:error_AD;//42
  189.          error_AD=error_AD>45?45:error_AD;
  190.        }
  191.        else  if(Round_flag_R==0&&Round_L==2)
  192.        {
  193.          error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
  194.          error_AD=error_AD<-45?-45:error_AD;//42
  195.          error_AD=error_AD>45?45:error_AD;
  196.        }
  197.        if(Deraction%3==0)
  198.       {
  199.          if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
  200.         {
  201.           g_fLeft_AD=400-Counter*1;
  202.          error_AD=(g_fLeft_AD-g_fRight_AD)*100/(g_fLeft_AD+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
  203.          error_AD=error_AD<-45?-45:error_AD;//42
  204.          error_AD=error_AD>45?45:error_AD;
  205.          }
  206.       }
  207.        else if(Round_flag_L==0&&Round_R==1) //进入环岛电感处理&&Counter>0
  208.        {
  209.          error_AD=(350-g_fRight_AD)*100/(g_fRight_AD+350+1*g_fCenter_AD);//差比和获取偏差
  210.          error_AD=error_AD<-45?-45:error_AD;//42
  211.          error_AD=error_AD>45?45:error_AD;
  212.        }
  213.        else if(Round_flag_L==0&&Round_R==2)
  214.        {
  215.          error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
  216.          error_AD=error_AD<-45?-45:error_AD;//42
  217.          error_AD=error_AD>45?45:error_AD;
  218.        }
  219.    }
  220.    if(fall_flag==2)//三轮状态下环岛检测
  221.    {
  222.       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)
  223.         {
  224.             if(Deraction%3==0)
  225.             {
  226.               Round_flag_R=1;
  227.               Deraction=1;
  228.             }
  229.             else  if(Deraction==1)
  230.             {
  231.                Round_flag_L=1;
  232.                Deraction=2;
  233.             }
  234.             else if(Deraction==2)
  235.             {
  236.                  Round_flag_L=1;
  237.                  Deraction=0;
  238.             }
  239.         }
  240.         if(Standar_AD>550&&Round_flag_R==1)//环岛入口
  241.         {
  242.            Round_flag_R=2;
  243.            Counter=-1;
  244.         }
  245.         if(Standar_AD>535*(150.0/g_fRealSpeed)&&Round_flag_L==1&&Deraction==2)////环岛入口
  246.         {
  247.            Round_flag_L=2;
  248.            Counter=-1;
  249.         }
  250.         if(Standar_AD>600&&Round_flag_L==1&&Deraction==0)////环岛入口
  251.         {
  252.            Round_flag_L=2;
  253.            Counter=-1;

  254.         }
  255.         if(Round_flag_R==2)//右环打角
  256.         {
  257.            error_AD=(g_fLeft_AD*0.25-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.25+0.5*g_fCenter_AD);//差比和获取偏差
  258.            error_AD=error_AD>(42*g_fRealSpeed/140)?(42*g_fRealSpeed/140):error_AD;//42
  259.            error_AD=error_AD<(-42*g_fRealSpeed/140)?(-42*g_fRealSpeed/140):error_AD;
  260.         }
  261.         if(Round_flag_L==2)//左环打角
  262.         {
  263.            //差比和获取偏差
  264.           if(Deraction%3==0)
  265.           {
  266.             error_AD=17*(g_fRealSpeed/135.0);//error_AD>40?40:error_AD;
  267.           }
  268.           else
  269.           {
  270.             error_AD=(g_fLeft_AD-g_fRight_AD*0.25)*300/(g_fRight_AD*0.25+g_fLeft_AD+0.5*g_fCenter_AD);
  271.             error_AD=error_AD<-42?-42:error_AD;//42
  272.             error_AD=error_AD>42?42:error_AD;
  273.           }
  274.         }
  275.         if(Round_flag_L==0&&Round_R==1) //进入环岛电感处理&&Counter>0
  276.         {
  277.            error_AD=(200-g_fRight_AD)*100/(250+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
  278.            error_AD=error_AD<-42?-42:error_AD;//42
  279.            error_AD=error_AD>42?42:error_AD;
  280.        }
  281.        else if(Round_flag_L==0&&Round_R==2)
  282.        {
  283.            error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
  284.            error_AD=error_AD<-42?-42:error_AD;//42
  285.            error_AD=error_AD>42?42:error_AD;
  286.        }
  287.        if(Round_flag_R==0&&Round_L==1) //进入环岛电感处理//(1+(Counter-300)/500)
  288.       {  
  289.         if(Deraction%3==2)
  290.         {
  291.            error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
  292.            error_AD=error_AD<-45?-45:error_AD;//42
  293.            error_AD=error_AD>45?45:error_AD;
  294.         }
  295.         else
  296.         {
  297.            error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和获取偏差
  298.            error_AD=error_AD<-45?-45:error_AD;//42
  299.            error_AD=error_AD>45?45:error_AD;
  300.         }
  301.       }
  302.       if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
  303.       {
  304.          error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
  305.          error_AD=error_AD<-45?-45:error_AD;//42
  306.          error_AD=error_AD>45?45:error_AD;
  307.       }
  308. //      if(Round_flag_R==0&&Round_L==2) //进入环岛电感处理//(1+(Counter-300)/500)
  309. //      {
  310. //       error_AD=(200-g_fRight_AD)*100/(200+g_fRight_AD+1*g_fCenter_AD);//差比和获取偏差
  311. //       error_AD=error_AD<-45?-45:error_AD;//42
  312. //       error_AD=error_AD>45?45:error_AD;
  313. //       }


  314.    }
  315.    if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
  316.    {
  317.       Counter--;
  318.    }
  319.    if((Round_R==1||Round_L==1||Round_L==2||Round_R==2)&&Counter==0)//变量清零
  320.    {
  321.       Round_R=0;
  322.       Round_L=0;
  323.    }
  324. }
  325. /**
  326. * @file                方向外环
  327. * @note                      三电感
  328. * @brief                山外K60平台
  329. * @author                电子信息工程学院 李科磊
  330. * @date                2019年3月22日
  331. */
  332. void DerectError_Get(void)  //外环
  333. {
  334.   Get_AD();
  335.   if(obstacl_Delay<300)
  336.   {
  337.      Round_Judge();
  338.   }


  339.   if(fall_flag==2)
  340.   {
  341.      target_vel_z=(5.7)*error_AD+4.5*(error_AD-last_error_AD);
  342.      target_vel_z=target_vel_z>240?240:target_vel_z;
  343.      target_vel_z=target_vel_z<-240?-240:target_vel_z;
  344.   }
  345.   else
  346.   {
  347.      target_vel_z=(5.0)*error_AD+3.0*(error_AD-last_error_AD);
  348.      target_vel_z=target_vel_z>250?250:target_vel_z;
  349.      target_vel_z=target_vel_z<-250?-250:target_vel_z;
  350.   }
  351.   last_error_AD=error_AD;
  352. }
  353. /**
  354. * @file                方向内环
  355. * @note                      串级
  356. * @brief                山外K60
  357. * @author                沈阳航空航天大学 天牛座
  358. * @date                2019年3月22日
  359. */
  360. void Direction_out(void)
  361. {
  362.   float error;
  363.   static  float last_error;
  364.   float price_Differention;
  365.   float price_Proportion;
  366.   static float Pre1_Error[4];
  367.   //error = target_vel_z*13*(-89/QZAngle) - GYRO_Speed_Z*0.25*(-100/QZAngle);//转向角度串方向
  368.   error = target_vel_z*13.0 - GYRO_Speed_Z*0.25;
  369.   price_Proportion=g_priceControl_P_in * error;
  370.   price_Differention=g_priceControl_D_in * (error - last_error);
  371.   if(fall_flag==2)
  372.   {
  373.      error = target_vel_z*13.1 - GYRO_Speed_Z*0.245;//转向角度串方向
  374.      price_Proportion=g_priceControl_P_in * error;
  375.      price_Differention=g_priceControl_D_in * (error - last_error)*1.1;
  376.   }
  377.   Direct_Parameter =price_Proportion  + price_Differention ;//(1+error*error*0.000000001f)

  378.   last_error =  error;
  379.   Pre1_Error[3]=Pre1_Error[2];
  380.   Pre1_Error[2]=Pre1_Error[1];
  381.   Pre1_Error[1]=Pre1_Error[0];
  382.   Pre1_Error[0]=Direct_Parameter;
  383.   Direct_Parameter=Pre1_Error[0]*0.8+Pre1_Error[1]*0.1+Pre1_Error[2]*0.06+Pre1_Error[3]*0.02 ;
  384.   price_PWM=Direct_Parameter;
  385.   price_PWM=price_PWM>550?550:price_PWM;
  386.   price_PWM=price_PWM<-550?-550:price_PWM;
  387. }
复制代码


评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|单片机论坛 |51黑电子论坛技术交流 管理员QQ:125739409;技术交流QQ群636986012

Powered by 单片机教程网

快速回复 返回顶部 返回列表