久久久久久久999_99精品久久精品一区二区爱城_成人欧美一区二区三区在线播放_国产精品日本一区二区不卡视频_国产午夜视频_欧美精品在线观看免费

 找回密碼
 立即注冊

QQ登錄

只需一步，快速開始

搜索
查看: 3371|回復: 0
打印 上一主題 下一主題
收起左側

十四屆恩智浦變形金剛三輪組資料

[復制鏈接]
跳轉到指定樓層
樓主

部分代碼
源碼太大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.           }

  48.         }
  49.       }
  50. /*===========================中值濾波=================================*/

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

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

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


  115. }
  116. /**********環島判斷**********/
  117. void Round_Judge()
  118. {
  119.    if(QZAngle<-95&&(fall_flag==0||fall_flag==4))//直立狀態下環島檢測
  120.    {
  121.      if(Deraction%3==1)
  122.      {
  123.        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)//預判斷
  124.         {
  125.                Round_flag_L=1;
  126.                if(Deraction%3==1)
  127.                {
  128.                   Deraction=2;
  129.                }
  130.                else
  131.                {
  132.                   Deraction=0;
  133.                }
  134.         }
  135.      }
  136.      else  if(Deraction%3==0)
  137.      {
  138.         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)//預判斷
  139.         {
  140.                   Round_flag_R=1;
  141.                   Deraction=1;
  142.           }
  143.       }
  144.       else if(Deraction%3==2)
  145.       {
  146.          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)//預判斷
  147.          {
  148.                   Round_flag_L=1;
  149.                   Deraction=0;
  150.           }
  151.       }
  152.       if(AD_right_slope>40&&Round_flag_R==1)//環島入口+(QZAngle+91)*10   Standar_AD>220  60 40
  153.       {
  154.          Round_flag_R=2;
  155.          Counter=-1;
  156.       }
  157.       if(AD_left_slope>0&&Round_flag_L==1&&AD_right_slope==0&&Deraction%3==2)////環島入口 200*(130/g_fRealSpeed）
  158.       {
  159.            Round_flag_L=2;
  160.            Counter=-1;
  161.       }
  162.       if(AD_left_slope>0&&Round_flag_L==1&&Deraction%3==0)//環島入口 200*(130/g_fRealSpeed）
  163.       {
  164.            Round_flag_L=2;
  165.            Counter=-1;
  166.       }
  167.         if(Round_flag_R==2)//右環打角
  168.         {
  169.            error_AD=(g_fLeft_AD*0.24-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.24+0.5*g_fCenter_AD);//差比和獲取偏差
  170.            error_AD=error_AD>55?55:error_AD;//42
  171.            error_AD=error_AD<-55?-55:error_AD;

  173.         }
  174.         if(Round_flag_L==2)//左環打角
  175.         {
  176.            error_AD=(g_fLeft_AD-g_fRight_AD*0.24)*300/(g_fRight_AD*0.24+g_fLeft_AD+0.5*g_fCenter_AD);//差比和獲取偏差
  177.            if(Deraction%3==0)
  178.            {
  179.               //error_AD=error_AD<-40?-40:error_AD;//42
  180.               error_AD=25;//error_AD>40?40:error_AD;
  181.            }
  182.            else
  183.            {
  184.               error_AD=error_AD<-50?-50:error_AD;//42
  185.               error_AD=error_AD>50?50:error_AD;
  186.            }

  188.         }
  189.         if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
  190.         {
  191.             Counter--;
  192.         }
  193.        if(Round_flag_R==0&&Round_L==1) //進入環島電感處理&&Counter>0
  194.        {
  195.          error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和獲取偏差
  196.          error_AD=error_AD<-45?-45:error_AD;//42
  197.          error_AD=error_AD>45?45:error_AD;
  198.        }
  199.        else  if(Round_flag_R==0&&Round_L==2)
  200.        {
  201.          error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和獲取偏差
  202.          error_AD=error_AD<-45?-45:error_AD;//42
  203.          error_AD=error_AD>45?45:error_AD;
  204.        }
  205.        if(Deraction%3==0)
  206.       {
  207.          if(Round_flag_R==0&&Round_L==2) //進入環島電感處理//(1+(Counter-300)/500)
  208.         {
  209.           g_fLeft_AD=400-Counter*1;
  210.          error_AD=(g_fLeft_AD-g_fRight_AD)*100/(g_fLeft_AD+g_fRight_AD+1*g_fCenter_AD);//差比和獲取偏差
  211.          error_AD=error_AD<-45?-45:error_AD;//42
  212.          error_AD=error_AD>45?45:error_AD;
  213.          }
  214.       }
  215.        else if(Round_flag_L==0&&Round_R==1) //進入環島電感處理&&Counter>0
  216.        {
  217.          error_AD=(350-g_fRight_AD)*100/(g_fRight_AD+350+1*g_fCenter_AD);//差比和獲取偏差
  218.          error_AD=error_AD<-45?-45:error_AD;//42
  219.          error_AD=error_AD>45?45:error_AD;
  220.        }
  221.        else if(Round_flag_L==0&&Round_R==2)
  222.        {
  223.          error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和獲取偏差
  224.          error_AD=error_AD<-45?-45:error_AD;//42
  225.          error_AD=error_AD>45?45:error_AD;
  226.        }
  227.    }
  228.    if(fall_flag==2)//三輪狀態下環島檢測
  229.    {
  230.       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)
  231.         {
  232.             if(Deraction%3==0)
  233.             {
  234.               Round_flag_R=1;
  235.               Deraction=1;
  236.             }
  237.             else  if(Deraction==1)
  238.             {
  239.                Round_flag_L=1;
  240.                Deraction=2;
  241.             }
  242.             else if(Deraction==2)
  243.             {
  244.                  Round_flag_L=1;
  245.                  Deraction=0;
  246.             }
  247.         }
  248.         if(Standar_AD>550&&Round_flag_R==1)//環島入口
  249.         {
  250.            Round_flag_R=2;
  251.            Counter=-1;
  252.         }
  253.         if(Standar_AD>535*(150.0/g_fRealSpeed)&&Round_flag_L==1&&Deraction==2)////環島入口
  254.         {
  255.            Round_flag_L=2;
  256.            Counter=-1;
  257.         }
  258.         if(Standar_AD>600&&Round_flag_L==1&&Deraction==0)////環島入口
  259.         {
  260.            Round_flag_L=2;
  261.            Counter=-1;

  263.         }
  264.         if(Round_flag_R==2)//右環打角
  265.         {
  266.            error_AD=(g_fLeft_AD*0.25-g_fRight_AD)*300/(g_fRight_AD+g_fLeft_AD*0.25+0.5*g_fCenter_AD);//差比和獲取偏差
  267.            error_AD=error_AD>(42*g_fRealSpeed/140)?(42*g_fRealSpeed/140):error_AD;//42
  268.            error_AD=error_AD<(-42*g_fRealSpeed/140)?(-42*g_fRealSpeed/140):error_AD;
  269.         }
  270.         if(Round_flag_L==2)//左環打角
  271.         {
  272.            //差比和獲取偏差
  273.           if(Deraction%3==0)
  274.           {
  275.             error_AD=17*(g_fRealSpeed/135.0);//error_AD>40?40:error_AD;
  276.           }
  277.           else
  278.           {
  279.             error_AD=(g_fLeft_AD-g_fRight_AD*0.25)*300/(g_fRight_AD*0.25+g_fLeft_AD+0.5*g_fCenter_AD);
  280.             error_AD=error_AD<-42?-42:error_AD;//42
  281.             error_AD=error_AD>42?42:error_AD;
  282.           }
  283.         }
  284.         if(Round_flag_L==0&&Round_R==1) //進入環島電感處理&&Counter>0
  285.         {
  286.            error_AD=(200-g_fRight_AD)*100/(250+g_fRight_AD+1*g_fCenter_AD);//差比和獲取偏差
  287.            error_AD=error_AD<-42?-42:error_AD;//42
  288.            error_AD=error_AD>42?42:error_AD;
  289.        }
  290.        else if(Round_flag_L==0&&Round_R==2)
  291.        {
  292.            error_AD=(g_fLeft_AD-300)*100/(300+g_fLeft_AD+1*g_fCenter_AD);//差比和獲取偏差
  293.            error_AD=error_AD<-42?-42:error_AD;//42
  294.            error_AD=error_AD>42?42:error_AD;
  295.        }
  296.        if(Round_flag_R==0&&Round_L==1) //進入環島電感處理//(1+(Counter-300)/500)
  297.       {  
  298.         if(Deraction%3==2)
  299.         {
  300.            error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和獲取偏差
  301.            error_AD=error_AD<-45?-45:error_AD;//42
  302.            error_AD=error_AD>45?45:error_AD;
  303.         }
  304.         else
  305.         {
  306.            error_AD=(g_fLeft_AD-250)*100/(250+g_fLeft_AD+1*g_fCenter_AD);//差比和獲取偏差
  307.            error_AD=error_AD<-45?-45:error_AD;//42
  308.            error_AD=error_AD>45?45:error_AD;
  309.         }
  310.       }
  311.       if(Round_flag_R==0&&Round_L==2) //進入環島電感處理//(1+(Counter-300)/500)
  312.       {
  313.          error_AD=(300-g_fRight_AD)*100/(300+g_fRight_AD+1*g_fCenter_AD);//差比和獲取偏差
  314.          error_AD=error_AD<-45?-45:error_AD;//42
  315.          error_AD=error_AD>45?45:error_AD;
  316.       }
  317. //      if(Round_flag_R==0&&Round_L==2) //進入環島電感處理//(1+(Counter-300)/500)
  318. //      {
  319. //       error_AD=(200-g_fRight_AD)*100/(200+g_fRight_AD+1*g_fCenter_AD);//差比和獲取偏差
  320. //       error_AD=error_AD<-45?-45:error_AD;//42
  321. //       error_AD=error_AD>45?45:error_AD;
  322. //       }


  325.    }
  326.    if(Round_flag_R==0&&Round_flag_L==0&&Counter>0)
  327.    {
  328.       Counter--;
  329.    }
  330.    if((Round_R==1||Round_L==1||Round_L==2||Round_R==2)&&Counter==0)//變量清零
  331.    {
  332.       Round_R=0;
  333.       Round_L=0;
  334.    }
  335. }
  336. /**
  337. * @file                方向外環
  338. * @note                      三電感
  339. * @brief                山外K60平臺
  340. * @author                電子信息工程學院 李科磊
  341. * @date                2019年3月22日
  342. */
  343. void DerectError_Get(void)  //外環
  344. {
  345.   Get_AD();
  346.   if(obstacl_Delay<300)
  347.   {
  348.      Round_Judge();
  349.   }


  352.   if(fall_flag==2)
  353.   {
  354.      target_vel_z=(5.7)*error_AD+4.5*(error_AD-last_error_AD);
  355.      target_vel_z=target_vel_z>240?240:target_vel_z;
  356.      target_vel_z=target_vel_z<-240?-240:target_vel_z;
  357.   }
  358.   else
  359.   {
  360.      target_vel_z=(5.0)*error_AD+3.0*(error_AD-last_error_AD);
  361.      target_vel_z=target_vel_z>250?250:target_vel_z;
  362.      target_vel_z=target_vel_z<-250?-250:target_vel_z;
  363.   }
  364.   last_error_AD=error_AD;
  365. }
  366. /**
  367. * @file                方向內環
  368. * @note                      串級
  369. * @brief                山外K60
  370. * @author                沈陽航空航天大學 天牛座
  371. * @date                2019年3月22日
  372. */
  373. void Direction_out(void)
  374. {
  375.   float error;
  376.   static  float last_error;
  377.   float price_Differention;
  378.   float price_Proportion;
  379.   static float Pre1_Error[4];
  380.   //error = target_vel_z*13*(-89/QZAngle) - GYRO_Speed_Z*0.25*(-100/QZAngle);//轉向角度串方向
  381.   error = target_vel_z*13.0 - GYRO_Speed_Z*0.25;
  382.   price_Proportion=g_priceControl_P_in * error;
  383.   price_Differention=g_priceControl_D_in * (error - last_error);
  384.   if(fall_flag==2)
  385.   {
  386.      error = target_vel_z*13.1 - GYRO_Speed_Z*0.245;//轉向角度串方向
  387.      price_Proportion=g_priceControl_P_in * error;
  388.      price_Differention=g_priceControl_D_in * (error - last_error)*1.1;
  389.   }
  390.   Direct_Parameter =price_Proportion  + price_Differention ;//(1+error*error*0.000000001f)

  392.   last_error =  error;
  393.   Pre1_Error[3]=Pre1_Error[2];
  394.   Pre1_Error[2]=Pre1_Error[1];
  395.   Pre1_Error[1]=Pre1_Error[0];
  396.   Pre1_Error[0]=Direct_Parameter;
  397.   Direct_Parameter=Pre1_Error[0]*0.8+Pre1_Error[1]*0.1+Pre1_Error[2]*0.06+Pre1_Error[3]*0.02 ;
  398.   price_PWM=Direct_Parameter;
  399.   price_PWM=price_PWM>550?550:price_PWM;
  400.   price_PWM=price_PWM<-550?-550:price_PWM;
  401. }
