87 lines
2.4 KiB
C++
87 lines
2.4 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/(ticksPerSecond*2*3.14159*wheelRadius))
|
|
|
|
extern UART_HandleTypeDef huart1;
|
|
extern TIM_HandleTypeDef htim1;
|
|
extern TIM_HandleTypeDef htim2;
|
|
extern TIM_HandleTypeDef htim3;
|
|
extern TIM_HandleTypeDef htim4;
|
|
extern SerialDebug debug;
|
|
|
|
uint8_t buf[64];
|
|
|
|
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
|
|
sprintf((char*)buf, ">Speed:%+0.6e,%+0.6e,%+0.6e,%+0.6e",
|
|
motor0.getCurrentRevPerTick()/CONVERSION,
|
|
motor1.getCurrentRevPerTick()/CONVERSION,
|
|
desiredSpeed0,
|
|
desiredSpeed1);
|
|
HAL_UART_Transmit_DMA(&huart1, buf, 64);
|
|
}
|
|
|
|
void Robot::init(){
|
|
debug.info("Init ESP8266 begin");
|
|
HAL_Delay(10000);
|
|
debug.info("Init ESP8266 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_Encoder_Start(&htim1, TIM_CHANNEL_1);
|
|
HAL_TIM_Encoder_Start(&htim1, TIM_CHANNEL_ALL);
|
|
//HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_1);
|
|
HAL_TIM_Encoder_Start(&htim3, TIM_CHANNEL_ALL);
|
|
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);
|
|
}
|