95 lines
2.4 KiB
C++
95 lines
2.4 KiB
C++
#include "Start.hpp"
|
|
#include "SerialDebug.hpp"
|
|
#include "ESP8266.hpp"
|
|
#include "BTS7960B.hpp"
|
|
#include <cstring>
|
|
|
|
extern UART_HandleTypeDef huart1;
|
|
extern UART_HandleTypeDef huart2;
|
|
extern TIM_HandleTypeDef htim1;
|
|
extern TIM_HandleTypeDef htim2;
|
|
extern TIM_HandleTypeDef htim3;
|
|
extern TIM_HandleTypeDef htim4;
|
|
|
|
SerialDebug debug(&huart2, 32);
|
|
//ESP8266 esp0(&huart1);
|
|
BTS7960B motor0(&(TIM2->CCR1), &(TIM2->CCR2), GPIOB, GPIO_PIN_4, GPIOB, GPIO_PIN_5);
|
|
BTS7960B motor1(&(TIM2->CCR3), &(TIM2->CCR4), GPIOB, GPIO_PIN_0, GPIOB, GPIO_PIN_1);
|
|
|
|
//Temporary variables begin
|
|
uint8_t buf[1500];
|
|
float error = 0;
|
|
float lastError = 0;
|
|
float derror = 0;
|
|
float ierror = 0;
|
|
float deltaT = 33;
|
|
float kp = 16.0715;
|
|
float ki = 0.028822;
|
|
float kd = 1750.3479;
|
|
//Temporary Variables end
|
|
|
|
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim){
|
|
if(htim==&htim4){
|
|
|
|
}
|
|
}
|
|
|
|
void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) {
|
|
if(huart == &huart2){
|
|
debug.serialTxCpltCallback();
|
|
}
|
|
}
|
|
|
|
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {
|
|
|
|
}
|
|
|
|
void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size){
|
|
HAL_GPIO_TogglePin(LED_BUILTIN_GPIO_Port, LED_BUILTIN_Pin);
|
|
HAL_UARTEx_ReceiveToIdle_DMA(&huart1, buf, 256);
|
|
}
|
|
|
|
void pid(int32_t input){
|
|
lastError = error;
|
|
error = input - (int32_t)TIM3->CNT;
|
|
derror = error - lastError;
|
|
ierror += error;
|
|
if(ierror > 2047){
|
|
ierror = 2047;
|
|
}else if(ierror < -2048){
|
|
ierror = -2048;
|
|
}
|
|
float pid = kp*error + ki*ierror*deltaT + kd*derror/deltaT;
|
|
if(pid > 2047){
|
|
pid = 2047;
|
|
}else if(pid<-2047){
|
|
pid = -2047;
|
|
}
|
|
motor1.setSpeed((int32_t)pid);
|
|
}
|
|
|
|
void start(){
|
|
debug.setLevel(SerialDebug::DEBUG_LEVEL_INFO);
|
|
debug.info("-----Init-----");
|
|
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(&htim4);
|
|
debug.info("Init timers end");
|
|
debug.info("Init ESP8266 begin");
|
|
HAL_Delay(1000);
|
|
HAL_UARTEx_ReceiveToIdle_DMA(&huart1, buf, 256);
|
|
debug.info("Init ESP8266 end");
|
|
while(true){
|
|
|
|
}
|
|
}
|
|
|