STM32超聲波測試程序（卡爾曼濾波）

ID:488090 · 發表于 2019-3-10 20:08

最近嘗試做出來的超聲波測試（卡爾曼濾波）

單片機源程序如下:

#include "led.h"
#include "delay.h"
#include "sys.h"
#include "math.h"
#include "usart.h"
#include "timer.h"
#include "can.h"
#include "wheelControl.h"
#include "encoder.h"
#include "coordinate.h"
#include "monition_control.h"
#include "pwm.h"
#include "exti.h"
#include "stm32f10x_exti.h"
#include "lidar.h"
#include "spi.h"
#include "24l01.h"
#include "QIANG.h"
#include "text.h"
#include "bsp_rcc.h"
#include "key.h"
#include "chaosheng.h"
#include "lb.h"
int speed=3000;
extern float zygj; //估計值
extern float sjjc; //檢測值
float s;//碼盤偏差糾正
extern coordinitioate_Struct World_Coordinate_system_Position/*世界坐標系--位置*/
,World_Coordinate_system_Velocity/*世界坐標系--速度*/
,Velocity_Coordinate_system/*速度坐標系*/
,Robot_Coordinate_system/*機器人車身坐標系*/
,TargetLine_Coordinate_system/*目標線坐標系，相當于在速度坐標系下的位置*/;
float data_tosend[6];
ROBOT_Status_Struct ROBOT_Status;
extern int flag_ea;
extern float targetY;
extern distancel distance_angle;
char start_flag=0;
extern int flag_dingwei;
extern float gyroIntegral_tri;
extern PID_Struct Angle_PID,Abjust_PID;
//extern int32_t motorSpeed_1, motorSpeed_2, motorSpeed_3, motorSpeed_4;
int main(void)
{
// HSE_SetSysClock(RCC_PLLMul_15);
delay_init(); //延時函數初始化
NVIC_Configuration(); //設置NVIC中斷分組2:2位搶占優先級，2位響應優先級
qigang_init();
KEY_Init();
NRF24L01_Init();
NRF24L01_TX_Mode();
CAN_Configuration(); //can初始化
CAN_SetMsg_1(0x110);
delay_ms(100);
CAN_SetMsg_1(0x210);
delay_ms(100);
CAN_SetMsg_1(0x310);
delay_ms(100);
CAN_SetMsg_1(0x410);
delay_ms(100);
CAN_SetMsg_2(0x111);
delay_ms(100);
CAN_SetMsg_2(0x211);
delay_ms(100);
CAN_SetMsg_2(0x311);
delay_ms(100);
CAN_SetMsg_2(0x411);
delay_ms(100);
usart1_init(115200);
delay_ms(300);
TIM5_Configuration(49,7199);//10Khz的計數頻率，計數到5000為500ms
TIM3_Cap_Init(0XFFFF,72-1);
while(!Key_Scan())//等待按鍵初始化完成
{
}
while(Key_Scan())//等待按鍵按下
{
}
ROBOT_Status.Target_Angle.angle_reg=0; // 機器人姿態角不
ROBOT_Status.Target_Angle.angle_deg=0;
ROBOT_Status.angular_velocity=0; // 車身運動時姿態角不變
ROBOT_Status.Start_Speed=1000;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type= 0 ;
ROBOT_Status.Target.x=1196.6;
ROBOT_Status.Target.y=943.92;
ROBOT_Status.Slow_accelerated_speed=3000; // 減速加速度
ROBOT_Status.Speedup_accelerated_speed=5000;
ROBOT_Status.Target_Speed=speed; // 末端速度
ROBOT_Status.Stop_length =0;
while(Robot_control_line(&ROBOT_Status)>50);
ROBOT_Status.angular_velocity=0;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type=2;
ROBOT_Status.angle_reg_Sum=3.1415926*103.46/180;
ROBOT_Status.R=600;
ROBOT_Status.Heart.x=825;
ROBOT_Status.Heart.y=1415;
ROBOT_Status.Slow_accelerated_speed=3000; // 減速加速度
ROBOT_Status.Target_Speed=speed;
ROBOT_Status.Stop_length = 10;
while(Robot_control_Circle(&ROBOT_Status)>50);
ROBOT_Status.Start_Speed=speed;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type= 0 ;
ROBOT_Status.Target.x=464.91;
ROBOT_Status.Target.y=2435.07;
ROBOT_Status.Slow_accelerated_speed=3000; // 減速加速度
ROBOT_Status.Speedup_accelerated_speed=5000;
ROBOT_Status.Target_Speed=speed; // 末端速度
ROBOT_Status.Stop_length =0;
while(Robot_control_line(&ROBOT_Status)>50);
ROBOT_Status.angular_velocity=0;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type=1;
ROBOT_Status.angle_reg_Sum=3.1415926*106.24/180;
ROBOT_Status.R=600;
ROBOT_Status.Heart.x=825;
ROBOT_Status.Heart.y=2915;
ROBOT_Status.Slow_accelerated_speed=1000; // 減速加速度
ROBOT_Status.Target_Speed=speed;
ROBOT_Status.Stop_length = 10;
while(Robot_control_Circle(&ROBOT_Status)>20);
ROBOT_Status.Start_Speed=speed;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type= 0 ;
ROBOT_Status.Target.x=1185;
ROBOT_Status.Target.y=3935;
ROBOT_Status.Slow_accelerated_speed=3000; // 減速加速度
ROBOT_Status.Speedup_accelerated_speed=5000;
ROBOT_Status.Target_Speed=speed; // 末端速度
ROBOT_Status.Stop_length =10;
while(Robot_control_line(&ROBOT_Status)>20);
ROBOT_Status.angular_velocity=0;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type=2;
ROBOT_Status.angle_reg_Sum=3.1415926*127/180;
ROBOT_Status.R=600;
ROBOT_Status.Heart.x=825;
ROBOT_Status.Heart.y=4415;
ROBOT_Status.Slow_accelerated_speed=3000; // 減速加速度
ROBOT_Status.Target_Speed=speed;
ROBOT_Status.Stop_length = 10;
while(Robot_control_Circle(&ROBOT_Status)>50);
Abjust_PID.KP=5;
Angle_PID.KP=10;
ROBOT_Status.angular_velocity=0;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type=1;
ROBOT_Status.angle_reg_Sum=3.1415926*83/180;
ROBOT_Status.R=170.0840;
ROBOT_Status.Heart.x=995.0840;
ROBOT_Status.Heart.y=5170.0000;
ROBOT_Status.Slow_accelerated_speed=1000; // 減速加速度
ROBOT_Status.Target_Speed=0;
ROBOT_Status.Stop_length = 10;
while(Robot_control_Circle(&ROBOT_Status)>20)
{
delay_ms(500);
break;
}
while(1)
{
if(zygj>850)
{
CAN_SetMsg(-500,0x114);
CAN_SetMsg(-500,0x214);
CAN_SetMsg(500,0x314);
CAN_SetMsg((int32_t) 500/17.8*12,0x414);
}
else if(zygj<800)
{
CAN_SetMsg(500,0x114);
CAN_SetMsg(500,0x214);
CAN_SetMsg(-500,0x314);
CAN_SetMsg((int32_t)-500/17.8*12,0x414);
}
else if(zygj<850&zygj>800)
{
break;
}
}
ROBOT_Status.Start_Speed=1000;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type= 0 ;
ROBOT_Status.Target.x=750;
ROBOT_Status.Target.y=7920;
ROBOT_Status.Slow_accelerated_speed=300; // 減速加速度
ROBOT_Status.Speedup_accelerated_speed=1000;
ROBOT_Status.Target_Speed=speed; // 末端速度
ROBOT_Status.Stop_length =0;
while(Robot_control_line(&ROBOT_Status)>5);
//過橋后
Angle_PID.KP=10;
Angle_PID.KD=0;
ROBOT_Status.angular_velocity=0;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type=1;
ROBOT_Status.angle_reg_Sum=3.1415926*85/180;
ROBOT_Status.R=585.0000;
ROBOT_Status.Heart.x=1360;//775+585=1360
ROBOT_Status.Heart.y=7920;
ROBOT_Status.Slow_accelerated_speed=3000; // 減速加速度
ROBOT_Status.Target_Speed=speed;
ROBOT_Status.Stop_length = 10;
while(Robot_control_Circle(&ROBOT_Status)>20);
ROBOT_Status.Start_Speed=speed;
ROBOT_Status.Max_speed=speed;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type= 0 ;
ROBOT_Status.Target.x=3963.8097;
ROBOT_Status.Target.y=8505;
ROBOT_Status.Slow_accelerated_speed=500; // 減速加速度
ROBOT_Status.Speedup_accelerated_speed=1000;
ROBOT_Status.Target_Speed=1500; // 末端速度
ROBOT_Status.Stop_length =0;
while(Robot_control_line(&ROBOT_Status)>20);
ROBOT_Status.angular_velocity=0;
ROBOT_Status.Max_speed=1500;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type=1;
ROBOT_Status.angle_reg_Sum=3.1415926*90/180;
ROBOT_Status.R=510.0000;
ROBOT_Status.Heart.x=3964.0000;
ROBOT_Status.Heart.y=8020.0000;
ROBOT_Status.Slow_accelerated_speed=500; // 減速加速度
ROBOT_Status.Target_Speed=700;
ROBOT_Status.Stop_length = 10;
while(Robot_control_Circle(&ROBOT_Status))
{
if(lb_level!=1)
break;
}
//交接令牌
Angle_PID.KP=0;
Angle_PID.KI=0;
Angle_PID.KD=0;
Abjust_PID.KP=1;
Abjust_PID.KI=0;
Abjust_PID.KD=0;
while(1)
{
if(lb_level==1)
{
CAN_SetMsg(-500,0x114);
CAN_SetMsg( 500,0x214);
CAN_SetMsg(500,0x314);
CAN_SetMsg((int32_t) -500/17.8*12,0x414);
}
else
{
ROBOT_Status.Start_Speed=700;
ROBOT_Status.Max_speed=700;
ROBOT_Status.NewState=ENABLE;
ROBOT_Status.Robot_type= 0 ;
ROBOT_Status.Target.x=7450;
ROBOT_Status.Target.y=8020.0000-20;
ROBOT_Status.Slow_accelerated_speed=500; // 減速加速度
ROBOT_Status.Speedup_accelerated_speed=1000;
ROBOT_Status.Target_Speed=0; // 末端速度
ROBOT_Status.Stop_length =0;
while(Robot_control_line(&ROBOT_Status))
{
if(lb_level==1)
break;
}
}
}
}

復制代碼

所有資料51hei提供下載:

超聲波測試1.3(卡爾曼濾波).7z (238.41 KB, 下載次數: 43)

ID:1 · 發表于 2019-3-11 02:01

本帖需要重新編輯補全電路原理圖,源碼,詳細說明與圖片即可獲得100+黑幣(帖子下方有編輯按鈕)

ID:487397 · 發表于 2019-3-11 11:07

樓主：不給原理圖大家怎么玩呀

ID:334435 · 發表于 2019-7-31 20:47

float R = MeasureNoise_R; // R:測量噪聲，R增大，動態響應變慢，收斂穩定性變好
float Q = ProcessNiose_Q; // Q:過程噪聲，Q增大，動態響應變快，收斂穩定性變壞
大佬這些值是怎么給的，設定這些值有什么要求啊，是怎么算出來的

帳號		自動登錄	找回密碼
密碼			立即注冊

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

STM32超聲波測試程序（卡爾曼濾波）