Dummy-Robot/2.Firmware/Ctrl-Step-Driver-STM32F1-fw/UserApp/main.cpp

208 lines
6.1 KiB
C++

#include "common_inc.h"
#include "configurations.h"
#include "Platform/Utils/st_hardware.h"
#include <tim.h>
/* Component Definitions -----------------------------------------------------*/
BoardConfig_t boardConfig;
Motor motor;
TB67H450 tb67H450;
MT6816 mt6816;
EncoderCalibrator encoderCalibrator(&motor);
Button button1(1, 1000), button2(2, 3000);
void OnButton1Event(Button::Event _event);
void OnButton2Event(Button::Event _event);
Led statusLed;
/* Main Entry ----------------------------------------------------------------*/
void Main()
{
uint64_t serialNum = GetSerialNumber();
uint16_t defaultNodeID = 0;
// Change below to fit your situation
switch (serialNum)
{
case 431466563640: //J1
defaultNodeID = 1;
break;
case 384624576568: //J2
defaultNodeID = 2;
break;
case 384290670648: //J3
defaultNodeID = 3;
break;
case 431531051064: //J4
defaultNodeID = 4;
break;
case 431466760248: //J5
defaultNodeID = 5;
break;
case 431484848184: //J6
defaultNodeID = 6;
break;
default:
break;
}
/*---------- Apply EEPROM Settings ----------*/
// Setting priority is EEPROM > Motor.h
EEPROM eeprom;
eeprom.get(0, boardConfig);
if (boardConfig.configStatus != CONFIG_OK) // use default settings
{
boardConfig = BoardConfig_t{
.configStatus = CONFIG_OK,
.canNodeId = defaultNodeID,
.encoderHomeOffset = 0,
.defaultMode = Motor::MODE_COMMAND_POSITION,
.currentLimit = 1 * 1000, // A
.velocityLimit = 30 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS, // r/s
.velocityAcc = 100 * motor.MOTOR_ONE_CIRCLE_SUBDIVIDE_STEPS, // r/s^2
.calibrationCurrent=2000,
.dce_kp = 200,
.dce_kv = 80,
.dce_ki = 300,
.dce_kd = 250,
.enableMotorOnBoot=false,
.enableStallProtect=false
};
eeprom.put(0, boardConfig);
}
motor.config.motionParams.encoderHomeOffset = boardConfig.encoderHomeOffset;
motor.config.motionParams.ratedCurrent = boardConfig.currentLimit;
motor.config.motionParams.ratedVelocity = boardConfig.velocityLimit;
motor.config.motionParams.ratedVelocityAcc = boardConfig.velocityAcc;
motor.motionPlanner.velocityTracker.SetVelocityAcc(boardConfig.velocityAcc);
motor.motionPlanner.positionTracker.SetVelocityAcc(boardConfig.velocityAcc);
motor.config.motionParams.caliCurrent = boardConfig.calibrationCurrent;
motor.config.ctrlParams.dce.kp = boardConfig.dce_kp;
motor.config.ctrlParams.dce.kv = boardConfig.dce_kv;
motor.config.ctrlParams.dce.ki = boardConfig.dce_ki;
motor.config.ctrlParams.dce.kd = boardConfig.dce_kd;
motor.config.ctrlParams.stallProtectSwitch = boardConfig.enableStallProtect;
/*---------------- Init Motor ----------------*/
motor.AttachDriver(&tb67H450);
motor.AttachEncoder(&mt6816);
motor.controller->Init();
motor.driver->Init();
motor.encoder->Init();
/*------------- Init peripherals -------------*/
button1.SetOnEventListener(OnButton1Event);
button2.SetOnEventListener(OnButton2Event);
/*------- Start Close-Loop Control Tick ------*/
HAL_Delay(100);
HAL_TIM_Base_Start_IT(&htim1); // 100Hz
HAL_TIM_Base_Start_IT(&htim4); // 20kHz
if (button1.IsPressed() && button2.IsPressed())
encoderCalibrator.isTriggered = true;
for (;;)
{
encoderCalibrator.TickMainLoop();
if (boardConfig.configStatus == CONFIG_COMMIT)
{
boardConfig.configStatus = CONFIG_OK;
eeprom.put(0, boardConfig);
} else if (boardConfig.configStatus == CONFIG_RESTORE)
{
eeprom.put(0, boardConfig);
HAL_NVIC_SystemReset();
}
}
}
/* Event Callbacks -----------------------------------------------------------*/
extern "C" void Tim1Callback100Hz()
{
__HAL_TIM_CLEAR_IT(&htim1, TIM_IT_UPDATE);
button1.Tick(10);
button2.Tick(10);
statusLed.Tick(10, motor.controller->state);
}
extern "C" void Tim4Callback20kHz()
{
__HAL_TIM_CLEAR_IT(&htim4, TIM_IT_UPDATE);
if (encoderCalibrator.isTriggered)
encoderCalibrator.Tick20kHz();
else
motor.Tick20kHz();
}
void OnButton1Event(Button::Event _event)
{
switch (_event)
{
case ButtonBase::UP:
break;
case ButtonBase::DOWN:
break;
case ButtonBase::LONG_PRESS:
HAL_NVIC_SystemReset();
break;
case ButtonBase::CLICK:
if (motor.controller->modeRunning != Motor::MODE_STOP)
{
boardConfig.defaultMode = motor.controller->modeRunning;
motor.controller->requestMode = Motor::MODE_STOP;
} else
{
motor.controller->requestMode = static_cast<Motor::Mode_t>(boardConfig.defaultMode);
}
break;
}
}
void OnButton2Event(Button::Event _event)
{
switch (_event)
{
case ButtonBase::UP:
break;
case ButtonBase::DOWN:
break;
case ButtonBase::LONG_PRESS:
switch (motor.controller->modeRunning)
{
case Motor::MODE_COMMAND_CURRENT:
case Motor::MODE_PWM_CURRENT:
motor.controller->SetCurrentSetPoint(0);
break;
case Motor::MODE_COMMAND_VELOCITY:
case Motor::MODE_PWM_VELOCITY:
motor.controller->SetVelocitySetPoint(0);
break;
case Motor::MODE_COMMAND_POSITION:
case Motor::MODE_PWM_POSITION:
motor.controller->SetPositionSetPoint(0);
break;
case Motor::MODE_COMMAND_Trajectory:
case Motor::MODE_STEP_DIR:
case Motor::MODE_STOP:
break;
}
break;
case ButtonBase::CLICK:
motor.controller->ClearStallFlag();
break;
}
}