復制代碼


評分

參與人數 1黑幣 +50 收起 理由
admin + 50 共享資料的黑幣獎勵！

查看全部評分

分享到:  QQ好友和群QQ好友和群 QQ空間QQ空間 騰訊微博騰訊微博 騰訊朋友騰訊朋友
收藏收藏1 分享淘帖 頂頂 踩踩
回復

使用道具 舉報

您需要登錄后才可以回帖 登錄 | 立即注冊

本版積分規則

手機版|小黑屋|51黑電子論壇 |51黑電子論壇6群 QQ 管理員QQ:125739409;技術交流QQ群281945664

Powered by 單片機教程網

快速回復 返回頂部 返回列表
主站蜘蛛池模板： 欧美老妇交乱视频 | 日韩欧美三级电影 | 欧美a在线观看 | 国产精品毛片一区二区三区 | 成人激情视频在线播放 | 日本久久久影视 | 在线欧美亚洲 | 国产视频观看 | 国产96色在线 | 婷婷福利 | 精品久久久久久 | 国产农村妇女毛片精品久久麻豆 | 久久综合伊人 | 在线观看精品 | 国产精品久久久久不卡 | 国产一区 | www.夜夜骑 | 欧美日韩毛片 | 五月婷婷导航 | 欧美视频偷拍 | 黄网站在线播放 | 亚洲www啪成人一区二区 | 国产成人av电影 | 欧美日韩国产在线观看 | 国产福利视频导航 | 久久精品国产一区 | 国产电影一区二区 | 中文字幕成人av | 在线激情视频 | 国产福利91精品一区二区三区 | 欧美精品一区三区 | av免费网站在线观看 | 精品一二区| 人人干在线 | 国产精品久久久久久一区二区三区 | 精品欧美乱码久久久久久 | 51ⅴ精品国产91久久久久久 | 亚洲高清视频在线观看 | 亚洲一区二区在线 | 国产一级淫片免费视频 | 91精品国产综合久久久久久丝袜 |