120 lines
3.2 KiB
C++
120 lines
3.2 KiB
C++
/*
|
|
* Robot.cpp
|
|
*
|
|
* Created on: Mar 17, 2023
|
|
* Author: Gabriel
|
|
*/
|
|
|
|
#include "Robot.hpp"
|
|
#include <cstdlib>
|
|
#include "SerialDebug.hpp"
|
|
#include "Definitions.hpp"
|
|
|
|
#define CONVERSION (reductionRatio/wheelRadius)
|
|
|
|
extern UART_HandleTypeDef huart1;
|
|
extern TIM_HandleTypeDef htim2;
|
|
extern TIM_HandleTypeDef htim4;
|
|
extern I2C_HandleTypeDef hi2c1;
|
|
extern SerialDebug debug;
|
|
|
|
uint8_t buf[92];
|
|
|
|
Robot::Robot() {
|
|
|
|
}
|
|
|
|
Robot::~Robot() {
|
|
|
|
}
|
|
|
|
void Robot::uartCallback(){
|
|
HAL_UARTEx_ReceiveToIdle_DMA(&huart1, rxBuffer, 256);
|
|
}
|
|
|
|
void Robot::controlCallback(){
|
|
HAL_GPIO_TogglePin(LED_BUILTIN_GPIO_Port, LED_BUILTIN_Pin);
|
|
//float desiredSpeed0 = strtof((const char*)rxBuffer, NULL);
|
|
float desiredSpeed0;
|
|
float desiredSpeed1;
|
|
sscanf((char*)rxBuffer, "%f,%f", &desiredSpeed0, &desiredSpeed1); // Desired speed in mm/s
|
|
float convertedSpeed0 = desiredSpeed0*CONVERSION; // Converted speed in rev/tick
|
|
float convertedSpeed1 = desiredSpeed1*CONVERSION;
|
|
#ifdef CONTROL_DISABLED
|
|
motor0.setPower(desiredSpeed0);
|
|
motor1.setPower(desiredSpeed1);
|
|
#else
|
|
motor0.pid(convertedSpeed0);
|
|
motor1.pid(convertedSpeed1);
|
|
#endif
|
|
imu0.update();
|
|
float imux = imu0.getAccX();
|
|
float imuy = imu0.getAccY();
|
|
if (isnan(imux)){
|
|
imux = 0;
|
|
}
|
|
if (isnan(imuy)){
|
|
imuy = 0;
|
|
}
|
|
sprintf((char*)buf, ">Speed:%+0.6e,%+0.6e,%+0.6e,%+0.6e,%+0.6e,%+0.6e",
|
|
motor0.getCurrentRadPerSecond()/CONVERSION,
|
|
motor1.getCurrentRadPerSecond()/CONVERSION,
|
|
desiredSpeed0,
|
|
desiredSpeed1,
|
|
imux,
|
|
imuy);
|
|
HAL_UART_Transmit_DMA(&huart1, buf, 92);
|
|
}
|
|
|
|
void Robot::print_roll_pitch_yaw() {
|
|
static char printbuf[128];
|
|
sprintf(printbuf, "X %f, Y %f, Z %f", imu0.getAccX(), imu0.getAccY(), imu0.getAccZ());
|
|
debug.debug(printbuf);
|
|
}
|
|
|
|
void Robot::init(){
|
|
debug.setLevel(SerialDebug::DEBUG_LEVEL_DEBUG);
|
|
debug.info("Init DWT begin");
|
|
CoreDebug->DEMCR |= 0x01000000; // TRCENA
|
|
DWT->CYCCNT = 0; // reset the counter
|
|
DWT->CTRL |= 1 ; // enable the counter
|
|
debug.info("Init DWT end");
|
|
debug.info("Init ESP8266 begin");
|
|
HAL_Delay(10000);
|
|
debug.info("Init ESP8266 end");
|
|
debug.info("Init IMU begin");
|
|
imu0.setup(0x68, MPU9250Setting(), &hi2c1);
|
|
imu0.calibrateAccelGyro();
|
|
debug.info("Init IMU end");
|
|
motor0.setEncoder(&encoder0);
|
|
motor1.setEncoder(&encoder1);
|
|
motor0.kp = 1.3016/CONVERSION;
|
|
motor0.ki = 22.3758/(ticksPerSecond*CONVERSION);
|
|
motor0.kd = 0*ticksPerSecond/CONVERSION;
|
|
motor1.kp = 1.3016/CONVERSION;
|
|
motor1.ki = 22.3758/(ticksPerSecond*CONVERSION);
|
|
motor1.kd = 0*ticksPerSecond/CONVERSION;
|
|
debug.info("Init timers begin");
|
|
HAL_TIM_Base_Start(&htim2);
|
|
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
|
|
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_2);
|
|
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_3);
|
|
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_4);
|
|
HAL_TIM_Base_Start_IT(&htim4);
|
|
HAL_UARTEx_ReceiveToIdle_DMA(&huart1, rxBuffer, 256);
|
|
debug.info("Init timers end");
|
|
motor0.setEnabled(true);
|
|
motor1.setEnabled(true);
|
|
//Begin gambiarrra
|
|
/*while(1){
|
|
if (imu0.update()) {
|
|
static uint32_t prev_ms = HAL_GetTick();
|
|
if (HAL_GetTick() > prev_ms + 100) {
|
|
print_roll_pitch_yaw();
|
|
prev_ms = HAL_GetTick();
|
|
}
|
|
}
|
|
}*/
|
|
//End gambiarra
|
|
}
|