STM32基于HAL库TMC5130\5160驱动程序

#include "stm32f4xx_hal.h" #include "../App/Modules/DeviceManager.h" #include <stdlib.h> #define SPI_NSS_Start(gpio, pin) HAL_GPIO_WritePin(gpio, pin, GPIO_PIN_RESET) #define SPI_NSS_Stop(gpio, pin) HAL_GPIO_WritePin(gpio, pin, GPIO_PIN_SET) #define _currSpdParam _spdParams[SpdMod_Total] ErrorCode CStepMotor::SetAutoStopLeft(bool enable) // 左零位自动停止功能（复位） { Reg0x34* pReg0x34 = (Reg0x34*)&_regValue0x34; if (enable != pReg0x34->stop_l_enable) { pReg0x34->stop_l_enable = enable; return WriteReg(TMC5160_REGADDR_SW_MODE, _regValue0x34); } else return EC_False; } ErrorCode CStepMotor::SetAutoStopRight(bool enable) // 右零位自动停止功能（液面探测，扎针等异常情况） { Reg0x34* pReg0x34 = (Reg0x34*)&_regValue0x34; _status.autoStop = enable; if (enable != pReg0x34->stop_r_enable) { pReg0x34->stop_r_enable = enable; return WriteReg(TMC5160_REGADDR_SW_MODE, _regValue0x34); } else return EC_False; } ErrorCode CStepMotor::SetAutoStopStall(bool enable) // 堵转检测功能 { Reg0x34* pReg0x34 = (Reg0x34*)&_regValue0x34; if (enable != pReg0x34->sg_stop) { pReg0x34->sg_stop = enable; return WriteReg(TMC5160_REGADDR_SW_MODE, _regValue0x34); } else return EC_False; } #define CHOPCONF_INTPOL_MASK (0x1 << 28) CStepMotor::CStepMotor(SPI_HandleTypeDef hspi, GPIO_TypeDef *gpio, u32 pin, OutputPortID opid_enable, bool is5160) { _hspi = hspi; _gpio = gpio; _pin = pin; _resetStage = SMRS_HighSpeedSearchSensor; _currMode = TMC_Mode_Invalid; _currSpdMod = SpdMod_Invalid; _lastVMax = 0; _regValue0x34 = 0; _status.isRunning = false; _status.isResetted = false; _status.isResetting = false; _opEnable = DeviceManager->GetOutputPort(opid_enable); _direction = true; _dirReset = false; _position._posCurrent = -1; _position._posTarget = -1; _position._stepsIn = 10; _position._stepsOut = 20; _position._posHome = 0; Reg0x34* pReg0x34 = (Reg0x34*)&_regValue0x34; pReg0x34->pol_stop_l = true; pReg0x34->pol_stop_r = true; pReg0x34->swap_lr = true; pReg0x34->stop_l_enable = true; Enable(); WriteReg(TMC5160_REGADDR_CHOPCONF, (0x000100C3 | CHOPCONF_INTPOL_MASK)); // PAGE46:CHOPCONF: TOFF=3, HSTRT=4, HEND=1,TBL=2, CHM=0 (spreadcycle) WriteReg(TMC5160_REGADDR_TPOWERDOWN, 0x0000000A); // PAGE33:TPOWERDOWN=10:电机静止到电流减小之间的延时 WriteReg(TMC5160_REGADDR_GCONF, 0x00000004); // PAGE27:EN_PWM_MODE=1 WriteReg(TMC5160_REGADDR_PWMCONF, 0x000C0000 | (1 << 20)); // PAGE43:PWMCONF WriteReg(TMC5160_REGADDR_SW_MODE, _regValue0x34); WriteReg(TMC5160_REGADDR_COOLCONF, ((1<<24 | 1<<15 | 1<<13 | 1 << 5 | 5))); //WriteReg(0x2C, 2000); SetSubdivision(TMC_Subdivition_64); SetDirection(_direction); u32 pps = 64 * 200 * 60 / 60; WriteReg(TMC5160_REGADDR_TPWMTHRS, pps); // PAGE33:TPWM_THRS=500,对应切换速度35000=ca.30RPM if (is5160) { SetSpeedParams(SpdMod_Normal, 50, 400, 5000, 300, pps, 500, 400, 600); SetSpeedParams(SpdMod_Manual, 50, 40, 200, 30, 400, 35, 50, 60); SetSpeedParams(SpdMod_Fast, 50, 500, 8000, 400, 12000, 600, 800, 100); SetSpeedParams(SpdMod_Reset, 20, 20, 100, 30, 200, 60, 50, 30); WriteReg(TMC5160_REGADDR_TCOOLTHRS, 350); _regCurrent.work = 10; _regCurrent.hold = 0; _regCurrent.delay = 3; UpdateCurrent(); } else { SetSpeedParams(SpdMod_Normal, 50, 50, 1000, 80, 2000, 90, 120, 80); //SetSpeedParams(SpdMod_Normal, 50, 300, 12000, 200, 20000, 150, 350, 80); SetSpeedParams(SpdMod_Fast, 50, 500, 8000, 400, 12000, 600, 800, 100); SetSpeedParams(SpdMod_Reset, 50, 50, 800, 80, 1000, 90, 120, 80); WriteReg(TMC5160_REGADDR_TCOOLTHRS, 50); _regCurrent.work = 10; _regCurrent.hold = 2; _regCurrent.delay = 3; UpdateCurrent(); } SetAutoStopStall(true); Stop(); SetSpeedMode(SpdMod_Normal); } ErrorCode CStepMotor::SetParam(u8 pid, u32 val) { ErrorCode ec = EC_NoError; if (pid < PID_SM_Total) { switch ((ParameterID)pid) { case PID_SM_Direction: ec = SetDirection(!!val); break; case PID_SM_Current_Work: ec = SetCurrentWork(val); break; case PID_SM_Current_Static: ec = SetCurrentHold(val); break; case PID_SM_Subdivide: if (val < TMC_subdivition_Total) ec = SetSubdivision((TMCSubdivision)val); else ec = EC_InvalidParam; break; case PID_SM_Stall_Detect: SetAutoStopStall(!!val); break; case PID_SM_Stall_Threshold: // TODO break; case PID_SM_Normal_VStart: ec = SetVStart(SpdMod_Normal, val); break; case PID_SM_Normal_V1: ec = SetV1(SpdMod_Normal, val); break; case PID_SM_Normal_A1: ec = SetA1(SpdMod_Normal, val); break; case PID_SM_Normal_VMax: ec = SetVMax(SpdMod_Normal, val); break; case PID_SM_Normal_AMax: ec = SetAMax(SpdMod_Normal, val); break; case PID_SM_Normal_DMax: ec = SetDMax(SpdMod_Normal, val); break; case PID_SM_Normal_VStop: ec = SetVStop(SpdMod_Normal, val); break; case PID_SM_Normal_D1: ec = SetD1(SpdMod_Normal, val); break; default: ec = EC_InvalidParam; break; } } else ec = EC_InvalidParam; return ec; } #define DIRECTION_MASK (0x1 << 4) ErrorCode CStepMotor::SetDirection(bool forward) { ErrorCode ec = EC_NoError; do { u32 temp = 0; bool needWriteReg = false; if (ActionFailed(ec = ReadReg(TMC5160_REGADDR_GCONF, &temp))) break; if (forward) { if ((temp & DIRECTION_MASK) == DIRECTION_MASK) { temp &= ~DIRECTION_MASK; needWriteReg = true; } } else if ((temp & DIRECTION_MASK) == 0) { temp |= DIRECTION_MASK; needWriteReg = true; } if (needWriteReg && ActionFailed(ec = WriteReg(TMC5160_REGADDR_GCONF, temp))) break; _direction = forward; } while (0); return ec; } ErrorCode CStepMotor::SetSpeedParams(SpeedMode spdMod, u32 VStart, u32 A1, u32 V1, u32 AMax, u32 VMax, u32 D1, u32 DMax, u32 VStop) { ErrorCode ec = EC_NoError; if (spdMod > SpdMod_Invalid && spdMod < SpdMod_Total) { TMCSpeedParam* p = &_spdParams[spdMod]; p->VStart = VStart; p->A1 = A1; p->V1 = V1; p->AMax = AMax; p->VMax = VMax; p->D1 = D1; p->DMax = DMax; p->VStop = VStop; if (_currSpdMod == spdMod) ec = SetSpeedMode(_currSpdMod); else ec = EC_False; } else ec = EC_InvalidParam; return ec; } ErrorCode CStepMotor::Enable() { ErrorCode ec = EC_False; if (!_status.isEnable) { ec = _opEnable->On(); //WriteReg(TMC5160_REGADDR_IHOLD_IRUN, 0x00061F0A); // PAGE33:IHOLD_IRUN: IHOLD=10, IRUN=31(max.current), IHOLDDELAY=6 _status.isEnable = true; } return ec; } ErrorCode CStepMotor::Disable() { ErrorCode ec = EC_False; if (_status.isEnable) {