forked from logzhan/RobotHardware-UESTC
37 lines
866 B
C++
37 lines
866 B
C++
|
#include "robot.h"
|
||
|
|
#include "delay.h"
|
||
|
|
#include "board.h"
|
||
|
|
#include "print.h"
|
||
|
|
int main(void)
|
||
|
|
{
|
||
|
|
//初始化
|
||
|
|
Robot::get()->init();
|
||
|
|
|
||
|
|
while (1) {
|
||
|
|
// 检测接收的命令
|
||
|
|
Robot::get()->check_command();
|
||
|
|
|
||
|
|
// 运动解算 把上位机下发的机器人的速度（线速度角速度）转为各个轮子的速度
|
||
|
|
Robot::get()->do_kinmatics();
|
||
|
|
|
||
|
|
// 根据轮子的编码器计算得到机器人的里程计
|
||
|
|
Robot::get()->calc_odom();
|
||
|
|
|
||
|
|
// 读取imu的值
|
||
|
|
Robot::get()->get_imu_data();
|
||
|
|
|
||
|
|
// 检测处理ps2的按键信息
|
||
|
|
Robot::get()->check_joystick();
|
||
|
|
|
||
|
|
|
||
|
|
//// Board::get()->motor_pwm(0, 2479);
|
||
|
|
//// Board::get()->motor_pwm(1, -2107);
|
||
|
|
//
|
||
|
|
// int count_left = Board::get()->get_encoder_count(0);
|
||
|
|
// int count_right = Board::get()->get_encoder_count(1);
|
||
|
|
//
|
||
|
|
// pb_printf("left = %d, right = %d\n", count_left, count_right);
|
||
|
|
// delay_ms(10);
|
||
|
|
}
|
||
|
|
}
|