标题:
STM32超声波测试程序(卡尔曼滤波)
[打印本页]
作者:
linzi1998
时间:
2019-3-10 20:08
标题:
STM32超声波测试程序(卡尔曼滤波)
最近尝试做出来的超声波测试(卡尔曼滤波)
单片机源程序如下:
#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)
2019-3-11 02:01 上传
点击文件名下载附件
下载积分: 黑币 -5
作者:
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