forked from logzhan/RobotHardware-UESTC
583 lines
21 KiB
C++
583 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_DebugInit();
|
||
#endif
|
||
|
||
// 加载参数, 该参数一般存储在flash或者eeprom
|
||
DataHolder::get()->loadParameter();
|
||
|
||
logi("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 SimpleDataFrame _frame(&_trans);
|
||
|
||
// 保存对象指针
|
||
trans = &_trans;
|
||
frame = &_frame;
|
||
|
||
// 初始化
|
||
trans->init();
|
||
frame->init();
|
||
|
||
// 注册消息处理对象 ！！！ 这里把各个消息id的处理注册了一个观察者， 即再收到相应的消息后会回调update函数
|
||
// Robot(this)继承实现了notify
|
||
frame->RegisterNotify(ID_SET_ROBOT_PARAMTER, this);
|
||
frame->RegisterNotify(ID_CLEAR_ODOM, this);
|
||
frame->RegisterNotify(ID_SET_VELOCITY, this);
|
||
frame->RegisterNotify(ID_GET_ENCODER_COUNT, this);
|
||
frame->RegisterNotify(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:
|
||
// 更新encoder count
|
||
update_encoder_count();
|
||
break;
|
||
case ID_SET_MOTOR_PWM:
|
||
// 设置pwm值
|
||
update_pwm_value();
|
||
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
|
||
pid[i]->Clear();
|
||
// 实时更新用于pid的encoder值
|
||
encoder[i]->get_increment_count_for_dopid();
|
||
}
|
||
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
|
||
log("x=%ld cm/s y=%ld cm/s yaw=%ld", long(odom.vel_x * 100), long(odom.vel_y * 100), long(odom.z * 1000)); // mm
|
||
|
||
|
||
|
||
// 转换单位保存到DataHolder
|
||
DataHolder::get()->odom.v_liner_x = odom.vel_x * 100; // 转为cm/s
|
||
DataHolder::get()->odom.v_liner_y = odom.vel_y * 100; // 转为cm/s
|
||
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->GetData(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
|
||
}
|