178 lines
3.5 KiB
C
178 lines
3.5 KiB
C
#include "config.h"
|
|
#include "flash.h"
|
|
#include "string.h"
|
|
#define DEBUG
|
|
|
|
#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)
|
|
|
|
#define DEG_TO_RAD(x) ((x) * (3.141592653 / 180.0)) //角度转弧度
|
|
|
|
|
|
#ifdef DEBUG
|
|
|
|
ConfigParam_t param ={
|
|
|
|
#else
|
|
|
|
static const ConfigParam_t param ={
|
|
|
|
|
|
#endif
|
|
.up={
|
|
|
|
|
|
/*上层pid参数*/
|
|
.Shoot_M2006_angle_param = {
|
|
.p = 25.0f,
|
|
.i = 0.0f,
|
|
.d = 1.5f,
|
|
.i_limit = 1000.0f,
|
|
.out_limit = 3000.0f,
|
|
},
|
|
.Shoot_M2006_speed_param = {
|
|
.p = 5.0f,
|
|
.i = 0.3f,
|
|
.d = 0.0f,
|
|
.i_limit = 2000.0f,
|
|
.out_limit = 3000.0f,
|
|
},
|
|
.Pitch_M2006_angle_param = {
|
|
.p = 25.0f,
|
|
.i = 0.0f,
|
|
.d = 1.5f,
|
|
.i_limit = 1000.0f,
|
|
.out_limit = 3000.0f,
|
|
},
|
|
.Pitch_M2006_speed_param={
|
|
.p = 5.0f,
|
|
.i = 0.3f,
|
|
.d = 0.0f,
|
|
.i_limit = 2000.0f,
|
|
.out_limit = 3000.0f,
|
|
},
|
|
|
|
|
|
.go_cmd={
|
|
.id =0,
|
|
.mode = 1,
|
|
.K_P = 1.0f,
|
|
.K_W = 0.05,
|
|
.Pos = 0,
|
|
.W = 0,
|
|
.T = 0,
|
|
|
|
},
|
|
|
|
|
|
/*上层其他参数*/
|
|
/*运球*/
|
|
.DribbleConfig_Config = {
|
|
.m3508_init_angle = 50,
|
|
.m3508_high_angle = 1200,
|
|
.go2_init_angle = 0,
|
|
.go2_flip_angle = -250,
|
|
.flip_timing = 200,
|
|
.go2_release_threshold = -550.0f,
|
|
},
|
|
/*投球*/
|
|
.PitchConfig_Config = {
|
|
.m2006_init_angle =-170,
|
|
.m2006_trigger_angle =0,
|
|
.go1_init_position = -50,
|
|
.go1_release_threshold =-210,
|
|
.m2006_Screw_init=0,
|
|
},
|
|
|
|
|
|
},
|
|
.chassis = {/**/
|
|
|
|
|
|
.M3508_param = {
|
|
.p = 15.1f,
|
|
.i = 0.02f,
|
|
.d = 3.2f,
|
|
.i_limit = 200.0f,
|
|
.out_limit =6000.0f,
|
|
},
|
|
/*视觉*/
|
|
.chassis_PICKWzPID_HIGN_param ={ //高响应
|
|
.p = 1.0f,
|
|
.i = 0.03f,
|
|
.d = 0.03f,
|
|
.i_limit = 100.0f,
|
|
.out_limit =2000.0f,
|
|
},
|
|
.chassis_PICKWzPID_LOW_param ={ //高精度
|
|
.p = 0.5f, //1.0 0.5
|
|
.i = 0.5f, //0.01 0.04
|
|
.d = 0.0f, //0.02 0.02
|
|
.i_limit = 50.0f,
|
|
.out_limit =1000.0f,
|
|
},
|
|
|
|
|
|
},
|
|
|
|
|
|
.can = {
|
|
.pitch6020 = BSP_CAN_1,
|
|
.motor_CHASSIS3508 = BSP_CAN_1,
|
|
.motor_UP3508 = BSP_CAN_2,
|
|
|
|
// .chassis6020 = BSP_CAN_1,//禁用
|
|
.chassis5065 = BSP_CAN_1,
|
|
|
|
.sick = BSP_CAN_2,
|
|
.encoder=BSP_CAN_2,
|
|
|
|
},
|
|
|
|
};
|
|
const ConfigParam_t *Config_ChassisGet(void)
|
|
{
|
|
return ¶m;
|
|
}
|
|
|
|
///*获取导航地图*/
|
|
//void set_ops_path(ConfigParam_t *config, const point_t *path, int8_t path_num) {
|
|
// config->ops.path = path;
|
|
// config->ops.path_num = path_num;
|
|
//}
|
|
/**
|
|
* \brief 从Flash读取配置信息
|
|
*
|
|
* \param cfg 配置信息
|
|
*/
|
|
void Config_Get(Config_t *cfg) {
|
|
BSP_Flash_ReadBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
|
|
// /* 防止第一次烧写后访问NULL指针 */
|
|
cfg->config = ¶m;
|
|
if (cfg->config == NULL) cfg->config = ¶m;
|
|
/* 防止擦除后全为1 */
|
|
if ((uint32_t)(cfg->config) == UINT32_MAX)
|
|
cfg->config = ¶m;
|
|
|
|
if (memcmp(&cfg->cali_088.gyro_offset.x, "\xFF\xFF\xFF\xFF", 4) == 0) {
|
|
cfg->cali_088.gyro_offset.x = 0.0f;
|
|
}
|
|
if (memcmp(&cfg->cali_088.gyro_offset.y, "\xFF\xFF\xFF\xFF", 4) == 0) {
|
|
cfg->cali_088.gyro_offset.y = 0.0f;
|
|
}
|
|
if (memcmp(&cfg->cali_088.gyro_offset.z, "\xFF\xFF\xFF\xFF", 4) == 0) {
|
|
cfg->cali_088.gyro_offset.z = 0.0f;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* \brief 将配置信息写入Flash
|
|
*
|
|
* \param cfg 配置信息
|
|
*/
|
|
void Config_Set(Config_t *cfg) {
|
|
osKernelLock();
|
|
BSP_Flash_EraseSector(11);
|
|
BSP_Flash_WriteBytes(CONFIG_BASE_ADDRESS, (uint8_t *)cfg, sizeof(*cfg));
|
|
osKernelUnlock();
|
|
}
|