BNO055 使用三个三轴传感器同时测量切向加速度（通过加速度计），旋转加速度（通过陀螺仪）和局部磁场强度（通过磁力计）。然后，可以将数据发送到外部微处理器，或使用运行专有融合算法的 M0+微处理器在传感器内部进行分析。然后，用户可以选择以各种格式从传感器请求数据。
该芯片还具有中断功能，可以在发生某些运动（方向变化、突然加速等）时通知主机微控制器。
博世传感器BNO055智能9轴的绝对方向传感器是在封装（SiP）一个系统，整合了三轴14位加速度计，三轴16位陀螺仪与一系列每秒±2000度，三轴地磁传感器和一个32比特的ARM Cortex M0 +微控制器上运行博世传感器传感器融合软件，在单一封装中。相应的芯片集被集成到一个单一的28引脚的LGA3.8毫米点x5.2毫米点x1.1毫米壳体。为了获得最佳的系统集成BNO055配有数字双向I²C和UART接口。该I²C接口进行编程与HID-I²C协议打开BNO055到插件和播放传感器集线器解决方案运行Windows 8.0或8.1的操作系统的设备上运行。主要特征:输出融合传感器数据先进的三轴16位陀螺仪LGA封装28个引脚低功耗，并暂停提供模式电压范围：2.4V至3.6V工作温度：-40°C至+ 85°C加速度计特点:加速度范围：±2G /±4G /±8G /±16G低通滤波器带宽：1kHz时 - <8HZ陀螺仪功能:范围切换从±125°/ s至±2000°/秒低通滤波器带宽：523Hz - 12赫兹磁力特性磁场范围：±典型1300μT（X，Y轴）;±2500μT（z轴）磁场分辨率：〜0.3μT
两个定时器使用8个PWM通道驱动4个电机驱动芯片，使用PWM占空比调节输出电压和正反转。
/**
******************************************************************************
* File Name : TIM.h
* Description : This file provides code for the configuration
* of the TIM instances.
******************************************************************************
** This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* COPYRIGHT(c) 2018 STMicroelectronics
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __tim_H
#define __tim_H
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "stm32f2xx_hal.h"
#include "comtypes.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
#define bool BOOLEAN
extern TIM_HandleTypeDef htim3;
extern TIM_HandleTypeDef htim4;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_TIM3_Init(void);
void MX_TIM4_Init(void);
void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /*__ tim_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
/**
******************************************************************************
* File Name : TIM.c
* Description : This file provides code for the configuration
* of the TIM instances.
******************************************************************************
** This notice applies to any and all portions of this file
* that are not between comment pairs USER CODE BEGIN and
* USER CODE END. Other portions of this file, whether
* inserted by the user or by software development tools
* are owned by their respective copyright owners.
*
* COPYRIGHT(c) 2018 STMicroelectronics
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted provided that the following conditions are met:
* 1. Redistributions of source code must retain the above copyright notice,
* this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright notice,
* this list of conditions and the following disclaimer in the documentation
* and/or other materials provided with the distribution.
* 3. Neither the name of STMicroelectronics nor the names of its contributors
* may be used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
******************************************************************************
*/
/* Includes ------------------------------------------------------------------*/
#include "tim.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
TIM_HandleTypeDef htim3;
TIM_HandleTypeDef htim4;
static void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
/* USER CODE END Error_Handler_Debug */
while(1);
}
/* TIM3 init function */
void MX_TIM3_Init(void)
{
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
htim3.Instance = TIM3;
htim3.Init.Prescaler = 2-1;
htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
htim3.Init.Period = 5040-1;
htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
{
Error_Handler();
}
HAL_TIM_MspPostInit(&htim3);
}
/* TIM4 init function */
void MX_TIM4_Init(void)
{
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
htim4.Instance = TIM4;
htim4.Init.Prescaler = 2-1;
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
htim4.Init.Period = 5040-1;
htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
{
Error_Handler();
}
HAL_TIM_MspPostInit(&htim4);
}
void HAL_TIM_PWM_MspInit(TIM_HandleTypeDef* tim_pwmHandle)
{
if(tim_pwmHandle->Instance==TIM3)
{
__HAL_RCC_TIM3_CLK_ENABLE();
}
else if(tim_pwmHandle->Instance==TIM4)
{
__HAL_RCC_TIM4_CLK_ENABLE();
}
}
void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
if(timHandle->Instance==TIM3)
{
/* USER CODE BEGIN TIM3_MspPostInit 0 */
/* USER CODE END TIM3_MspPostInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
/**TIM3 GPIO Configuration
PB1 ------> TIM3_CH4
PC6 ------> TIM3_CH1
PC7 ------> TIM3_CH2
PC8 ------> TIM3_CH3
*/
GPIO_InitStruct.Pin = GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/* USER CODE BEGIN TIM3_MspPostInit 1 */
/* USER CODE END TIM3_MspPostInit 1 */
}
else if(timHandle->Instance==TIM4)
{
/* USER CODE BEGIN TIM4_MspPostInit 0 */
/* USER CODE END TIM4_MspPostInit 0 */
__HAL_RCC_GPIOB_CLK_ENABLE();
/**TIM4 GPIO Configuration
PB6 ------> TIM4_CH1
PB7 ------> TIM4_CH2
PB8 ------> TIM4_CH3
PB9 ------> TIM4_CH4
*/
GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_7|GPIO_PIN_8|GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN TIM4_MspPostInit 1 */
/* USER CODE END TIM4_MspPostInit 1 */
}
}
void HAL_TIM_PWM_MspDeInit(TIM_HandleTypeDef* tim_pwmHandle)
{
if(tim_pwmHandle->Instance==TIM3)
{
__HAL_RCC_TIM3_CLK_DISABLE();
}
else if(tim_pwmHandle->Instance==TIM4)
{
__HAL_RCC_TIM4_CLK_DISABLE();
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
BNO055寄存器读写配置驱动。
#ifndef __BMO_055_H
#define __BMO_055_H
#include "mpuiic.h"
#include "stm32f2xx_hal.h"
#define BNO055_I2C_ADDR2 (0X29)
#define SYS_TRIGGER (0X3F)
//BNO055电源模式
#define PWR_MODE (0X3E)
#define NORMAL_MODE (0X00)
#define LOW_POWER_MODE (0X01)
#define SUSPEND_MODE (0X02)
//传感器切换模式
#define OPR_MODE (0X3D)
//传感器配置模式
#define CONFIG_MODE (0X00)
//NON_FUSION_MODE
#define ACCONLY (0X01)
#define MAGONLY (0X02)
#define GRYOONOL (0X03)
#define ACCMAG (0X04)
#define ACCGYRO (0X05)
#define MAGGYRO (0X06)
#define AMG (0X07)
//FUSION MODE
#define IMU (0X80)
#define COMPASS (0X90)
#define M4G (0X0A)
#define NODF_FMC_OFF (0X0B)
#define NODF (0x0C)
//传感器校准状态查询
#define CALIB_STAT (0X35)
//传感器温度数据地址
#define TEMP (0X34)
//传感器数据地址
#define ACC_DATA_X_LSB (0X08)
#define ACC_DATA_X_MSB (0X09)
#define ACC_DATA_Y_LSB (0X0A)
#define ACC_DATA_Y_MSB (0X0B)
#define ACC_DATA_Z_LSB (0X0C)
#define ACC_DATA_Z_MSB (0X0D)
#define MAG_DATA_X_LSB (0X0E)
#define MAG_DATA_X_MSB (0X0F)
#define MAG_DATA_Y_LSB (0X10)
#define MAG_DATA_Y_MSB (0X11)
#define MAG_DATA_Z_LSB (0X12)
#define MAG_DATA_Z_MSB (0X13)
#define GYR_DATA_X_LSB (0X14)
#define GYR_DATA_X_MSB (0X15)
#define GYR_DATA_Y_LSB (0X16)
#define GYR_DATA_Y_MSB (0X17)
#define GYR_DATA_Z_LSB (0X18)
#define GYR_DATA_Z_MSB (0X19)
#define EUL_Heading_LSB (0X1A)
#define EUL_Heading_MSB (0X1B)
#define EUL_Roll_LSB (0X1C)
#define EUL_Roll_MSB (0X1D)
#define EUL_Pitch_LSB (0X1E)
#define EUL_Pitch_MSB (0X1F)
#define QUA_Data_w_LSB (0X20)
#define QUA_Data_w_MSB (0X21)
#define QUA_Data_x_LSB (0X22)
#define QUA_Data_x_MSB (0X23)
#define QUA_Data_y_LSB (0X24)
#define QUA_Data_y_MSB (0X25)
#define QUA_Data_z_LSB (0X26)
#define QUA_Data_z_MSB (0X27)
#define LIA_Data_X_LSB (0X28)
#define LIA_Data_X_MSB (0X29)
#define LIA_Data_Y_LSB (0X2A)
#define LIA_Data_Y_MSB (0X2B)
#define LIA_Data_Z_LSB (0X2C)
#define LIA_Data_Z_MSB (0X2D)
#define GRV_Data_X_LSB (0X2E)
#define GRV_Data_X_MSB (0X2F)
#define GRV_Data_Y_LSB (0X30)
#define GRV_Data_Y_MSB (0X31)
#define GRV_Data_Z_LSB (0X32)
#define GRV_Data_Z_MSB (0X33)
//传感器配置地址
#define ACC_Config (0x08)
#define MAG_Config (0x09)
#define GYR_Config_0 (0x0A)
#define GYR_Config_1 (0x0B)
#define ACC_Sleep_Config (0x0C)
#define GYR_Sleep_Config (0x0D)
extern uint8_t mag_need_cal_flag;
extern uint8_t acc_need_cal_flag;
extern uint8_t gry_need_cal_flag;
/**********传感器复位*********************
请勿调用该函数。
无返回值
******************************************/
void SET_BNO055_REST(void);
/**********设置传感器模式*********************
k：寄存器数值
请勿调用该函数。
无返回值
******************************************/
void SET_BNO055_OPR_MODE(uint8_t k);
/**********设置传感器电源模式*********************
k：寄存器数值
请勿调用该函数。
无返回值
******************************************/
void SET_BNO055_POWER_MODE(uint8_t k);
/**********输出四元数数据*********************
*q：输出的数据数组
请勿调用该函数。
无返回值
******************************************/
void READ_QUA_FROM_BNO055(float *q);
/**********输出欧拉角数据*********************
*q：输出的数据数组
请勿调用该函数。
无返回值
******************************************/
void READ_EUL_FROM_BNO055(float *q);
/**********输出加速度数据*********************
*q：输出的数据数组
请勿调用该函数。
无返回值
******************************************/
void READ_ACC_FROM_BNO055(float *q);
/**********输出陀螺仪数据*********************
*q：输出的数据数组
请勿调用该函数。
无返回值
******************************************/
void READ_GRY_FROM_BNO055(float *q);
/**********输出磁罗盘数据*********************
*q：输出的数据数组
请勿调用该函数。
无返回值
******************************************/
void READ_MAG_FROM_BNO055(float *q);
/**********加载传感器校准数据*********************
请勿调用该函数。
无返回值
******************************************/
void load_cal_data(void);
/**********获取传感器温度数据*********************
请勿调用该函数。
返回传感器温度值
******************************************/
uint8_t READ_TEMP_BNO055(void);
/**********获取传感器校准状态*********************
允许调用该函数。
返回传感器校准状态值
******************************************/
uint8_t CHECK_CALIB_STAT(void);
/**********获取传感器磁罗盘校准状态*********************
请勿调用该函数。
返回传感器磁罗盘校准状态值
******************************************/
uint8_t CHECK_MAG_CALIB_STAT(void);
/**********获取传感器加速度校准状态*********************
请勿调用该函数。
返回加速度计校准状态值
******************************************/
uint8_t CHECK_ACC_CALIB_STAT(void);
/**********获取传感器陀螺仪校准状态*********************
请勿调用该函数。
返回陀螺仪校准状态值
******************************************/
uint8_t CHECK_GRY_CALIB_STAT(void);
/**********校准传感器*********************
请勿调用该函数。
无返回值
******************************************/
void CALIB_SENSOR(void);
/**********传感器初始化*********************
请勿调用该函数。
无返回值
******************************************/
void BNO055_INIT(void);
/**********输出yaw数据*********************
允许调用该函数。
返回yaw数据
机体启动方向为0度
******************************************/
int32_t Yaw_out(void);
#endif
#include "bmo_055.h"
uint8_t mag_need_cal_flag = 0;
uint8_t acc_need_cal_flag = 0;
uint8_t gry_need_cal_flag = 0;
void bno_055_delay()
{
uint16_t i,k;
for(i = 0; i < 100; i++){
for(k = 0; k < 100; k++);
}
}
//复位BNO055
void SET_BNO055_REST()
{
uint8_t r = 0x20;
WriteData(BNO055_I2C_ADDR2,SYS_TRIGGER,&r,1);
}
//设置BNO055的电源模式
void SET_BNO055_POWER_MODE(uint8_t k)
{
WriteData(BNO055_I2C_ADDR2,PWR_MODE,&k,1);
}
//设置传感器操作模式
void SET_BNO055_OPR_MODE(uint8_t k)
{
WriteData(BNO055_I2C_ADDR2,OPR_MODE,&k,1);
}
//传感器四元数
void READ_QUA_FROM_BNO055(float *q)
{
float w,x,y,z;
uint8_t qua_data[8];
ReadData(BNO055_I2C_ADDR2,QUA_Data_w_LSB,qua_data,8);
w=(int16_t)((int16_t)qua_data[1]<<8|(int16_t)qua_data[0])/16384.0;
x=(int16_t)((int16_t)qua_data[3]<<8|(int16_t)qua_data[2])/16384.0;
y=(int16_t)((int16_t)qua_data[5]<<8|(int16_t)qua_data[4])/16384.0;
z=(int16_t)((int16_t)qua_data[7]<<8|(int16_t)qua_data[6])/16384.0;
q[0] = w;
q[1] = x;
q[2] = y;
q[3] = z;
}
//读取传感器欧拉角,单位度
void READ_EUL_FROM_BNO055(float *q)
{
float pitch,roll,yaw;
uint8_t eul_data[6];
ReadData(BNO055_I2C_ADDR2,EUL_Heading_LSB,eul_data,6);
yaw = (int16_t)((int16_t)eul_data[1]<<8|(int16_t)eul_data[0])/16.0f;
roll = (int16_t)((int16_t)eul_data[3]<<8|(int16_t)eul_data[2])/16.0f;
pitch = (int16_t)((int16_t)eul_data[5]<<8|(int16_t)eul_data[4])/16.0f;
q[0] = pitch;
q[1] = roll;
q[2] = yaw;
}
//读取加速度计数据，单位m/s^2
void READ_ACC_FROM_BNO055(float *q)
{
float acc_x,acc_y,acc_z;
uint8_t acc_data[6];
ReadData(BNO055_I2C_ADDR2,ACC_DATA_X_LSB,acc_data,6);
acc_x =(int16_t)((int16_t)acc_data[1]<<8|(int16_t)acc_data[0])/100.0f;
acc_y =(int16_t)((int16_t)acc_data[3]<<8|(int16_t)acc_data[2])/100.0f;
acc_z =(int16_t)((int16_t)acc_data[5]<<8|(int16_t)acc_data[4])/100.0f;
q[0] = acc_x;
q[1] = acc_y;
q[2] = acc_z;
}
//读取陀螺仪数据，单位DPS
void READ_GRY_FROM_BNO055(float *q)
{
float gry_x,gry_y,gry_z;
uint8_t gry_data[6];
ReadData(BNO055_I2C_ADDR2,GYR_DATA_X_LSB,gry_data,6);
gry_x =(int16_t)((int16_t)gry_data[1]<<8|(int16_t)gry_data[0])/16.0f;
gry_y =(int16_t)((int16_t)gry_data[3]<<8|(int16_t)gry_data[2])/16.0f;
gry_z =(int16_t)((int16_t)gry_data[5]<<8|(int16_t)gry_data[4])/16.0f;
q[0] = gry_x;
q[1] = gry_y;
q[2] = gry_z;
}
//读取磁罗盘数据，单位UT
void READ_MAG_FROM_BNO055(float *q)
{
float mag_x,mag_y,mag_z;
uint8_t mag_data[6];
ReadData(BNO055_I2C_ADDR2,MAG_DATA_X_LSB,mag_data,6);
mag_x =(int16_t)((int16_t)mag_data[1]<<8|(int16_t)mag_data[0])/16.0f;
mag_y =(int16_t)((int16_t)mag_data[3]<<8|(int16_t)mag_data[2])/16.0f;
mag_z =(int16_t)((int16_t)mag_data[5]<<8|(int16_t)mag_data[4])/16.0f;
q[0] = mag_x;
q[1] = mag_y;
q[2] = mag_z;
}
//读取传感器线性加速度数据
void READ_LINER_ACC_FROM_BNO055(float *q)
{
float liner_acc_x,liner_acc_y,liner_acc_z;
uint8_t liner_acc_data[6];
ReadData(BNO055_I2C_ADDR2,LIA_Data_X_LSB,liner_acc_data,6);
liner_acc_x =(int16_t)((int16_t)liner_acc_data[1]<<8|(int16_t)liner_acc_data[0])/100.0f;
liner_acc_y =(int16_t)((int16_t)liner_acc_data[3]<<8|(int16_t)liner_acc_data[2])/100.0f;
liner_acc_z =(int16_t)((int16_t)liner_acc_data[5]<<8|(int16_t)liner_acc_data[4])/100.0f;
q[0] = liner_acc_x;
q[1] = liner_acc_y;
q[2] = liner_acc_z;
}
//读取传感器重力矢量数据
void READ_GRAVITY_VECTOR_FROM_BNO055(float *q)
{
float gv_x,gv_y,gv_z;
uint8_t gv_data[6];
ReadData(BNO055_I2C_ADDR2,GRV_Data_X_LSB,gv_data,6);
gv_x =(int16_t)((int16_t)gv_data[1]<<8|(int16_t)gv_data[0])/100.0f;
gv_y =(int16_t)((int16_t)gv_data[3]<<8|(int16_t)gv_data[2])/100.0f;
gv_z =(int16_t)((int16_t)gv_data[5]<<8|(int16_t)gv_data[4])/100.0f;
q[0] = gv_x;
q[1] = gv_y;
q[2] = gv_z;
}
//检查传感器校准状态
uint8_t CHECK_CALIB_STAT()
{
uint8_t calib_stat;
uint8_t mag_calib_stat;
uint8_t acc_calib_stat;
uint8_t gry_calib_stat;
ReadData(BNO055_I2C_ADDR2,CALIB_STAT ,&calib_stat,1);
mag_calib_stat = calib_stat & 0x03;
acc_calib_stat = (calib_stat & 0x0C) >> 2;
gry_calib_stat = (calib_stat & 0x20) >> 4;
if(mag_calib_stat < 2){
mag_need_cal_flag = 1;
return 1;
}
if(acc_calib_stat<2){
acc_need_cal_flag = 1;
return 1;
}
if(gry_calib_stat<2){
gry_need_cal_flag = 1;
return 1;
}
return 0;
}
uint8_t CHECK_MAG_CALIB_STAT()
{
uint8_t calib_stat;
uint8_t mag_calib_stat;
ReadData(BNO055_I2C_ADDR2,CALIB_STAT ,&calib_stat,1);
mag_calib_stat = calib_stat & 0x03;
if(mag_calib_stat < 2){
mag_need_cal_flag = 1;
return 1;
}
mag_need_cal_flag = 0;
return 0;
}
uint8_t CHECK_ACC_CALIB_STAT()
{
uint8_t calib_stat;
uint8_t acc_calib_stat;
ReadData(BNO055_I2C_ADDR2,CALIB_STAT ,&calib_stat,1);
acc_calib_stat = (calib_stat & 0x0C) >> 2;
if(acc_calib_stat < 2){
acc_need_cal_flag = 1;
return 1;
}
acc_need_cal_flag = 0;
return 0;
}
uint8_t CHECK_GRY_CALIB_STAT()
{
uint8_t calib_stat;
uint8_t gry_calib_stat;
ReadData(BNO055_I2C_ADDR2,CALIB_STAT ,&calib_stat,1);
gry_calib_stat = (calib_stat & 0x20) >> 4;
if(gry_calib_stat < 2){
gry_need_cal_flag = 1;
return 1;
}
gry_need_cal_flag = 0;
return 0;
}
//读取传感器温度数据，单位度
uint8_t READ_TEMP_BNO055()
{
uint8_t t;
ReadData(BNO055_I2C_ADDR2,TEMP,&t,1);
return t;
}
//初始化传感器
void BNO055_INIT()
{
uint8_t acc_config = 0x0F;
uint8_t mag_config = 0x1F;
IIC1_Init(0x30);
HAL_Delay(1000);
SET_BNO055_POWER_MODE(NORMAL_MODE);
WriteData(BNO055_I2C_ADDR2,ACC_Config,&acc_config,1); //配置加速度计量程为+/-16g
WriteData(BNO055_I2C_ADDR2,MAG_Config,&mag_config,1); //配置磁罗盘刷新速度为30hz，
SET_BNO055_OPR_MODE(NODF);
bno_055_delay();
CHECK_CALIB_STAT(); //检测传感器校准状态
}
BNO055 IIC总线驱动。
#ifndef __MPUIIC_H
#define __MPUIIC_H
#include "bmo_055.h"
#include "stm32f2xx_hal.h"
void IIC1_Init(uint8_t Addr);
void BNO055_WriteByte(uint8_t DevID,uint8_t ByteAddr,uint8_t Data);
int BNO055_ReadByte(uint8_t DevID,uint8_t ByteAddr);
void WriteData(uint8_t DevID,uint8_t Addr,uint8_t *Dat,uint8_t Num);
void ReadData(uint8_t DevID,uint8_t Addr,uint8_t *Pbuf,uint8_t Num);
#endif
#include "mpuiic.h"
void IIC1_Init(uint8_t Addr)
{
GPIO_InitTypeDef GPIO_InitStruct;
__HAL_RCC_I2C3_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOC_CLK_ENABLE();
GPIO_InitStruct.Pin = GPIO_PIN_8;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF4_I2C3;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_9;
GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF4_I2C3;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
RCC->APB1ENR |= 2<<21;
RCC->APB1RSTR |= 2<<21;
RCC->APB1RSTR &= ~(2<<21);
I2C3->CR1 |= 1<<15;
I2C3->CR1 &= ~(1<<15);
I2C3->CR2 |= 36 ;
I2C3->CCR &= ~(1<<15);
I2C3->CCR |= 90<<0;
I2C3->TRISE |= 37;
I2C3->CR1 |= 1<<6;
I2C3->CR1 &= ~(1<<1);
I2C3->OAR1 &= ~(1<<15);
I2C3->OAR1 |= 1<<14;
I2C3->OAR1 |= Addr<<1;
I2C3->CR2 |= 1<<8;
I2C3->CR1 |= 1<<0;
HAL_GPIO_WritePin(GPIOC,GPIO_PIN_9,GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOA,GPIO_PIN_8,GPIO_PIN_SET);
}
void BNO055_WriteByte(uint8_t DevID,uint8_t ByteAddr,uint8_t Data)
{
int clear;
clear=clear;
while(I2C3->SR2&=1<<1);
I2C3->CR1 |= 1<<8;
while(!(I2C3->SR1&=1<<0));
I2C3->SR1 &= ~(1<<10);
I2C3->DR = DevID << 1|0;
while(!(I2C3->SR1&=1<<1));
clear=I2C3->SR1;
clear=I2C3->SR2;
I2C3->DR = ByteAddr;
while(!(I2C3->SR1&=1<<2));
clear=I2C3->SR1;
I2C3->DR = Data;
while(!(I2C3->SR1&=1<<2));
I2C3->CR1 |= 1<<9;
}
int BNO055_ReadByte(uint8_t DevID,uint8_t ByteAddr)
{
int clear;
clear=clear;
while(I2C3->SR2&=1<<1);
I2C3->CR1 |= 1<<8;
while(!(I2C3->SR1&=1<<0));
I2C3->SR1 &= ~(1<<10);
I2C3->DR = DevID <<1|0;
while(!(I2C3->SR1&=1<<1));
clear=I2C3->SR1;
clear=I2C3->SR2;
I2C3->DR = ByteAddr;
I2C3->CR1 |= 1<<8;
while(!(I2C3->SR1&=1<<0));
I2C3->SR1 &= ~(1<<10);
I2C3->DR = DevID <<1|1;
while(!(I2C3->SR1&=1<<1));
clear=I2C3->SR1;
clear=I2C3->SR2;
while(!(I2C3->SR1&=1<<6));
I2C3->CR1 |= 1<<9;
return I2C3->DR;
}
void I2C3_ER_IRQHandler(void)
{
if(I2C3->SR1 & 1<<10)
{
I2C3->SR1 &=~(1<<10);
}
if(I2C3->SR1 & 1<<14)
{
I2C3->SR1 &=~(1<<14);
}
if(I2C3->SR1 & 1<<11)
{
I2C3->SR1 &=~(1<<11);
}
if(I2C3->SR1 & 1<<9)
{
I2C3->SR1 &=~(1<<9);
}
if(I2C3->SR1 & 1<<8)
{
I2C3->SR1 &=~(1<<8);
}
}
void ReadData(uint8_t DevID,uint8_t Addr,uint8_t *Pbuf,uint8_t Num)
{
uint8_t i,x_addr;
x_addr = Addr;
for( i = 0;i < Num; i++){
Pbuf[i] = BNO055_ReadByte(DevID,x_addr);
x_addr++;
}
}
void WriteData(uint8_t DevID,uint8_t Addr,uint8_t *Dat,uint8_t Num)
{
uint8_t i,a_addr;
a_addr = Addr;
for(i = 0 ;i < Num; i++){
BNO055_WriteByte(DevID,a_addr,Dat[i]);
a_addr++;
}
}
运动姿态控制。
#ifndef __MOTION_CONTROL_H
#define __MOTION_CONTROL_H
#include "stm32f2xx_hal.h"
#include "attitude_pid.h"
#include "stdlib.h"
#include "publish.h"
#define MOTION_MAX_SPEED_LIMIT 5040
#define MOTION_MAX_SPEED 5000
#define FRAME_HEAD_1 0XB5 //帧头1
#define FRAME_HEAD_2 0X5B //帧头2
#define FRAME_TAIL 0XFF //帧尾
#define MOVE 0X70 //移动控制标志位
#define REPORT 0X80 //汇报标志位
#define HEAD_UNLOCK 0X90 //解锁航向锁定
#define HEAD_LOCK 0XA0 //启动航向锁定
#define ZERO_POINT_SET 0XB0 //航向零点设置标志位
/
#define MIN_GLOBAL_MOTOR_SPEED (0.0f)
#define DEFAULT_MOTOR_SPEED (3500.0f)
#define MAX_GLOBAL_MOTOR_SPEED (4000.0f)
extern bool HEADING_LOCK_FLAG;
extern bool power_on;
extern bool motion_state;
extern bool test_flag;
extern bool zero_point_set_flag;
///
extern __IO uint16_t global_motor_speed;
//
/**********底盘控制数据解析函数***********
*buf ：输入接收到的数据包
l：数据包的长度
请勿调用改函数
无返回值
******************************************/
void Motion_control_data_analysis(uint8_t * buf,uint8_t l);
/**********动力混合器*********************
x：机体x轴方向控制数据
y：机体Y轴方向控制数据
z：航向控制数据
允许调用该函数。
无返回值
******************************************/
void Motor_mixer(int16_t x,int16_t y,int16_t z);
/**********底盘控制系统主要执行函数*********************
允许调用该函数
无返回值
******************************************/
void Attitude_system(void);
/**********底盘控制系统初始化函数*********************
允许调用该函数
无返回值
******************************************/
void System_init(void);
/**********底盘复位程序*********************
调用该函数，底盘将会回到0度。
允许调用该函数
无返回值
******************************************/
void Reset_body_yaw(void);
/**********底盘停止*********************
调用该函数，底盘将会停止移动。
允许调用该函数
无返回值
******************************************/
void Body_stop(void);
/**********底盘前进与后退*********************
调用该函数，底盘将会前进或后退
speed:正数前进，负数后退，范围+-3600
允许调用该函数
无返回值
******************************************/
void Body_move_FB(int16_t speed);
/**********底盘左右移动*********************
调用该函数，底盘将会前进或后退
speed:正数前进，负数后退，范围+-3600
允许调用该函数
无返回值
******************************************/
void Body_move_LR(int16_t speed);
/**********底盘旋转功能*********************
调用该函数，底盘将会前进或后退
speed:正数顺时针，负数逆时针，范围+-360
允许调用该函数
无返回值
******************************************/
void Body_rotate(int16_t speed);
/*直线运动通过对X,Y 赋值操作来完成，确保走的直*/
void moveFront(uint8_t speedLevel);
void moveBack (uint8_t speedLevel);
void moveLeft (uint8_t speedLevel);
void moveRight(uint8_t speedLevel);
/*旋转：只要4个轮子方向相同即可，直接控制PWM*/
void rotateClockWise(uint8_t speedLevel);
void rotageAntiClock(uint8_t speedLevel);
/*停止：马达驱动芯片输入高电平，刹车*/
void bottomStop(void);
/*复位：芯片RESET并把角度等参数置0*/
void bottomReset(void);
void wheelTest(uint8_t data);
void Move_control(void);
#endif
#include "tim.h"
#include "Motion_control.h"
#include "motor_control.h"
#include "publish.h"
#include "bmo_055.h"
#include "protocol.h"
#include "version.h"
#ifdef BUSINESS_SHOP_VERSION_ENABLE
//使能倒下检测
#define FALL_DOWN_CHECK
#endif
bool HEADING_LOCK_FLAG = FALSE;
bool power_on = (BOOLEAN)0;
bool motion_state = (BOOLEAN)0;
bool test_flag = (BOOLEAN)0;
bool zero_point_set_flag = (BOOLEAN)0;
float euler_angle[3];
float angular_velocity[3];
void System_init()
{
BNO055_INIT();
AttitudeControlInit();
}
void Attitude_system()
{
static float gry_z_old[4];
int32_t age = 0;
float gry_z = 0;
#ifdef FALL_DOWN_CHECK
static int fall_down_cnt = 0;
uint8_t usrdata[2] = {FALLDOWN,0x00};
#endif
//printCMDxyz(robot_attitude_data.x_speed,robot_attitude_data.y_speed,robot_attitude_data.desiredAngle);
if(HEADING_LOCK_FLAG == FALSE)
{
READ_EUL_FROM_BNO055(euler_angle);//yaw:euler_angle[2] 范围为[0~359]，上电为0 _20190111
// printCMDxyz(99, 99,euler_angle[2]);
READ_GRY_FROM_BNO055(angular_velocity);
age = Transformation_angle_mode(euler_angle[2]);//直接使用读取的yaw
//转换后范围：[-180~179]，第一调用计0。 顺时针增大，逆时针减小。 _20190111
//printCMDxyz(robot_attitude_data.x_speed, robot_attitude_data.y_speed,age);
robot_attitude_data.actualAngle = age;
gry_z = angular_velocity[2];//陀螺仪数据
if(power_on == 0){
gry_z_old[0] = 0;
gry_z_old[1] = 0;
gry_z_old[2] = 0;
}
robot_attitude_data.actualRate = ( gry_z + gry_z_old[0] + gry_z_old[1] + gry_z_old[2] ) / 4;
gry_z_old[0] = gry_z_old[1];
gry_z_old[1] = gry_z_old[0];//多余代码？ _paul 20190116
gry_z_old[2] = robot_attitude_data.actualRate;
// printf("a = %d, s = %d Rate = %d\r\n",robot_attitude_data.actualAngle,robot_attitude_data.desiredAngle,robot_attitude_data.actualRate);
if(power_on == 0){
robot_attitude_data.desiredAngle = age;
}
StateControl(&robot_attitude_control,&robot_attitude_data);
Move_control();
//motor_move_flag = (BOOLEAN)1;
power_on = (BOOLEAN)1;
//机器倒下检测
#ifdef FALL_DOWN_CHECK
// {
//
// uint8_t datas[30] = {0};
// sprintf(datas,"pitch=%f\troll=%f\n",euler_angle[0],euler_angle[1]);
// USART1_SendBuf(datas,strlen(datas));
//
// uint8_t datas[30] = {0};
// sprintf(datas,"roll=%f\n",euler_angle[1]);
// USART1_SendBuf(datas,strlen(datas));
// }
/**
统一机体从后背包往前看,euler_angle[0]:pitch,euler_angle[1]:roll
经测试发现 pitch以当前竖直为0度,向前仰为0--(-180°),向后仰为0--(+180°)
roll以当前竖直为0度,向左为0--(-90°)---0,向右为0--(+90°)---0
**/
//前边倒,后边倒
if(euler_angle[0] < -45.0f || euler_angle[0] > 45.0f ||
//左边倒,右边倒
euler_angle[1] < -45.0f || euler_angle[1] > 45.0f)
{
//1s = 20 * 50
if(!(++fall_down_cnt % 50))
{
usrdata[1] = 0x01;
Build_Send_Protocol(SENSOR_ECHO_CTL,usrdata,sizeof(usrdata));
}
}
else{
if(fall_down_cnt >= 50)
{
usrdata[1] = 0x00;
Build_Send_Protocol(SENSOR_ECHO_CTL,usrdata,sizeof(usrdata));
//try send once
Build_Send_Protocol(SENSOR_ECHO_CTL,usrdata,sizeof(usrdata));
}
fall_down_cnt = 0;
}
#endif
}
}
void Move_control(void)
{
int16_t turn_speed = 0;
if(robot_attitude_control.yaw < 800 && robot_attitude_control.yaw > -800){
turn_speed = 0;
}else {
//旋转速度特殊定义
turn_speed = robot_attitude_control.yaw * 0.11f;
}
if(robot_attitude_data.x_speed > 4500) robot_attitude_data.x_speed = 1700;
if(robot_attitude_data.x_speed < -4500) robot_attitude_data.x_speed = -1700;
if(robot_attitude_data.y_speed > 4500) robot_attitude_data.y_speed = 1700;
if(robot_attitude_data.y_speed < -4500) robot_attitude_data.y_speed = -1700;
Motor_mixer(robot_attitude_data.x_speed,robot_attitude_data.y_speed,turn_speed);
}
void Motor_mixer(int16_t x,int16_t y,int16_t z)
{
float set_corrct_Percentage = 0;
// float k = 0.045;
// static int16_t x_last = 0,y_last = 0;
static int16_t err = 0;
int16_t motor_1_power = 0;
int16_t motor_2_power = 0;
int16_t motor_3_power = 0;
int16_t motor_4_power = 0;
//
float speed_factor = global_motor_speed / MAX_GLOBAL_MOTOR_SPEED;
//
if(x == 0 && y == 0){
//旋转速度区分点
set_corrct_Percentage = 1;
if(z > 0){
z = z + 500;
}else if(z < 0){
z = z - 500;
}
// x_last = 0;
// y_last = 0;
err = 0;
//旋转速度不可调速
speed_factor = 1.0f;
}else {
set_corrct_Percentage = 1.27;
// x = x * k + x_last * (1 - k);
// y = y * k + y_last * (1 - k);
//取消渐加速
x = x;
y = y;
// x_last = x;
// y_last = y;
if(x > 0){
x = x + 200;
}else if(x < 0){
x = x - 200;
}
if(y > 0){
y = y + 200;
}else if(y < 0){
y = y - 200;
}
}
motor_1_power = x - y + z * set_corrct_Percentage;
motor_2_power = (-x) - y + z * set_corrct_Percentage;
motor_3_power = (-x) + y + z * set_corrct_Percentage;
motor_4_power = x + y + z * set_corrct_Percentage;
/**
通过手机app调整转速,调整区间为0--4000
*/
if(motor_1_power >= 0){
motor_p.MOTOR_1.turn_x = 2;
motor_p.MOTOR_1.speed = (motor_1_power - err) * speed_factor;
}else {
motor_p.MOTOR_1.turn_x = 1;
motor_p.MOTOR_1.speed = (- motor_1_power + err) * speed_factor;
}
if(motor_2_power >= 0){
motor_p.MOTOR_2.turn_x = 2;
motor_p.MOTOR_2.speed = (motor_2_power) * speed_factor;
}else {
motor_p.MOTOR_2.turn_x = 1;
motor_p.MOTOR_2.speed = (- motor_2_power) * speed_factor;
}
if(motor_3_power >= 0){
motor_p.MOTOR_3.turn_x = 2;
motor_p.MOTOR_3.speed = (motor_3_power - err) * speed_factor;
}else {
motor_p.MOTOR_3.turn_x = 1;
motor_p.MOTOR_3.speed = (- motor_3_power + err) * speed_factor;
}
if(motor_4_power >= 0){
motor_p.MOTOR_4.turn_x = 2;
motor_p.MOTOR_4.speed = (motor_4_power) * speed_factor;
}else {
motor_p.MOTOR_4.turn_x = 1;
motor_p.MOTOR_4.speed = (- motor_4_power) * speed_factor;
}
if(motor_p.MOTOR_1.speed > MOTION_MAX_SPEED_LIMIT) motor_p.MOTOR_1.speed = MOTION_MAX_SPEED;
if(motor_p.MOTOR_2.speed > MOTION_MAX_SPEED_LIMIT) motor_p.MOTOR_2.speed = MOTION_MAX_SPEED;
if(motor_p.MOTOR_3.speed > MOTION_MAX_SPEED_LIMIT) motor_p.MOTOR_3.speed = MOTION_MAX_SPEED;
if(motor_p.MOTOR_4.speed > MOTION_MAX_SPEED_LIMIT) motor_p.MOTOR_4.speed = MOTION_MAX_SPEED;
if(x == 0 && y == 0){
Motor_control(motor_p,0);
}else {
Motor_control(motor_p,0);
}
}
/*
IN1 IN2 功能
L L 待机/滑行
H H 刹车
PWM L 正转
L PWM 反转
*/
void bottomReset(void);
void moveFront(uint8_t speedLevel);
void moveBack (uint8_t speedLevel);
void moveLeft (uint8_t speedLevel);
void moveRight(uint8_t speedLevel);
/*旋转：只要4个轮子方向相同即可，直接控制PWM*/
void rotateClockWise(uint8_t speedLevel);
void rotageAntiClock(uint8_t speedLevel);
/*停止：马达驱动芯片输入高电平，刹车*/
void bottomStop(void);
static void wheelSpeedZero(void)
{
TIM3->CCR1 = 0; TIM3->CCR2 = 0; TIM3->CCR3 = 0; TIM3->CCR4 = 0;
TIM4->CCR1 = 0; TIM4->CCR2 = 0; TIM4->CCR3 = 0; TIM4->CCR4 = 0;
}
void bottomReset(void)
{
zero_piont_set_flag = FALSE;
}
#define SPEED_TEST_VAL 4900
void wheelTest(uint8_t data)
{
uint8_t uWhellNum = (data&0xF0) >>4;
uint8_t uDirs = data&0x0F;
if(uWhellNum>4 || uDirs>1 )
return ;
wheelSpeedZero();
if(uWhellNum ==1){ //1号电机——已经测试20190125
if(!uDirs)TIM3->CCR4 = SPEED_TEST_VAL; //CD1010--逆时针
else TIM3->CCR3 = SPEED_TEST_VAL; //CD1011--顺时针
}
else if(uWhellNum ==2){ //2号电机——已经测试20190125
if(!uDirs)TIM3->CCR2 = SPEED_TEST_VAL; //CD1020--逆时针
else TIM3->CCR1 = SPEED_TEST_VAL; //CD1021--顺时针
}
else if(uWhellNum ==3){ //3号电机——已经测试20190125
if(!uDirs)TIM4->CCR4 = SPEED_TEST_VAL; //CD1030--逆时针
else TIM4->CCR3 = SPEED_TEST_VAL; //CD1031--顺时针
}
else if(uWhellNum ==4){ //4号电机
if(!uDirs)TIM4->CCR2 = SPEED_TEST_VAL; //CD1040--逆时针
else TIM4->CCR1 = SPEED_TEST_VAL; //CD1041--顺时针
}
}
void rotateClockWise(uint8_t speedLevel)
{//顺时针
// uint16_t speedABS = MOTOR_MAX_SPPPE*speedLevel/10;
// Motor_stop();
// TIM4->CCR1 = speedABS; //4号电机
// TIM4->CCR2 = 0;
//
// TIM3->CCR1 = speedABS; //2号电机
// TIM3->CCR2 = 0;
// TIM4->CCR3 = speedABS; //3号电机
// TIM4->CCR4 = 0;
//
// TIM3->CCR3 = speedABS; //1号电机
// TIM3->CCR4 = 0;
}
void rotageAntiClock(uint8_t speedLevel)
{//逆时针
// uint16_t speedABS = MOTOR_MAX_SPPPE*speedLevel/10;
// Motor_stop();
// TIM4->CCR1 = 0; //4号电机
// TIM4->CCR2 = speedABS;
//
// TIM3->CCR1 = 0; //2号电机
// TIM3->CCR2 = speedABS;
// TIM4->CCR3 = 0; //3号电机
// TIM4->CCR4 = speedABS;
//
// TIM3->CCR3 = 0; //1号电机
// TIM3->CCR4 = speedABS;
}
//MOTOR_MAX_SPPPE?
void moveFront(uint8_t speedLevel)
{
uint16_t speedABS = MOTOR_MAX_SPPPE*speedLevel/10;
robot_attitude_data.y_speed = 0;
robot_attitude_data.x_speed = speedABS;
}
void moveBack (uint8_t speedLevel)
{
uint16_t speedABS = MOTOR_MAX_SPPPE*speedLevel/10;
robot_attitude_data.y_speed = 0;
robot_attitude_data.x_speed = 0-speedABS;
}
void moveRight (uint8_t speedLevel)
{
uint16_t speedABS = MOTOR_MAX_SPPPE*speedLevel/10;
robot_attitude_data.x_speed = 0;
robot_attitude_data.y_speed = speedABS;
}
void moveLeft (uint8_t speedLevel)
{
uint16_t speedABS = MOTOR_MAX_SPPPE*speedLevel/10;
robot_attitude_data.x_speed = 0;
robot_attitude_data.y_speed = 0-speedABS;
}
void bottomStop(void)
{
#if 1
Motor_stop();
bottomReset();
robot_attitude_data.x_speed = 0;
robot_attitude_data.y_speed = 0;
robot_attitude_data.desiredAngle = 0;
#else
//关闭定时器，PWM全输出高电平，轮子刹车
//HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
#endif
}
#ifndef __ATTITUDE_PID_H
#define __ATTITUDE_PID_H
#include "stm32f2xx_hal.h"
#include "pid.h"
#include "Heading_lock.h"
#include "publish.h"
#define SENSOR9_UPDATE_RATE 50 //底盘PID执行频率
#define SENSOR9_UPDATE_DT (1/SENSOR9_UPDATE_RATE) //底盘PID执行周期
//#pragma pack(1)
typedef struct
{
int32_t x;
int32_t y;
int32_t z;
} Axis3f;
//#pragma pack(1)
typedef struct //底盘主要数据结构体
{
uint32_t timestamp; //时间戳
int32_t DesiredRate_z; //目标角速度
int32_t angle_err; //角度差
int16_t actualRate; //当前机体角速度
int16_t actualAngle; //当前机体角度
__IO int16_t desiredAngle; //目标角度，改变此角度可以旋转机体，范围+-180度
__IO int16_t x_speed; //机体前进与后退的速度值，范围+-4000，前进为正值。
__IO int16_t y_speed; //机体左右移动的速度值，范围+-4000，右移为正值。
} attitude_t;
//#pragma pack(1)
typedef struct
{
int16_t roll;
int16_t pitch;
int16_t yaw;
} control_t;
extern attitude_t robot_attitude_data;
extern control_t robot_attitude_control;
extern control_t robot_rc_control;
/**********底盘姿态控制初始化*********************
请勿调用该函数
无返回值
******************************************/
void AttitudeControlInit(void);
/**********底盘姿态PID算法*********************
*attitude_data：底盘数据结构体
请勿调用该函数
无返回值
******************************************/
void AttitudeAnglePID(attitude_t *attitude_data);
/*******************************
*attitude_data：底盘数据结构体
*control：控制数据的结构体
请勿调用该函数
无返回值
******************************************/
void StateControl(control_t *control,attitude_t *attitude_data); //姿态控制主函数
#endif
#include "attitude_pid.h"
#include "publish.h"
PidObject pidAngleYaw;
PidObject pidRateYaw;
pidInit_t AngleYaw;
pidInit_t RateYaw;
attitude_t robot_attitude_data;
control_t robot_attitude_control;
int16_t pidOutLimit(int32_t in)
{
if (in > INT16_MAX)
return INT16_MAX;
else if (in < -INT16_MAX)
return -INT16_MAX;
else
return (int16_t)in;
}
void AttitudeControlInit()
{
AngleYaw.kp = 11.7;
AngleYaw.ki = 0.0;
AngleYaw.kd = 12.5;
RateYaw.kp = 36.5;
RateYaw.ki = 6.0;
RateYaw.kd = 0.0;
pidInit(&pidAngleYaw, 0, AngleYaw, SENSOR9_UPDATE_DT); /*yaw 角度PID初始化*/
pidSetIntegralLimit(&pidAngleYaw, PID_ANGLE_YAW_INTEGRATION_LIMIT); /*yaw 角度积分限幅设置*/
pidInit(&pidRateYaw, 0, RateYaw, SENSOR9_UPDATE_DT); /*yaw 角速度PID初始化*/
pidSetIntegralLimit(&pidRateYaw, PID_RATE_YAW_INTEGRATION_LIMIT); /*yaw 角速度积分限幅设置*/
}
void AttitudeAnglePID(attitude_t *attitude_data) /* 角度环PID */
{
attitude_data->DesiredRate_z = pidUpdate(&pidAngleYaw, attitude_data->angle_err);
}
void AttitudeRatePID(attitude_t *attitude_data,control_t *output) /* 角速度环PID */
{
output->yaw = pidOutLimit(pidUpdate(&pidRateYaw, attitude_data->DesiredRate_z - attitude_data->actualRate));
}
void StateControl(control_t *control,attitude_t *attitude_data)
{
Calculating_angle_error(attitude_data->desiredAngle, attitude_data->actualAngle);
AttitudeRatePID(attitude_data, control); /* 角速度(内环)PID */
}
双环PID控制。
#ifndef __PID_H
#define __PID_H
#include "stm32f2xx_hal.h"
#include "publish.h"
/*角度环积分限幅*/
#define PID_ANGLE_YAW_INTEGRATION_LIMIT 167
/*角速度环积分限幅*/
#define PID_RATE_YAW_INTEGRATION_LIMIT 360
#define DEFAULT_PID_INTEGRATION_LIMIT 4000
typedef struct
{
float kp;
float ki;
float kd;
} pidInit_t;
typedef struct
{
int32_t desired; //< set point
int32_t error; //< error
int32_t prevError; //< previous error
int32_t integ; //< integral
int32_t deriv; //< derivative
int32_t kp; //< proportional gain
int32_t ki; //< integral gain
int32_t kd; //< derivative gain
int32_t outP; //< proportional output (debugging)
int32_t outI; //< integral output (debugging)
int32_t outD; //< derivative output (debugging)
int32_t iLimit; //< integral limit
int32_t iLimitLow; //< integral limit
int32_t dt; //< delta-time dt
} PidObject;
/*********PID积分限幅*********************
pid：pid数据结构体
limit：限制值
请勿调用该函数。
无返回值
******************************************/
void pidSetIntegralLimit(PidObject* pid, int32_t limit);
/*********PID初始化函数*********************
pid：pid数据结构体
desired：目标值
pidParam：pid参数结构体
dt：积分时间
请勿调用该函数。
无返回值
******************************************/
void pidInit(PidObject* pid, int32_t desired, const pidInit_t pidParam, int32_t dt);
/**********PID计算函数*********************
pid：pid数据结构体
error：误差
请勿调用该函数。
返回计算结果
******************************************/
int32_t pidUpdate(PidObject* pid, int32_t error);
/**********姿态控制初始化*********************
pid：pid数据结构体
error：误差
请勿调用该函数。
返回计算结果
******************************************/
void attitudeControlInit(void);
#endif /* __PID_H */
#include "pid.h"
void pidInit(PidObject* pid, int32_t desired, const pidInit_t pidParam, int32_t dt)
{
pid->error = 0;
pid->prevError = 0;
pid->integ = 0;
pid->deriv = 0;
pid->desired = desired;
pid->kp = pidParam.kp;
pid->ki = pidParam.ki;
pid->kd = pidParam.kd;
pid->iLimit = DEFAULT_PID_INTEGRATION_LIMIT;
pid->iLimitLow = -DEFAULT_PID_INTEGRATION_LIMIT;
pid->dt = dt;
}
int32_t pidUpdate(PidObject* pid, int32_t error)
{
float output;
pid->error = error;
pid->integ += pid->error * pid->dt;
if (pid->integ > pid->iLimit)
{
pid->integ = pid->iLimit;
}
else if (pid->integ < pid->iLimitLow)
{
pid->integ = pid->iLimitLow;
}
pid->deriv = (pid->error - pid->prevError) / pid->dt;
pid->outP = pid->kp * pid->error;
pid->outI = pid->ki * pid->integ;
pid->outD = pid->kd * pid->deriv;
output = pid->outP + pid->outI + pid->outD;
pid->prevError = pid->error;
return output;
}
void pidSetIntegralLimit(PidObject* pid, int32_t limit)
{
pid->iLimit = limit;
}
void pidSetIntegralLimitLow(PidObject* pid, int32_t limitLow)
{
pid->iLimitLow = limitLow;
}
void pidReset(PidObject* pid)
{
pid->error = 0;
pid->prevError = 0;
pid->integ = 0;
pid->deriv = 0;
}
void pidSetError(PidObject* pid, int32_t error)
{
pid->error = error;
}
void pidSetDesired(PidObject* pid, int32_t desired)
{
pid->desired = desired;
}
int32_t pidGetDesired(PidObject* pid)
{
return pid->desired;
}
uint8_t pidIsActive(PidObject* pid)
{
uint8_t isActive = 1;
if (pid->kp < 0.0001f && pid->ki < 0.0001f && pid->kd < 0.0001f)
{
isActive = 0;
}
return isActive;
}
void pidSetKp(PidObject* pid, int32_t kp)
{
pid->kp = kp;
}
void pidSetKi(PidObject* pid, int32_t ki)
{
pid->ki = ki;
}
void pidSetKd(PidObject* pid, int32_t kd)
{
pid->kd = kd;
}
void pidSetDt(PidObject* pid, int32_t dt)
{
pid->dt = dt;
}
电机运动方式控制。
#ifndef __motor_control_h
#define __motor_control_h
#include "stm32f2xx_hal.h"
#include "publish.h"
#define MOTOR_MAX_SPPPE 5040 //马达最大限速
typedef struct
{
uint8_t turn_x;
uint16_t speed;
} _MOTOR;
typedef struct
{
_MOTOR MOTOR_1;
_MOTOR MOTOR_2;
_MOTOR MOTOR_3;
_MOTOR MOTOR_4;
} _MOTOR_CONTROL;
extern _MOTOR_CONTROL motor_c;
extern _MOTOR_CONTROL motor_p;
extern _MOTOR_CONTROL motor_A;
extern uint16_t last_m1,last_m2,last_m3,last_m4;
/*******马达控制程序***********************
m：马达控制数据结构体
x：马达使能标志位
请勿调用该函数。
无返回值
******************************************/
void Motor_control(_MOTOR_CONTROL m,uint8_t x); //马达控制函数
/*********马达停止*********************
请勿调用该函数。
无返回值
******************************************/
void Motor_stop(void); //刹车函数
#endif
#include "motor_control.h"
#include "Motion_control.h"
_MOTOR_CONTROL motor_c;
_MOTOR_CONTROL motor_p;
_MOTOR_CONTROL motor_A;
uint16_t last_m1 = 0 ,last_m2 = 0 ,last_m3 = 0 ,last_m4 = 0;
void Motor_control(_MOTOR_CONTROL m,uint8_t x)
{
float k = 0.045;
uint16_t m1_set = 0,m2_set = 0,m3_set = 0, m4_set = 0;
uint16_t m1_T1 = 0, m2_T1 = 0, m3_T1 = 0, m4_T1 = 0;
uint16_t m1_T2 = 0, m2_T2 = 0, m3_T2 = 0, m4_T2 = 0;
if(x == 1){
m1_set = last_m1 * (1 - k) + m.MOTOR_1.speed * k;
m2_set = last_m2 * (1 - k) + m.MOTOR_2.speed * k;
m3_set = last_m3 * (1 - k) + m.MOTOR_3.speed * k;
m4_set = last_m4 * (1 - k) + m.MOTOR_4.speed * k;
last_m1 = m1_set;
last_m2 = m2_set;
last_m3 = m3_set;
last_m4 = m4_set;
}
if(x == 0){
m1_set = m.MOTOR_1.speed;
m2_set = m.MOTOR_2.speed;
m3_set = m.MOTOR_3.speed;
m4_set = m.MOTOR_4.speed;
last_m1 = 0;
last_m2 = 0;
last_m3 = 0;
last_m4 = 0;
}
//**************电机1**********************
if(m.MOTOR_1.turn_x == 1){
m1_T1 = MOTOR_MAX_SPPPE;
m1_T2 = MOTOR_MAX_SPPPE - m1_set;
}
if(m.MOTOR_1.turn_x == 2){
m1_T1 = MOTOR_MAX_SPPPE - m1_set;
m1_T2 = MOTOR_MAX_SPPPE;
}
//*************电机2***********************
if(m.MOTOR_2.turn_x == 1){
m2_T1 = MOTOR_MAX_SPPPE - m2_set;
m2_T2 = MOTOR_MAX_SPPPE;
}
if(m.MOTOR_2.turn_x == 2){
m2_T1 = MOTOR_MAX_SPPPE;
m2_T2 = MOTOR_MAX_SPPPE - m2_set;
}
//***********电机3*****************************
if(m.MOTOR_3.turn_x == 1){
m3_T1 = MOTOR_MAX_SPPPE;
m3_T2 = MOTOR_MAX_SPPPE - m3_set;
}
if(m.MOTOR_3.turn_x == 2){
m3_T1 = MOTOR_MAX_SPPPE - m3_set;
m3_T2 = MOTOR_MAX_SPPPE;
}
//*****************电机4*************************
if(m.MOTOR_4.turn_x == 1){
m4_T1 = MOTOR_MAX_SPPPE - m4_set;
m4_T2 = MOTOR_MAX_SPPPE;
}
if(m.MOTOR_4.turn_x == 2){
m4_T1 = MOTOR_MAX_SPPPE;
m4_T2 = MOTOR_MAX_SPPPE - m4_set;
}
//**********************************************
TIM4->CCR1 = m1_T1; //4号电机
TIM4->CCR2 = m1_T2;
TIM3->CCR2 = m2_T1; //2号电机
TIM3->CCR1 = m2_T2;
TIM4->CCR3 = m3_T1; //3号电机
TIM4->CCR4 = m3_T2;
TIM3->CCR4 = m4_T1; //1号电机
TIM3->CCR3 = m4_T2;
}
void Motor_stop()
{
TIM3->CCR1 = MOTOR_MAX_SPPPE;
TIM3->CCR2 = MOTOR_MAX_SPPPE;
TIM4->CCR1 = MOTOR_MAX_SPPPE;
TIM4->CCR2 = MOTOR_MAX_SPPPE;
TIM3->CCR3 = MOTOR_MAX_SPPPE;
TIM3->CCR4 = MOTOR_MAX_SPPPE;
TIM4->CCR3 = MOTOR_MAX_SPPPE;
TIM4->CCR4 = MOTOR_MAX_SPPPE;
}
芯片校准。
#ifndef __BNO055_CALIBRATION__
#define __BNO055_CALIBRATION__
#include "stm32f2xx.h"
#include "mpuiic.h"
#include "delay.h"
#include "boot_act.h"
#include "at24c02.h"
#define BNO055_CALIB_STAT_ADDR (0x35)
#define BNO055_OPR_MODE_ADDR (0x3D)
#define ACCEL_OFFSET_X_LSB_ADDR (0x55)
#define MAG_OFFSET_X_LSB_ADDR (0x5B)
#define IS_CALIBRATE_OK_CNT (1000)
typedef struct
{
uint8_t cali_mag[6];
uint8_t cali_acc[6];
}cali_msg_t;
typedef enum
{
/* Operation mode settings*/
OPERATION_MODE_CONFIG = 0x00,
OPERATION_MODE_ACCONLY = 0x01,
OPERATION_MODE_MAGONLY = 0x02,
OPERATION_MODE_GYRONLY = 0x03,
OPERATION_MODE_ACCMAG = 0x04,
OPERATION_MODE_ACCGYRO = 0x05,
OPERATION_MODE_MAGGYRO = 0x06,
OPERATION_MODE_AMG = 0x07,
OPERATION_MODE_IMUPLUS = 0x08,
OPERATION_MODE_COMPASS = 0x09,
OPERATION_MODE_M4G = 0x0A,
OPERATION_MODE_NDOF_FMC_OFF = 0x0B,
OPERATION_MODE_NDOF = 0x0C
}bno055_opmode_t;
void calibrate_init(void);
void calibrate_loop(void);
void calibrate_execute(void);
#endif //__BNO055_CALIBRATION__
/
#include "calibration.h"
#define BOTTOM_9AXIS_CALI_ADDR (105)
#define BOTTOM_9AXUS_CALI_DATA_LEN (12)
static uint8_t is_calibrate_ok_flag = 0;
static cali_msg_t cali_msg_data;
static uint8_t fupdataEEPROM_YawCali(void)
{
uint8_t retval,trycnt = 0;
uint8_t data[BOTTOM_9AXUS_CALI_DATA_LEN] = {0x00};
memcpy(data,&cali_msg_data,sizeof(data));
RETRY:
retval = AT24_Write(data,BOTTOM_9AXIS_CALI_ADDR,sizeof(data));
if(HAL_OK != retval)
{
if(trycnt++ > MAX_TRY_CNT){
return 1;
}
goto RETRY;
}
return 0;
}
static BOOLEAN getEEData4YawCali(uint8_t *uBuf,uint8_t Len)
{
const uint8_t data_00[BOTTOM_9AXUS_CALI_DATA_LEN / 2] = {0x00,0x00,0x00,0x00,0x00,0x00};
const uint8_t data_ff[BOTTOM_9AXUS_CALI_DATA_LEN / 2] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
BOOLEAN ret = FALSE;
uint8_t retval,trycnt = 0;
if(Len != BOTTOM_9AXUS_CALI_DATA_LEN){
return FALSE;
}
RETRY:
retval = AT24_Read(uBuf,BOTTOM_9AXIS_CALI_ADDR,Len);
if(HAL_OK == retval)
{
if(!memcmp(uBuf,data_00,sizeof(data_00))){
return FALSE;
}
if(!memcmp(uBuf,data_ff,sizeof(data_ff))){
return FALSE;
}
ret = TRUE;
}else{
if(trycnt++ > MAX_TRY_CNT){
return ret;
}
goto RETRY;
}
return ret;
}
static uint8_t is_calibrate_data_exist(void)
{
uint8_t uDataMPU[BOTTOM_9AXUS_CALI_DATA_LEN];
if(getEEData4YawCali(uDataMPU,BOTTOM_9AXUS_CALI_DATA_LEN))
{
memcpy(&cali_msg_data,uDataMPU,sizeof(uDataMPU));
return 1;
}
else{
return 0;
}
}
static uint32_t set_calibrate_data_to_flash(cali_msg_t * p_data)
{
return fupdataEEPROM_YawCali();
}
static void set_bno_work_mode(bno055_opmode_t mode)
{
WriteData(BNO055_I2C_ADDR2, BNO055_OPR_MODE_ADDR,(uint8_t*)&mode,1);
}
static void get_bno_calibrate_data(cali_msg_t * p_data)
{
uint8_t tmp_bufs[6];
osMutexWait(mutex_i2c_id,osWaitForever);
set_bno_work_mode(OPERATION_MODE_CONFIG);
osMutexRelease(mutex_i2c_id);
osDelay(10);
osMutexWait(mutex_i2c_id,osWaitForever);
ReadData(BNO055_I2C_ADDR2, ACCEL_OFFSET_X_LSB_ADDR,tmp_bufs,6);
memcpy(p_data->cali_acc,tmp_bufs,6);
ReadData(BNO055_I2C_ADDR2, MAG_OFFSET_X_LSB_ADDR,tmp_bufs,6);
memcpy(p_data->cali_mag,tmp_bufs,6);
set_bno_work_mode(OPERATION_MODE_NDOF);
osMutexRelease(mutex_i2c_id);
}
static uint32_t set_bno_calibrate_data(cali_msg_t * p_data)
{
osMutexWait(mutex_i2c_id,osWaitForever);
set_bno_work_mode(OPERATION_MODE_CONFIG);
osMutexRelease(mutex_i2c_id);
osDelay(10);
osMutexWait(mutex_i2c_id,osWaitForever);
WriteData(BNO055_I2C_ADDR2, ACCEL_OFFSET_X_LSB_ADDR,(uint8_t*)p_data->cali_acc,6);
WriteData(BNO055_I2C_ADDR2, MAG_OFFSET_X_LSB_ADDR,p_data->cali_mag,6);
set_bno_work_mode(OPERATION_MODE_NDOF);
osMutexRelease(mutex_i2c_id);
return 0;
}
static uint32_t is_calibrate_stat_ok(void)
{
static uint32_t state_ok_cnt = 0;
uint8_t tmp_val;
osMutexWait(mutex_i2c_id,osWaitForever);
ReadData(BNO055_I2C_ADDR2, BNO055_CALIB_STAT_ADDR,&tmp_val,1);
osMutexRelease(mutex_i2c_id);
#if 1
if(state_ok_cnt >= IS_CALIBRATE_OK_CNT)
return ++state_ok_cnt;
else
#endif
if((tmp_val & 0xc3) == 0xc3)
{
state_ok_cnt++;
}
else
{
state_ok_cnt = 0;
}
#if 1
return state_ok_cnt;
#else
if(state_ok_cnt > IS_CALIBRATE_OK_CNT)
{
return 0;
}
else
{
return 1;
}
#endif
}
void calibrate_init(void)
{
if(is_calibrate_data_exist())
{
set_bno_calibrate_data(&cali_msg_data);
}
}
/*************** method 1 ****************/
void calibrate_loop(void)
{
if(is_calibrate_ok_flag)
{
return;
}
else
{
uint32_t uTimesOK = is_calibrate_stat_ok();
if(uTimesOK >= IS_CALIBRATE_OK_CNT)
{
if(!is_calibrate_ok_flag)
{
is_calibrate_ok_flag = 1;
Mailbox_Send(GECMD_MAIL_BOTTOMCALIBRATE,0);
}
}
}
}
void calibrate_execute(void)
{
get_bno_calibrate_data(&cali_msg_data);
set_calibrate_data_to_flash(&cali_msg_data);
}
朝向锁定。
#ifndef __HEADING_LOCK_H
#define __HEADING_LOCK_H
#include "stm32f2xx_hal.h"
#include "attitude_pid.h"
#include "publish.h"
extern bool zero_piont_set_flag;
/**********角度转换函数*********************
a：输入角度
请勿调用该函数
无返回值
******************************************/
int32_t Transformation_angle_mode(int32_t a);
/**********角度误差计算函数*********************
set：设定角度
act：当前角度
请勿调用该函数
无返回值
******************************************/
int32_t Calculating_angle_error(int32_t set, int32_t act);
#endif
#include "Heading_lock.h"
#include "Motion_control.h"
#define SET_POWERUP_ANGLE_ZERO
bool zero_piont_set_flag = FALSE;
int32_t Transformation_angle_mode(int32_t a)
{
static int32_t out = 0;
static int32_t out_age_err = 0;
/* [0~359] mapTo [-180~179]*/
if(a >= 0 && a < 180){
out = -(180 - a);
}else if(a >= 180){
out = a - 180;
}
#ifdef SET_POWERUP_ANGLE_ZERO
//角度复位计0
if(zero_piont_set_flag == FALSE){
out_age_err = 0 - out;
zero_piont_set_flag = TRUE;
out = 0;
}else {
out = out + out_age_err;
}
if(out > 180) out = -(360 - out);
if(out < -180) out = 360 + out;
#endif
return out;
}
uint8_t check_quadrant(int32_t x)
{
if( (x <= 0 ) && (x >= (-90)) ){
return 1;
}
if( (x < (-90)) && (x >= (-180)) ){
return 2;
}
if( (x <= 180) && (x > 90) ){
return 3;
}
if( (x <= 90) && (x > 0) ){
return 4;
}
return 0xFF;
}
int32_t Calculating_angle_error(int32_t set, int32_t act)
{
uint8_t set_angle_quadrant = 0;
uint8_t act_angle_quadrant = 0;
uint8_t cusum = 0;
int32_t angle_err;
set_angle_quadrant = check_quadrant(set);
act_angle_quadrant = check_quadrant(act);
if(set_angle_quadrant == act_angle_quadrant){
angle_err = act - set;
} else {
cusum = set_angle_quadrant + act_angle_quadrant;
switch(cusum){
case 3:
if(set_angle_quadrant == 1 && act_angle_quadrant == 2){
angle_err = act - set;
}
if(set_angle_quadrant == 2 && act_angle_quadrant == 1){
angle_err = (act - set);
}
break;
case 4:
if(set_angle_quadrant == 1 && act_angle_quadrant == 3){
if(set == 0){
angle_err = act - set;
}else{
angle_err = - (360 + set - act);
if(angle_err < -180) angle_err = angle_err + 360;
}
}
if(set_angle_quadrant == 3 && act_angle_quadrant == 1){
angle_err = 360 + act - set;
if(angle_err > 180) angle_err = angle_err - 360;
}
break;
case 5:
if(set_angle_quadrant == 1 && act_angle_quadrant == 4){
if(set == 0){
angle_err = act - set;
}else{
angle_err = (act - set);
}
}
if(set_angle_quadrant == 4 && act_angle_quadrant == 1){
angle_err = act - set;
}
if(set_angle_quadrant == 2 && act_angle_quadrant == 3){
angle_err = - (360 + set - act);
}
if(set_angle_quadrant == 3 && act_angle_quadrant == 2){
angle_err = 360 + act - set;
}
break;
case 6:
if(set_angle_quadrant == 4 && act_angle_quadrant == 2){
angle_err = -(set - act);
if(angle_err < -180) angle_err = angle_err + 360;
}
if(set_angle_quadrant == 2 && act_angle_quadrant == 4){
angle_err = -(360 + set - act);
if(angle_err < -180) angle_err = angle_err + 360;
}
break;
case 7:
if(set_angle_quadrant == 3 && act_angle_quadrant == 4){
angle_err = act - set;
}
if(set_angle_quadrant == 4 && act_angle_quadrant == 3){
angle_err = act - set;
}
break;
}
}
if(1){
robot_attitude_data.angle_err = angle_err;
AttitudeAnglePID(&robot_attitude_data);
}else {
return angle_err;
}
return 0xFF;
}
外部调用封装。
#ifndef __BOTTOM_H__
#define __BOTTOM_H__
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "tim.h"
#include "bmo_055.h"
#include "Motion_control.h"
#include "attitude_pid.h"
#include "motor_control.h"
void checkBMO055Chip(void);
void BottomPan_Init(void);
#define BottomPan_Handle() Attitude_system()
#endif
#include "bottom.h"
/**
底盘全局速度控制
*/
__IO uint16_t global_motor_speed = DEFAULT_MOTOR_SPEED;
void checkBMO055Chip(void)
{
if((mag_need_cal_flag != 0) || (acc_need_cal_flag != 0) || (gry_need_cal_flag != 0))
{
CHECK_MAG_CALIB_STAT();
}
}
void BottomPan_Init(void)
{
MX_TIM3_Init();
MX_TIM4_Init();
//底盘初始化
System_init();
//启动八通道
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_4);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_2);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_3);
HAL_TIM_PWM_Start(&htim4, TIM_CHANNEL_4);
}
848
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
