555 lines
21 KiB
C++
555 lines
21 KiB
C++
#include "robot.h"
|
||
|
||
#include "board.h"
|
||
#include "usart_transport.h"
|
||
#include "simple_dataframe_slave.h"
|
||
#include "data_holder.h"
|
||
#include "usart.h"
|
||
|
||
|
||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||
#include "common_motor_controller.h"
|
||
#define MAX_PWM_VALUE 5000
|
||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||
#include "afs_motor_controller.h"
|
||
#define MAX_PWM_VALUE 255
|
||
#endif
|
||
|
||
#include "encoder_implement.h"
|
||
#include "pid.h"
|
||
#include "board_stm32.h"
|
||
|
||
#if IMU_ENABLE
|
||
#include "GY65.h"
|
||
#include "GY85.h"
|
||
#include "GY87.h"
|
||
#endif
|
||
|
||
#if JOYSTICK_ENABLE
|
||
#include "joystick.h"
|
||
#endif
|
||
|
||
#include "print.h"
|
||
|
||
#define min(a,b) ((a)<(b)?(a):(b))
|
||
#define max(a,b) ((a)>(b)?(a):(b))
|
||
|
||
Robot::Robot()
|
||
{
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
motor[i] = NULL;
|
||
encoder[i] = NULL;
|
||
pid[i] = NULL;
|
||
input[i] = 0.0f;
|
||
feedback[i] = 0.0f;
|
||
}
|
||
|
||
trans = NULL;
|
||
frame = NULL;
|
||
model = NULL;
|
||
do_kinmatics_flag = false;
|
||
last_velocity_command_time = 0;
|
||
|
||
do_set_pwm_flag = false;
|
||
|
||
#if IMU_ENABLE
|
||
imu = NULL;
|
||
#endif
|
||
|
||
#if JOYSTICK_ENABLE
|
||
joystick = NULL;
|
||
#endif
|
||
}
|
||
|
||
void Robot::Init()
|
||
{
|
||
// board初始化
|
||
Board::get()->init();
|
||
|
||
#if DEBUG_ENABLE
|
||
// 调试串口初始化
|
||
Board::get()->usart_debug_init();
|
||
#endif
|
||
|
||
// 加载参数, 该参数一般存储在flash或者eeprom
|
||
DataHolder::get()->loadParameter();
|
||
|
||
log("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
|
||
DataHolder::get()->parameter.params.wheel_diameter, DataHolder::get()->parameter.params.wheel_track, DataHolder::get()->parameter.params.encoder_resolution, DataHolder::get()->parameter.params.motor_ratio,
|
||
DataHolder::get()->parameter.params.do_pid_interval, DataHolder::get()->parameter.params.kp, DataHolder::get()->parameter.params.ki, DataHolder::get()->parameter.params.kd, DataHolder::get()->parameter.params.ko,
|
||
DataHolder::get()->parameter.params.cmd_last_time, DataHolder::get()->parameter.params.max_v_liner_x, DataHolder::get()->parameter.params.max_v_liner_y, DataHolder::get()->parameter.params.max_v_angular_z);
|
||
|
||
#if IMU_ENABLE
|
||
// imu 初始化
|
||
init_imu();
|
||
#endif
|
||
|
||
log("init_motor");
|
||
// 电机相关初始化
|
||
InitMotor();
|
||
|
||
log("init_trans");
|
||
// 通讯相关初始化
|
||
InitTrans();
|
||
|
||
// joystick初始化
|
||
InitJoystick();
|
||
|
||
log("gebot init finish.");
|
||
}
|
||
|
||
void Robot::init_imu()
|
||
{
|
||
#if IMU_ENABLE
|
||
if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY65) {
|
||
log("imu gy65");
|
||
static GY65 gy65;
|
||
imu = &gy65;
|
||
} else if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY85) {
|
||
log("imu gy85");
|
||
static GY85 gy85;
|
||
imu = &gy85;
|
||
} else if (DataHolder::get()->parameter.params.imu_type == IMU_TYPE_GY87) {
|
||
log("imu gy87");
|
||
static GY87 gy87;
|
||
imu = &gy87;
|
||
} else {
|
||
log("imu default null driver");
|
||
imu = NULL;
|
||
}
|
||
|
||
if (imu != NULL) {
|
||
imu->init();
|
||
}
|
||
#endif
|
||
}
|
||
|
||
void Robot::InitJoystick()
|
||
{
|
||
#if JOYSTICK_ENABLE
|
||
static Joystick joy;
|
||
joystick = &joy;
|
||
if (joystick)
|
||
joystick->init();
|
||
#endif
|
||
}
|
||
|
||
void Robot::InitMotor()
|
||
{
|
||
//MOTOR_COUNT为电机数量 该宏定义在相应的机器人模型文件中， KinematicModels/differential.h
|
||
#if MOTOR_COUNT>0 //电机1
|
||
// MOTOR_CONTROLLER在params.mk中定义 表示电机控制器类型
|
||
// 当前使用的COMMON_CONTROLLER为2个io控制方向一个PWD控制速度类型的控制器
|
||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||
static CommonMotorController motor1(MOTOR_1, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||
static AFSMotorController motor1(MOTOR_1_PORT_NUM, MAX_PWM_VALUE);
|
||
#endif
|
||
|
||
// 定义使用的编码器 传入参数为编码器的AB GPIO引脚 定义在params.mk中
|
||
static EncoderImp encoder1(MOTOR_1, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_1_FLAG)==0);
|
||
|
||
// 定义PID控制对象 传入参数为输入和输出的地址及kp ki kd 这里ko为比例
|
||
static PID pid1(&input[0], &feedback[0], float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||
#endif
|
||
|
||
#if MOTOR_COUNT>1 //电机2
|
||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||
static CommonMotorController motor2(MOTOR_2, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||
static AFSMotorController motor2(MOTOR_2_PORT_NUM, MAX_PWM_VALUE);
|
||
#endif
|
||
static EncoderImp encoder2(MOTOR_2, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_2_FLAG)==0);
|
||
static PID pid2(&input[1], &feedback[1], float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||
#endif
|
||
|
||
#if MOTOR_COUNT>2 //电机3
|
||
#if MOTOR_CONTROLLER == COMMON_CONTROLLER
|
||
static CommonMotorController motor3(MOTOR_3, MAX_PWM_VALUE, (DataHolder::get()->parameter.params.motor_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0); // MOTOR1_REVERS标识电机方向反向,等同于电机线交换
|
||
#elif MOTOR_CONTROLLER == AF_SHIELD_CONTROLLER
|
||
static AFSMotorController motor3(MOTOR_3);
|
||
#endif
|
||
|
||
static EncoderImp encoder3(MOTOR_3, (DataHolder::get()->parameter.params.encoder_nonexchange_flag & MOTOR_ENCODER_3_FLAG)==0);
|
||
static PID pid3(&input[2], &feedback[2], float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||
#endif
|
||
|
||
// 分别把地址保存在统一的数组内， 方便处理
|
||
#if MOTOR_COUNT>0
|
||
motor[0] = &motor1;
|
||
encoder[0] = &encoder1;
|
||
pid[0] = &pid1;
|
||
#endif
|
||
|
||
#if MOTOR_COUNT>1
|
||
motor[1] = &motor2;
|
||
encoder[1] = &encoder2;
|
||
pid[1] = &pid2;
|
||
#endif
|
||
|
||
#if MOTOR_COUNT>2
|
||
motor[2] = &motor3;
|
||
encoder[2] = &encoder3;
|
||
pid[2] = &pid3;
|
||
#endif
|
||
|
||
// 根据定义的模型参数ROBOT_MODEL， 创建模型对象及保存地址
|
||
#if ROBOT_MODEL == MODEL_TYPE_2WD_DIFF //2轮差分
|
||
static Differential diff(DataHolder::get()->parameter.params.wheel_diameter*0.0005, DataHolder::get()->parameter.params.wheel_track*0.0005);
|
||
model = &diff;
|
||
#endif
|
||
|
||
#if ROBOT_MODEL == MODEL_TYPE_3WD_OMNI //三轮全向
|
||
static Omni3 omni3(DataHolder::get()->parameter.params.wheel_diameter*0.0005, DataHolder::get()->parameter.params.wheel_track*0.0005);
|
||
model = &omni3;
|
||
#endif
|
||
|
||
// 分别初始化各个控制器
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
encoder[i]->init();
|
||
motor[i]->init();
|
||
}
|
||
|
||
// 初始化各个变量
|
||
do_kinmatics_flag = false;
|
||
|
||
memset(&odom, 0 , sizeof(odom));
|
||
memset(&input, 0 , sizeof(input));
|
||
memset(&feedback, 0 , sizeof(feedback));
|
||
|
||
last_velocity_command_time = 0;
|
||
}
|
||
|
||
void Robot::InitTrans()
|
||
{
|
||
//定义一个串口通讯对象 ！！！注意这里USART_transport继承实现Transport接口，
|
||
// 实现了一个通讯对象，这么做的原因是，如果我换为其他通讯方式，例如USB，只需要创建一个USB_Transport继承实现实现Transport接口就可以实现
|
||
static USART_transport _trans(MASTER_USART, 115200);
|
||
|
||
// 定义一个通讯协议处理对象， 同上， 我们需要更换协议只需要继承实现Dataframe的接口就可以实现
|
||
static Simple_dataframe _frame(&_trans);
|
||
|
||
// 保存对象指针
|
||
trans = &_trans;
|
||
frame = &_frame;
|
||
|
||
// 初始化
|
||
trans->init();
|
||
frame->init();
|
||
|
||
// 注册消息处理对象 ！！！ 这里把各个消息id的处理注册了一个观察者， 即再收到相应的消息后会回调update函数
|
||
// Robot(this)继承实现了notify
|
||
frame->register_notify(ID_SET_ROBOT_PARAMTER, this);
|
||
frame->register_notify(ID_CLEAR_ODOM, this);
|
||
frame->register_notify(ID_SET_VELOCITY, this);
|
||
frame->register_notify(ID_GET_ENCODER_COUNT, this);
|
||
frame->register_notify(ID_SET_MOTOR_PWM, this);
|
||
}
|
||
|
||
void Robot::CheckCommand()
|
||
{
|
||
static unsigned long lastMillis = 0;
|
||
if (Board::get()->getTickCount() - lastMillis >= 50){
|
||
lastMillis = Board::get()->getTickCount();
|
||
|
||
Board::get()->setDOState(_RUN_LED, 2);
|
||
}
|
||
|
||
uint8_t ch = 0;
|
||
|
||
// 从通讯设备(trans)读取数据交给协议处理模块(frame)处理
|
||
if (trans->read(ch)) {
|
||
if (frame->dataRecv(ch)) {
|
||
// 这里检测到正确的一帧命令，做解析
|
||
frame->dataParse();
|
||
}
|
||
}
|
||
}
|
||
|
||
// 实现Notify接口， 上面注册的命令id接收到时候，会回调，执行响应的动作
|
||
void Robot::update(const MESSAGE_ID id, void* data)
|
||
{
|
||
switch (id) {
|
||
case ID_SET_ROBOT_PARAMTER: //设置参数的回调， 这里会更新pid的参数， model的参数， 最终保存到flash
|
||
|
||
log("RobotParameters: %d %d %d %d %d %d %d %d %d %d %d %d %d",
|
||
DataHolder::get()->parameter.params.wheel_diameter, DataHolder::get()->parameter.params.wheel_track, DataHolder::get()->parameter.params.encoder_resolution, DataHolder::get()->parameter.params.motor_ratio,
|
||
DataHolder::get()->parameter.params.do_pid_interval, DataHolder::get()->parameter.params.kp, DataHolder::get()->parameter.params.ki, DataHolder::get()->parameter.params.kd, DataHolder::get()->parameter.params.ko,
|
||
DataHolder::get()->parameter.params.cmd_last_time, DataHolder::get()->parameter.params.max_v_liner_x, DataHolder::get()->parameter.params.max_v_liner_y, DataHolder::get()->parameter.params.max_v_angular_z);
|
||
// 更新pid参数， 这样可以保证设置参数实时生效
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
pid[i]->update(float(DataHolder::get()->parameter.params.kp)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.ki)/DataHolder::get()->parameter.params.ko,
|
||
float(DataHolder::get()->parameter.params.kd)/DataHolder::get()->parameter.params.ko , MAX_PWM_VALUE);
|
||
}
|
||
|
||
// 更新模型参数， 这样可以保证设置参数实时生效
|
||
model->set(DataHolder::get()->parameter.params.wheel_diameter*0.0005, DataHolder::get()->parameter.params.wheel_track*0.0005);
|
||
|
||
// 保存参数到flash
|
||
DataHolder::get()->save_parameter();
|
||
break;
|
||
case ID_CLEAR_ODOM: // 清除里程计信息
|
||
clear_odom();
|
||
break;
|
||
case ID_SET_VELOCITY: // 设置机器人的速度
|
||
updateVelocity(); //更新机器人的期望速度
|
||
break;
|
||
case ID_GET_ENCODER_COUNT:
|
||
update_encoder_count(); // 更新encoder count
|
||
break;
|
||
case ID_SET_MOTOR_PWM:
|
||
update_pwm_value(); // 设置pwm值
|
||
break;
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
|
||
// 清除里程计数据及相关变量
|
||
void Robot::clear_odom()
|
||
{
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
encoder[i]->clear();
|
||
}
|
||
|
||
memset(&odom, 0, sizeof(odom));
|
||
memset(&DataHolder::get()->odom, 0, sizeof(DataHolder::get()->odom));
|
||
}
|
||
|
||
#define __PI 3.1415926535897932384626433832795
|
||
|
||
// 根据下发的速度更新期望速度， 并转为pid时间间隔内的期望编码器的变化值
|
||
void Robot::updateVelocity()
|
||
{
|
||
// 下发速度检测 保证限制在设置的最大最小值内
|
||
short vx = min(max(DataHolder::get()->velocity.v_liner_x, -(short(DataHolder::get()->parameter.params.max_v_liner_x))), short(DataHolder::get()->parameter.params.max_v_liner_x));
|
||
short vy = min(max(DataHolder::get()->velocity.v_liner_y, -(short(DataHolder::get()->parameter.params.max_v_liner_y))), short(DataHolder::get()->parameter.params.max_v_liner_y));
|
||
short vz = min(max(DataHolder::get()->velocity.v_angular_z, -(short(DataHolder::get()->parameter.params.max_v_angular_z))), short(DataHolder::get()->parameter.params.max_v_angular_z));
|
||
|
||
// 保存速度转换单位,传入的速度值为cm/s 0.01rad/s 转为m/s 和rad/s
|
||
float vel[3]={vx/100.0, vy/100.0, vz/100.0};
|
||
float motor_speed[MOTOR_COUNT]={0};
|
||
|
||
model->motionSolver(vel, motor_speed); // 期望的机器人的速度转换为各个电机的速度(通过设置的机器人模型接口)
|
||
|
||
// 把得到的期望电机速度 (m/s)转换为期望pid时间间隔内编码器反馈的输入
|
||
// 该值即为PID的输入
|
||
for(int i = 0;i < MOTOR_COUNT; i++) {
|
||
input[i] = motor_speed[i]*short(DataHolder::get()->parameter.params.encoder_resolution)*short(DataHolder::get()->parameter.params.motor_ratio)/(2*__PI)*short(DataHolder::get()->parameter.params.do_pid_interval)*0.001;
|
||
}
|
||
|
||
//log("vx=%d %d motor_speed=%ld %ld", vx, vz, long(motor_speed[0]*1000), long(motor_speed[1]*1000));
|
||
|
||
//保存时间戳
|
||
last_velocity_command_time = Board::get()->getTickCount();
|
||
|
||
// 更新变量， 激活pid运算和控制电机
|
||
do_kinmatics_flag = true;
|
||
}
|
||
|
||
void Robot::update_encoder_count()
|
||
{
|
||
for (int i=0; i<MOTOR_COUNT; i++) {
|
||
DataHolder::get()->encoder_count[i] = encoder[i]->get_total_count();//输出累计编码器值 用于调试
|
||
}
|
||
}
|
||
|
||
void Robot::update_pwm_value()
|
||
{
|
||
do_set_pwm_flag = true;
|
||
}
|
||
|
||
// pid运算， 控制电机
|
||
void Robot::DoKinmatics()
|
||
{
|
||
if (do_set_pwm_flag) {
|
||
do_set_pwm_flag = false;
|
||
|
||
for (int i=0; i<MOTOR_COUNT; i++) {
|
||
motor[i]->control(DataHolder::get()->pwm.value[i]);
|
||
}
|
||
}
|
||
|
||
if (!do_kinmatics_flag) { // 停止后
|
||
for(int i=0;i<MOTOR_COUNT;i++) {
|
||
pid[i]->clear(); // 清除pid
|
||
encoder[i]->get_increment_count_for_dopid();// 实时更新用于pid的encoder值
|
||
}
|
||
return; //do_kinmatics_flag false表示电机停止 不需要做pid 故返回
|
||
}
|
||
|
||
static unsigned long last_millis=0;
|
||
// 判断时间间隔做pid运算
|
||
if (Board::get()->getTickCount()-last_millis>=DataHolder::get()->parameter.params.do_pid_interval) {
|
||
last_millis = Board::get()->getTickCount();
|
||
|
||
//得到编码器的反馈差值， 即在do_pid_interval这段间隔时间内编码器的差值， 该值作为pid的反馈
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
feedback[i] = encoder[i]->get_increment_count_for_dopid();
|
||
}
|
||
#if PID_DEBUG_OUTPUT
|
||
#if MOTOR_COUNT==2
|
||
log("input=%ld %ld feedback=%ld %ld", long(input[0]*1000), long(input[1]*1000),
|
||
long(feedback[0]), long(feedback[1]));
|
||
#endif
|
||
#if MOTOR_COUNT==3
|
||
log("input=%ld %ld %ld feedback=%ld %ld %ld", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000),
|
||
long(feedback[0]), long(feedback[1]), long(feedback[2]));
|
||
#endif
|
||
#if MOTOR_COUNT==4
|
||
log("input=%ld %ld %ld %ld feedback=%ld %ld %ld %ld", long(input[0]*1000), long(input[1]*1000), long(input[2]*1000), long(input[3]*1000),
|
||
long(feedback[0]), long(feedback[1]), long(feedback[2]), long(feedback[3]));
|
||
#endif
|
||
#endif
|
||
bool stoped=true;
|
||
//当期望和反馈都为0 置位stoped标志
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
if (input[i] != 0 || feedback[i] != 0) {
|
||
stoped = false;
|
||
break;
|
||
}
|
||
}
|
||
|
||
short output[MOTOR_COUNT]={0};
|
||
if (stoped) {
|
||
do_kinmatics_flag = false;
|
||
} else {
|
||
// 没有需要停止则做pid运算，得到out值，该值作为电机模块控制器输入
|
||
for(int i=0;i<MOTOR_COUNT;i++) {
|
||
output[i] = pid[i]->compute(DataHolder::get()->parameter.params.do_pid_interval*0.001);
|
||
}
|
||
}
|
||
|
||
//当次计算完成 重置变量
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
DataHolder::get()->pid_data.input[i] = int(input[i]);
|
||
DataHolder::get()->pid_data.output[i] = int(feedback[i]);
|
||
}
|
||
|
||
#if PID_DEBUG_OUTPUT
|
||
#if MOTOR_COUNT==2
|
||
log("output=%ld %ld", output[0], output[1]);
|
||
#endif
|
||
#if MOTOR_COUNT==3
|
||
log("output=%ld %ld %ld", output[0], output[1], output[2]);
|
||
#endif
|
||
#if MOTOR_COUNT==4
|
||
log("output=%ld %ld %ld %ld", output[0], output[1], output[2], output[3]);
|
||
#endif
|
||
#endif
|
||
|
||
long elapsed_ms = Board::get()->getTickCount()-last_velocity_command_time;
|
||
//判断上次下发的时间戳，如果超时，把各个电机期望的输入置零，pid下次会根据改期望计算pid输出转速，慢慢停止点击
|
||
if (elapsed_ms > DataHolder::get()->parameter.params.cmd_last_time) {
|
||
memset(input, 0, sizeof(input));
|
||
if (elapsed_ms > DataHolder::get()->parameter.params.cmd_last_time*2) {
|
||
memset(output, 0, sizeof(output));
|
||
}
|
||
}
|
||
|
||
// 把pid得到的值交给电机控制器，该值有符号表示正反转
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
motor[i]->control(output[i]);
|
||
}
|
||
}
|
||
}
|
||
|
||
//计算里程计
|
||
void Robot::CalcOdom()
|
||
{
|
||
static unsigned long last_millis=0;
|
||
//根据实际间隔计算轮子里程
|
||
if (Board::get()->getTickCount()-last_millis>=CALC_ODOM_INTERVAL) {
|
||
last_millis = Board::get()->getTickCount();
|
||
#if ODOM_DEBUG_OUTPUT
|
||
long total_count[MOTOR_COUNT]={0};
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
total_count[i] = encoder[i]->get_total_count(); //输出累计编码器值 用于调试
|
||
}
|
||
|
||
#if MOTOR_COUNT==2
|
||
log("total_count=[%ld %ld]", total_count[0], total_count[1]);
|
||
#endif
|
||
#if MOTOR_COUNT==3
|
||
log("total_count=[%ld %ld %ld]", total_count[0], total_count[1], total_count[2]);
|
||
#endif
|
||
#if MOTOR_COUNT==4
|
||
log("total_count=[%ld %ld %ld %ld]", total_count[0], total_count[1], total_count[2], total_count[3]);
|
||
#endif
|
||
#endif
|
||
float dis[MOTOR_COUNT] = {0};
|
||
for (int i=0;i<MOTOR_COUNT;i++) {
|
||
//根据CALC_ODOM_INTERVAL的间隔内的各个电机的编码器变化值，转换各个电机实际的里程
|
||
dis[i] = encoder[i]->get_increment_count_for_odom()*__PI*DataHolder::get()->parameter.params.wheel_diameter*0.001/DataHolder::get()->parameter.params.encoder_resolution/DataHolder::get()->parameter.params.motor_ratio;
|
||
#if ODOM_DEBUG_OUTPUT
|
||
log(" %ld ", long(dis[i]*1000000));
|
||
#endif
|
||
}
|
||
// 把计算得到的各个电机的里程转为为机器人里程(通过机器人运动模型对象)
|
||
model->get_odom(&odom, dis, CALC_ODOM_INTERVAL);
|
||
|
||
#if ODOM_DEBUG_OUTPUT
|
||
// 输出机器人的里程
|
||
log(" x=%ld y=%ld yaw=%ld", long(odom.x*1000), long(odom.y*1000), long(odom.z*1000));//mm
|
||
log("");
|
||
#endif
|
||
|
||
// 转换单位保存到DataHolder
|
||
DataHolder::get()->odom.v_liner_x = odom.vel_x*100; //转为cm/s
|
||
DataHolder::get()->odom.v_liner_y = odom.vel_y*100;
|
||
DataHolder::get()->odom.v_angular_z = odom.vel_z*100;
|
||
DataHolder::get()->odom.x = odom.x*100; //转为cm
|
||
DataHolder::get()->odom.y = odom.y*100;
|
||
DataHolder::get()->odom.yaw = long(odom.z*100)%628;//转为0.01rad 628为2pi*100
|
||
}
|
||
}
|
||
|
||
//获取imu数据
|
||
void Robot::GetImuData()
|
||
{
|
||
#if IMU_ENABLE
|
||
if (imu == NULL) {
|
||
return;
|
||
}
|
||
|
||
static unsigned long last_millis=0;
|
||
//按照设置的时间间隔获取imu数据
|
||
if (Board::get()->getTickCount()-last_millis>=CALC_IMU_INTERVAL) {
|
||
last_millis = Board::get()->getTickCount();
|
||
imu->get_data(DataHolder::get()->imu_data);
|
||
}
|
||
#endif
|
||
}
|
||
|
||
//检测joystick
|
||
void Robot::CheckJoystick()
|
||
{
|
||
#if JOYSTICK_ENABLE
|
||
if (joystick == NULL) {
|
||
return;
|
||
}
|
||
|
||
static unsigned long last_millis=0;
|
||
short liner_x=0, liner_y=0, angular_z=0;
|
||
if (Board::get()->getTickCount()-last_millis>=CHECK_JOYSTICK_INTERVAL){
|
||
last_millis = Board::get()->getTickCount();
|
||
// 按照设置的间隔间隔 更新期望速度
|
||
if (joystick->check(liner_x, liner_y, angular_z)) {
|
||
DataHolder::get()->velocity.v_liner_x = liner_x;
|
||
DataHolder::get()->velocity.v_liner_y = liner_y;
|
||
DataHolder::get()->velocity.v_angular_z = angular_z;
|
||
update_velocity();
|
||
}
|
||
|
||
joystick->test();
|
||
}
|
||
#endif
|
||
}
|