#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);
	}
}