标题: STM32超声波测试程序（卡尔曼滤波） [打印本页]
作者: linzi1998    时间: 2019-3-10 20:08
标题: STM32超声波测试程序（卡尔曼滤波）
最近尝试做出来的超声波测试（卡尔曼滤波）

单片机源程序如下:
  1. #include "led.h"
  2. #include "delay.h"
  3. #include "sys.h"
  4. #include "math.h"
  5. #include "usart.h"
  6. #include "timer.h"
  7. #include "can.h"
  8. #include "wheelControl.h"
  9. #include "encoder.h"
  10. #include "coordinate.h"
  11. #include "monition_control.h"
  12. #include "pwm.h"
  13. #include "exti.h"
  14. #include "stm32f10x_exti.h"
  15. #include "lidar.h"
  16. #include "spi.h"
  17. #include "24l01.h"
  18. #include "QIANG.h"
  19. #include "text.h"
  20. #include "bsp_rcc.h"
  21. #include "key.h"
  22. #include "chaosheng.h"
  23. #include "lb.h"
  24. int speed=3000;
  25. extern float zygj;    //估计值
  26. extern float sjjc;    //检测值

  28. float s;//码盘偏差纠正
  29. extern coordinitioate_Struct  World_Coordinate_system_Position/*世界坐标系--位置*/
  30.                                                                                   ,World_Coordinate_system_Velocity/*世界坐标系--速度*/
  31.                                                                                   ,Velocity_Coordinate_system/*速度坐标系*/
  32.                                                                                   ,Robot_Coordinate_system/*机器人车身坐标系*/
  33.                                                                                         ,TargetLine_Coordinate_system/*目标线坐标系，相当于在速度坐标系下的位置*/;
  34. float data_tosend[6];
  35. ROBOT_Status_Struct ROBOT_Status;
  36. extern int flag_ea;
  37. extern float targetY;
  38. extern distancel distance_angle;
  39. char start_flag=0;
  40. extern int flag_dingwei;
  41. extern float gyroIntegral_tri;

  43. extern PID_Struct Angle_PID,Abjust_PID;

  45. //extern int32_t motorSpeed_1, motorSpeed_2, motorSpeed_3, motorSpeed_4;

  47. int main(void)
  48. {       
  49.   
  50. //        HSE_SetSysClock(RCC_PLLMul_15);
  51.                  delay_init();                     //延时函数初始化       
  52.                 NVIC_Configuration();          //设置NVIC中断分组2:2位抢占优先级，2位响应优先级                     
  53.                 qigang_init();
  54.                  KEY_Init();
  55.          
  56.           NRF24L01_Init();
  57.                 NRF24L01_TX_Mode();
  58.                 CAN_Configuration();  //can初始化
  59.                 CAN_SetMsg_1(0x110);
  60.                 delay_ms(100);
  61.                 CAN_SetMsg_1(0x210);
  62.                 delay_ms(100);
  63.                 CAN_SetMsg_1(0x310);
  64.                 delay_ms(100);
  65.                 CAN_SetMsg_1(0x410);
  66.                 delay_ms(100);
  67.                 CAN_SetMsg_2(0x111);
  68.                 delay_ms(100);
  69.                 CAN_SetMsg_2(0x211);
  70.                 delay_ms(100);
  71.                 CAN_SetMsg_2(0x311);
  72.                 delay_ms(100);
  73.                 CAN_SetMsg_2(0x411);
  74.                 delay_ms(100);

  76.     usart1_init(115200);
  77.           delay_ms(300);
  78.                 TIM5_Configuration(49,7199);//10Khz的计数频率，计数到5000为500ms
  79.                 TIM3_Cap_Init(0XFFFF,72-1);
  80.        
  81.                 while(!Key_Scan())//等待按键初始化完成
  82.                 {
  83.                 }
  84.           while(Key_Scan())//等待按键按下
  85.                 {
  86.                 }
  87.                
  88.                
  89.                 ROBOT_Status.Target_Angle.angle_reg=0;                //  机器人姿态角不
  90.                 ROBOT_Status.Target_Angle.angle_deg=0;
  91.                 ROBOT_Status.angular_velocity=0;   // 车身运动时姿态角不变
  92.    
  93.        
  94.                 ROBOT_Status.Start_Speed=1000;
  95.                 ROBOT_Status.Max_speed=speed;
  96.                 ROBOT_Status.NewState=ENABLE;
  97.                 ROBOT_Status.Robot_type= 0 ;
  98.                 ROBOT_Status.Target.x=1196.6;
  99.                 ROBOT_Status.Target.y=943.92;
  100.                 ROBOT_Status.Slow_accelerated_speed=3000;  //  减速加速度
  101.                 ROBOT_Status.Speedup_accelerated_speed=5000;
  102.                 ROBOT_Status.Target_Speed=speed;             //  末端速度
  103.                 ROBOT_Status.Stop_length =0;
  104.                 while(Robot_control_line(&ROBOT_Status)>50);
  105.                
  106.                
  107.                 ROBOT_Status.angular_velocity=0;
  108.                 ROBOT_Status.Max_speed=speed;
  109.                 ROBOT_Status.NewState=ENABLE;
  110.                 ROBOT_Status.Robot_type=2;
  111.                 ROBOT_Status.angle_reg_Sum=3.1415926*103.46/180;
  112.                 ROBOT_Status.R=600;
  113.                 ROBOT_Status.Heart.x=825;
  114.                 ROBOT_Status.Heart.y=1415;
  115.                 ROBOT_Status.Slow_accelerated_speed=3000;  //  减速加速度
  116.                 ROBOT_Status.Target_Speed=speed;
  117.                 ROBOT_Status.Stop_length = 10;
  118.                 while(Robot_control_Circle(&ROBOT_Status)>50);
  119.                
  120.                
  121.                
  122.                 ROBOT_Status.Start_Speed=speed;
  123.                 ROBOT_Status.Max_speed=speed;
  124.                 ROBOT_Status.NewState=ENABLE;
  125.                 ROBOT_Status.Robot_type= 0 ;
  126.                 ROBOT_Status.Target.x=464.91;
  127.                 ROBOT_Status.Target.y=2435.07;
  128.                 ROBOT_Status.Slow_accelerated_speed=3000;  //  减速加速度
  129.                 ROBOT_Status.Speedup_accelerated_speed=5000;
  130.                 ROBOT_Status.Target_Speed=speed;             //  末端速度
  131.                 ROBOT_Status.Stop_length =0;
  132.                 while(Robot_control_line(&ROBOT_Status)>50);
  133.                
  134.                
  135.                 ROBOT_Status.angular_velocity=0;
  136.                 ROBOT_Status.Max_speed=speed;
  137.                 ROBOT_Status.NewState=ENABLE;
  138.                 ROBOT_Status.Robot_type=1;
  139.                 ROBOT_Status.angle_reg_Sum=3.1415926*106.24/180;
  140.                 ROBOT_Status.R=600;
  141.                 ROBOT_Status.Heart.x=825;
  142.                 ROBOT_Status.Heart.y=2915;
  143.                 ROBOT_Status.Slow_accelerated_speed=1000;  //  减速加速度
  144.                 ROBOT_Status.Target_Speed=speed;
  145.                 ROBOT_Status.Stop_length = 10;
  146.                 while(Robot_control_Circle(&ROBOT_Status)>20);
  147.                
  148.                
  149.                
  150.                 ROBOT_Status.Start_Speed=speed;
  151.                 ROBOT_Status.Max_speed=speed;
  152.                 ROBOT_Status.NewState=ENABLE;
  153.                 ROBOT_Status.Robot_type= 0 ;
  154.                 ROBOT_Status.Target.x=1185;
  155.                 ROBOT_Status.Target.y=3935;
  156.                 ROBOT_Status.Slow_accelerated_speed=3000;  //  减速加速度
  157.                 ROBOT_Status.Speedup_accelerated_speed=5000;
  158.                 ROBOT_Status.Target_Speed=speed;             //  末端速度
  159.                 ROBOT_Status.Stop_length =10;
  160.                 while(Robot_control_line(&ROBOT_Status)>20);
  161.                
  162.                 ROBOT_Status.angular_velocity=0;
  163.                 ROBOT_Status.Max_speed=speed;
  164.                 ROBOT_Status.NewState=ENABLE;
  165.                 ROBOT_Status.Robot_type=2;
  166.                 ROBOT_Status.angle_reg_Sum=3.1415926*127/180;
  167.                 ROBOT_Status.R=600;
  168.                 ROBOT_Status.Heart.x=825;
  169.                 ROBOT_Status.Heart.y=4415;
  170.                 ROBOT_Status.Slow_accelerated_speed=3000;  //  减速加速度
  171.                 ROBOT_Status.Target_Speed=speed;
  172.                 ROBOT_Status.Stop_length = 10;
  173.                 while(Robot_control_Circle(&ROBOT_Status)>50);
  174.                
  175.                 Abjust_PID.KP=5;
  176.                 Angle_PID.KP=10;
  177.                
  178.                 ROBOT_Status.angular_velocity=0;
  179.                 ROBOT_Status.Max_speed=speed;
  180.                 ROBOT_Status.NewState=ENABLE;
  181.                 ROBOT_Status.Robot_type=1;
  182.                 ROBOT_Status.angle_reg_Sum=3.1415926*83/180;
  183.                 ROBOT_Status.R=170.0840;
  184.                 ROBOT_Status.Heart.x=995.0840;
  185.                 ROBOT_Status.Heart.y=5170.0000;
  186.                 ROBOT_Status.Slow_accelerated_speed=1000;  //  减速加速度
  187.                 ROBOT_Status.Target_Speed=0;
  188.                 ROBOT_Status.Stop_length = 10;
  189.                 while(Robot_control_Circle(&ROBOT_Status)>20)
  190.                 {
  191.                    delay_ms(500);
  192.                   
  193.                        
  194.                         break;
  195.                 }


  198.                 while(1)
  199.                 {
  200.                 if(zygj>850)
  201.                 {
  202.                   CAN_SetMsg(-500,0x114);
  203.          
  204.             CAN_SetMsg(-500,0x214);
  205.          
  206.             CAN_SetMsg(500,0x314);
  207.        
  208.             CAN_SetMsg((int32_t) 500/17.8*12,0x414);
  209.                 }
  210.                 else if(zygj<800)
  211.                 {
  212.                    CAN_SetMsg(500,0x114);
  213.          
  214.             CAN_SetMsg(500,0x214);
  215.          
  216.             CAN_SetMsg(-500,0x314);
  217.        
  218.             CAN_SetMsg((int32_t)-500/17.8*12,0x414);
  219.                 }
  220.                 else if(zygj<850&zygj>800)
  221.                 {
  222.                   break;
  223.                 }
  224.         }
  225.           ROBOT_Status.Start_Speed=1000;
  226.                 ROBOT_Status.Max_speed=speed;
  227.                 ROBOT_Status.NewState=ENABLE;
  228.                 ROBOT_Status.Robot_type= 0 ;
  229.                 ROBOT_Status.Target.x=750;
  230.                 ROBOT_Status.Target.y=7920;
  231.                 ROBOT_Status.Slow_accelerated_speed=300;  //  减速加速度
  232.                 ROBOT_Status.Speedup_accelerated_speed=1000;
  233.                 ROBOT_Status.Target_Speed=speed;             //  末端速度
  234.                 ROBOT_Status.Stop_length =0;
  235.                 while(Robot_control_line(&ROBOT_Status)>5);
  236.                
  237.          
  238.                
  239.                 //过桥后
  240.                 Angle_PID.KP=10;
  241.                 Angle_PID.KD=0;
  242.                
  243.                 ROBOT_Status.angular_velocity=0;
  244.                 ROBOT_Status.Max_speed=speed;
  245.                 ROBOT_Status.NewState=ENABLE;
  246.                 ROBOT_Status.Robot_type=1;
  247.                 ROBOT_Status.angle_reg_Sum=3.1415926*85/180;
  248.                 ROBOT_Status.R=585.0000;
  249.                 ROBOT_Status.Heart.x=1360;//775+585=1360
  250.                 ROBOT_Status.Heart.y=7920;
  251.                 ROBOT_Status.Slow_accelerated_speed=3000;  //  减速加速度
  252.                 ROBOT_Status.Target_Speed=speed;
  253.                 ROBOT_Status.Stop_length = 10;
  254.                 while(Robot_control_Circle(&ROBOT_Status)>20);
  255.                
  256.                 ROBOT_Status.Start_Speed=speed;
  257.                 ROBOT_Status.Max_speed=speed;
  258.                 ROBOT_Status.NewState=ENABLE;
  259.                 ROBOT_Status.Robot_type= 0 ;
  260.                 ROBOT_Status.Target.x=3963.8097;
  261.                 ROBOT_Status.Target.y=8505;
  262.                 ROBOT_Status.Slow_accelerated_speed=500;  //  减速加速度
  263.                 ROBOT_Status.Speedup_accelerated_speed=1000;
  264.                 ROBOT_Status.Target_Speed=1500;             //  末端速度
  265.                 ROBOT_Status.Stop_length =0;
  266.                 while(Robot_control_line(&ROBOT_Status)>20);
  267.                
  268.                 ROBOT_Status.angular_velocity=0;
  269.                 ROBOT_Status.Max_speed=1500;
  270.                 ROBOT_Status.NewState=ENABLE;
  271.                 ROBOT_Status.Robot_type=1;
  272.                 ROBOT_Status.angle_reg_Sum=3.1415926*90/180;
  273.                 ROBOT_Status.R=510.0000;
  274.                 ROBOT_Status.Heart.x=3964.0000;
  275.                 ROBOT_Status.Heart.y=8020.0000;
  276.                 ROBOT_Status.Slow_accelerated_speed=500;  //  减速加速度
  277.                 ROBOT_Status.Target_Speed=700;
  278.                 ROBOT_Status.Stop_length = 10;
  279.                 while(Robot_control_Circle(&ROBOT_Status))
  280.                 {
  281.                   if(lb_level!=1)
  282.                                 break;
  283.                 }
  284.                
  285.                 //交接令牌
  286.                
  287.                 Angle_PID.KP=0;
  288.                 Angle_PID.KI=0;
  289.                 Angle_PID.KD=0;
  290.                
  291.                 Abjust_PID.KP=1;
  292.                 Abjust_PID.KI=0;
  293.                 Abjust_PID.KD=0;
  294.        
  295.                 while(1)
  296.                 {
  297.                 if(lb_level==1)
  298.                 {
  299.           CAN_SetMsg(-500,0x114);
  300.          
  301.           CAN_SetMsg(        500,0x214);
  302.          
  303.           CAN_SetMsg(500,0x314);
  304.        
  305.           CAN_SetMsg((int32_t) -500/17.8*12,0x414);
  306.                
  307.                 }
  308.                 else
  309.                 {
  310.                        
  311.                 ROBOT_Status.Start_Speed=700;
  312.                 ROBOT_Status.Max_speed=700;
  313.                 ROBOT_Status.NewState=ENABLE;
  314.                 ROBOT_Status.Robot_type= 0 ;
  315.                 ROBOT_Status.Target.x=7450;
  316.                 ROBOT_Status.Target.y=8020.0000-20;
  317.                 ROBOT_Status.Slow_accelerated_speed=500;  //  减速加速度
  318.                 ROBOT_Status.Speedup_accelerated_speed=1000;
  319.                 ROBOT_Status.Target_Speed=0;             //  末端速度
  320.                 ROBOT_Status.Stop_length =0;
  321.                 while(Robot_control_line(&ROBOT_Status))
  322.                 {
  323.                   if(lb_level==1)
  324.                                 break;
  325.                 }
  326.                
  327.                 }
  328.                

  330.         }
  331.         }
  332.                
  333.                
  334.                
复制代码

所有资料51hei提供下载:
超声波测试1.3(卡尔曼滤波).7z (238.41 KB, 下载次数: 43)


作者: admin    时间: 2019-3-11 02:01
本帖需要重新编辑补全电路原理图,源码,详细说明与图片即可获得100+黑币(帖子下方有编辑按钮)
作者: DBGB123    时间: 2019-3-11 11:07
楼主：不给原理图大家怎么玩呀
作者: LHM    时间: 2019-7-31 20:47
float R = MeasureNoise_R;    // R:测量噪声，R增大，动态响应变慢，收敛稳定性变好
float Q = ProcessNiose_Q;    // Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
大佬这些值是怎么给的，设定这些值有什么要求啊，是怎么算出来的




欢迎光临 (http://www.51hei.com/bbs/) Powered by Discuz! X3.1