/*
 * Robot.hpp
 *
 *  Created on: Mar 17, 2023
 *      Author: Gabriel
 */

#ifndef SRC_COMPONENTS_ROBOT_HPP_
#define SRC_COMPONENTS_ROBOT_HPP_

#include "main.h"
#include "BTS7960B.hpp"
#include "Encoder.hpp"
#include "MPU9250/MPU9250.h"

class Robot {
public:
	Robot();
	virtual ~Robot();
	void uartCallback();
	void controlCallback();
	void init();
private:
	void print_roll_pitch_yaw();
	const float reductionRatio = 30;
	const float wheelRadius = 0.073025;	//m
	const float ticksPerSecond = 100;
	uint8_t rxBuffer[1500] = "0.000000,0.000000";
	uint8_t txBuffer[1500];
	Encoder encoder0 = Encoder(&(TIM1->CNT), &(TIM1->ARR), 52);
	Encoder encoder1 = Encoder(&(TIM3->CNT), &(TIM3->ARR), 52);
	BTS7960B motor0 = BTS7960B(&(TIM2->CCR1), &(TIM2->CCR2), &(TIM2->ARR), &(TIM2->ARR), GPIOB, GPIO_PIN_4, GPIOB, GPIO_PIN_5);
	BTS7960B motor1 = BTS7960B(&(TIM2->CCR3), &(TIM2->CCR4), &(TIM2->ARR), &(TIM2->ARR), GPIOB, GPIO_PIN_0, GPIOB, GPIO_PIN_1);
	MPU9250 imu0;
};

#endif /* SRC_COMPONENTS_ROBOT_HPP_ */