#include "common_inc.h" #include "configurations.h" #include "Platform/Utils/st_hardware.h" #include /* 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(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; } }