![]() |
Open FFBoard
Open source force feedback firmware
|
#include <TMC4671.h>
Classes | |
class | TMC_ExternalEncoderUpdateThread |
Public Member Functions | |
const ClassIdentifier | getInfo () |
Command handlers always have class infos. Works well with ChoosableClass. More... | |
const ClassType | getClassType () override |
TMC4671 (SPIPort &spiport, OutputPin cspin, uint8_t address=1) | |
void | setHwType (TMC_HW_Ver type) |
void | setAddress (uint8_t address) |
uint8_t | getSpiAddr () |
bool | setSpiAddr (uint8_t chan) |
virtual | ~TMC4671 () |
bool | initialize () |
void | initializeWithPower () |
void | Run () |
bool | motorReady () |
bool | hasPower () |
int32_t | getTmcVM () |
bool | isSetUp () |
uint32_t | readReg (uint8_t reg) |
void | writeReg (uint8_t reg, uint32_t dat) |
void | writeRegAsync (uint8_t reg, uint32_t dat) |
void | updateReg (uint8_t reg, uint32_t dat, uint32_t mask, uint8_t shift) |
void | setMotorType (MotorType motor, uint16_t poles) |
void | runOpenLoop (uint16_t ud, uint16_t uq, int32_t speed, int32_t accel, bool torqueMode=false) |
void | setOpenLoopSpeedAccel (int32_t speed, uint32_t accel) |
bool | calibrateAdcOffset (uint16_t time=500) |
void | setup_ABN_Enc (TMC4671ABNConf encconf) |
void | setup_AENC (TMC4671AENCConf encconf) |
void | setup_HALL (TMC4671HALLConf hallconf) |
void | bangInitEnc (int16_t power) |
void | estimateABNparams () |
void | estimateExtEnc () |
bool | checkEncoder () |
void | calibrateAenc () |
void | calibrateEncoder () |
void | setEncoderType (EncoderType_TMC type) |
uint32_t | getEncCpr () |
void | setAdcOffset (uint32_t adc_I0_offset, uint32_t adc_I1_offset) |
void | setAdcScale (uint32_t adc_I0_scale, uint32_t adc_I1_scale) |
void | setupFeedForwardTorque (int32_t gain, int32_t constant) |
void | setupFeedForwardVelocity (int32_t gain, int32_t constant) |
void | setFFMode (FFMode mode) |
void | setSequentialPI (bool sequential) |
void | setBiquadFlux (const TMC4671Biquad &filter) |
void | setBiquadTorque (const TMC4671Biquad &filter) |
void | setBiquadPos (const TMC4671Biquad &filter) |
void | setBiquadVel (const TMC4671Biquad &filter) |
void | setTorqueFilter (TMC4671Biquad_conf &conf) |
bool | pingDriver () |
std::pair< uint32_t, std::string > | getTmcType () |
void | changeState (TMC_ControlState newState, bool force=false) |
bool | externalEncoderAllowed () |
void | setExternalEncoderAllowed (bool allow) |
float | getPwmFreq () |
void | setPwmFreq (float freq) |
void | setBBM (uint8_t bbml, uint8_t bbmh) |
void | timerElapsed (TIM_HandleTypeDef *htim) |
void | setPositionExt (int32_t pos) |
void | stopMotor () |
void | startMotor () |
void | emergencyStop (bool reset) |
void | turn (int16_t power) |
int16_t | controlFluxDissipate () |
void | setTorque (int16_t torque) |
void | setTargetPos (int32_t pos) |
int32_t | getTargetPos () |
void | setTargetVelocity (int32_t vel) |
int32_t | getTargetVelocity () |
int32_t | getVelocity () |
int16_t | getTorque () |
void | setFlux (int16_t flux) |
int16_t | getFlux () |
void | setFluxTorque (int16_t flux, int16_t torque) |
void | setFluxTorqueFF (int16_t flux, int16_t torque) |
std::pair< int32_t, int32_t > | getActualTorqueFlux () |
int32_t | getActualFlux () |
int32_t | getActualTorque () |
void | rampFlux (uint16_t target, uint16_t time_ms) |
bool | checkAdc () |
float | getTemp () |
TMC_ControlState | getState () |
void | setPhiEtype (PhiE type) |
PhiE | getPhiEtype () |
void | setPhiE_ext (int16_t phiE) |
int16_t | getPhiE () |
int16_t | getPhiEfromExternalEncoder () |
int16_t | getPhiE_Enc () |
void | setGpioMode (TMC_GpioMode mode) |
uint8_t | getGpioPins () |
void | setGpioPins (uint8_t pins) |
void | setPosSel (PosSelection psel) |
void | setVelSel (VelSelection vsel, uint8_t mode=0) |
void | setMotionMode (MotionMode mode, bool force=false) |
MotionMode | getMotionMode () |
void | setUdUq (int16_t ud, int16_t uq) |
void | setBrakeLimits (uint16_t low, uint16_t high) |
bool | reachedPosition (uint16_t tolerance) |
void | setPids (TMC4671PIDConf pids) |
TMC4671PIDConf | getPids () |
void | setLimits (TMC4671Limits limits) |
TMC4671Limits | getLimits () |
void | setUqUdLimit (uint16_t limit) |
void | setTorqueLimit (uint16_t limit) |
void | setPidPrecision (TMC4671PidPrecision setting) |
Encoder * | getEncoder () override |
void | setEncoder (std::shared_ptr< Encoder > &encoder) override |
bool | hasIntegratedEncoder () override |
bool | usingExternalEncoder () |
int32_t | getPos () override |
int32_t | getPosAbs () override |
void | setPos (int32_t pos) override |
void | setTmcPos (int32_t pos) |
uint32_t | getCpr () |
void | setCpr (uint32_t cpr) |
EncoderType | getEncoderType () override |
uint32_t | posToEnc (uint32_t pos) |
uint32_t | encToPos (uint32_t enc) |
void | exti (uint16_t GPIO_Pin) |
bool | findEncoderIndex (int32_t speed=10, uint16_t power=2500, bool offsetPhiM=false, bool zeroCount=false) |
bool | autohome () |
void | zeroAbnUsingPhiM (bool offsetPhiE=false) |
StatusFlags | readFlags (bool maskedOnly=true) |
void | setStatusMask (StatusFlags mask) |
void | setStatusMask (uint32_t mask) |
void | setStatusFlags (uint32_t flags) |
void | setStatusFlags (StatusFlags flags) |
void | setEncoderIndexFlagEnabled (bool enabled, bool zeroEncoder=false) |
void | statusCheck () |
void | saveFlash () override |
void | restoreFlash () override |
uint16_t | encodeEncHallMisc () |
void | restoreEncHallMisc (uint16_t val) |
void | beginSpiTransfer (SPIPort *port) |
void | endSpiTransfer (SPIPort *port) |
CommandStatus | command (const ParsedCommand &cmd, std::vector< CommandReply > &replies) |
void | registerCommands () |
virtual std::string | getHelpstring () |
![]() | |
MotorDriver () | |
virtual | ~MotorDriver () |
![]() | |
virtual | ~ChoosableClass () |
uint16_t | getSelectionID () |
![]() | |
PersistentStorage () | |
virtual | ~PersistentStorage () |
void | restoreFlashDelayed () |
![]() | |
Encoder () | |
virtual | ~Encoder () |
virtual float | getPos_f () |
virtual float | getPosAbs_f () |
![]() | |
CommandHandler (const char *clsname, uint16_t clsid, uint8_t instance=0) | |
virtual | ~CommandHandler () |
virtual bool | hasCommands () |
virtual void | setCommandsEnabled (bool enable) |
void | registerCommands () |
virtual CommandStatus | internalCommand (const ParsedCommand &cmd, std::vector< CommandReply > &replies) |
virtual std::string | getCommandsHelpstring () |
virtual std::string | getCsvHelpstring () |
virtual uint8_t | getCommandHandlerInstance () |
void | broadcastCommandReply (CommandReply reply, uint32_t cmdId, CMDtype type) |
void | sendCommandReplyAsync (CommandReply reply, uint32_t cmdId, CMDtype type, CommandInterface *interface=nullptr) |
virtual uint16_t | getCommandHandlerID () |
virtual CmdHandlerInfo * | getCommandHandlerInfo () |
virtual bool | isValidCommandId (uint32_t cmdid, uint32_t ignoredFlags=0, uint32_t requiredFlag=0) |
virtual CmdHandlerCommanddef * | getCommandFromName (const std::string &cmd, uint32_t ignoredFlags=0) |
virtual CmdHandlerCommanddef * | getCommandFromId (const uint32_t id, uint32_t ignoredFlags=0) |
template<typename ID > | |
void | registerCommand (const char *cmd, const ID cmdid, const char *help=nullptr, uint32_t flags=0) |
![]() | |
SPIDevice (SPIPort &port, OutputPin csPin) | |
SPIDevice (SPIPort &port, SPIConfig &spiConfig) | |
virtual | ~SPIDevice () |
void | assertChipSelect () |
void | clearChipSelect () |
virtual bool | updateCSPin (OutputPin &csPin) |
virtual void | spiTxCompleted (SPIPort *port) |
virtual void | spiRxCompleted (SPIPort *port) |
virtual void | spiTxRxCompleted (SPIPort *port) |
virtual void | spiRequestError (SPIPort *port) |
virtual SPIConfig * | getSpiConfig () |
![]() | |
ExtiHandler () | |
virtual | ~ExtiHandler () |
![]() | |
Thread (const std::string Name, uint16_t StackDepth, UBaseType_t Priority) | |
Thread (const char *Name, uint16_t StackDepth, UBaseType_t Priority) | |
Thread (uint16_t StackDepth, UBaseType_t Priority) | |
bool | Start () |
virtual | ~Thread () |
TaskHandle_t | GetHandle () |
void | Suspend () |
void | Resume () |
void | ResumeFromISR () |
void | Notify () |
void | NotifyFromISR () |
uint32_t | WaitForNotification (TickType_t Timeout=portMAX_DELAY) |
UBaseType_t | GetPriority () |
UBaseType_t | GetPriorityFromISR () |
void | SetPriority (UBaseType_t NewPriority) |
std::string | GetName () |
char * | GetName () |
Static Public Member Functions | |
static TMC4671MotConf | decodeMotFromInt (uint16_t val) |
static uint16_t | encodeMotToInt (TMC4671MotConf mconf) |
![]() | |
static bool | isCreatable () |
![]() | |
static std::vector< PersistentStorage * > & | getFlashHandlers () |
static void | restoreFlashStartupCb () |
![]() | |
static void | logSerial (std::string string) |
Send a log formatted sequence. More... | |
static void | logSerialDebug (std::string string) |
Send a log formatted sequence if debug is on. More... | |
static bool | logsEnabled () |
static void | setLogsEnabled (bool enabled) |
static uint32_t | getClassIdFromName (const char *name) |
static const char * | getClassNameFromId (const uint32_t id) |
static CommandHandler * | getHandlerFromHandlerId (const uint16_t cmdhandlerID) |
static CommandHandler * | getHandlerFromId (const uint16_t id, const uint8_t instance=0xFF) |
static CommandHandler * | getHandlerFromClassName (const char *name, const uint8_t instance=0xFF) |
static std::vector< CommandHandler * > | getHandlersFromClassName (const char *name) |
static std::vector< CommandHandler * > | getHandlersFromId (const uint16_t id) |
static bool | isInHandlerList (CommandHandler *handler) |
static std::string | getAllHelpstrings () |
static std::vector< CommandHandler * > & | getCommandHandlers () |
template<typename TVal > | |
static CommandStatus | handleGetSet (const ParsedCommand &cmd, std::vector< CommandReply > &replies, TVal &value) |
template<typename TVal , class cls , class cls1 > | |
static CommandStatus | handleGetSetFunc (const ParsedCommand &cmd, std::vector< CommandReply > &replies, TVal &value, void(cls1::*setfunc)(TVal), cls *obj) |
template<typename TVal , class cls , class cls1 , class cls2 > | |
static CommandStatus | handleGetFuncSetFunc (const ParsedCommand &cmd, std::vector< CommandReply > &replies, TVal(cls1::*getfunc)(), void(cls2::*setfunc)(TVal), cls *obj) |
template<typename TVal , class cls , class cls1 > | |
static CommandStatus | handleGetFuncSet (const ParsedCommand &cmd, std::vector< CommandReply > &replies, TVal &value, TVal(cls1::*getfunc)(), cls *obj) |
![]() | |
static void | Yield () |
static void | StartScheduler () |
static void | EndScheduler () |
Public Attributes | |
TMC4671MainConfig | conf |
bool | emergency = false |
bool | estopTriggered = false |
int16_t | nextFlux = 0 |
int16_t | idleFlux = 0 |
uint16_t | maxOffsetFlux = 0 |
int16_t | bangInitPower = 5000 |
const float | fluxDissipationLimit = 1000 |
TMC4671PIDConf | curPids |
TMC4671Limits | curLimits |
TMC4671PidPrecision | pidPrecision |
TMC4671ABNConf | abnconf |
TMC4671HALLConf | hallconf |
TMC4671AENCConf | aencconf |
bool | flagCheckInProgress = false |
StatusFlags | statusFlags = {0} |
StatusFlags | statusMask = {0} |
TMC4671FlashAddrs | flashAddrs |
bool | allowSlowSPI = true |
Static Public Attributes | |
static ClassIdentifier | info |
![]() | |
static ClassIdentifier | info ={.name = "None" , .id=CLSID_MOT_NONE, .visibility = ClassVisibility::visible} |
static const std::vector< class_entry< MotorDriver > > | all_drivers |
![]() | |
static ClassIdentifier | info |
![]() | |
static ClassIdentifier | info ={.name = "None" , .id=CLSID_ENCODER_NONE, .visibility = ClassVisibility::visible} |
static const std::vector< class_entry< Encoder > > | all_encoders |
![]() | |
static bool | logEnabled = true |
![]() | |
static std::vector< ExtiHandler * > | extiHandlers |
Private Types | |
enum class | TMC4671_commands : uint32_t { cpr , mtype , encsrc , tmcHwType , encalign , poles , acttrq , pwmlim , torqueP , torqueI , fluxP , fluxI , velocityP , velocityI , posP , posI , tmctype , pidPrec , phiesrc , fluxoffset , seqpi , tmcIscale , encdir , temp , reg , svpwm , fullCalibration , calibrated , abnindexenabled , findIndex , getState , encpol , combineEncoder , invertForce , vmTmc , extphie , torqueFilter_mode , torqueFilter_f , torqueFilter_q , pidautotune , fluxbrake , pwmfreq } |
Private Member Functions | |
void | initAdc (uint16_t mdecA, uint16_t mdecB, uint32_t mclkA, uint32_t mclkB) |
void | setPwm (uint8_t val, uint16_t maxcnt, uint8_t bbmL, uint8_t bbmH) |
void | setPwm (TMC_PwmMode val) |
void | setPwmMaxCnt (uint16_t maxcnt) |
void | setSvPwm (bool enable) |
void | encInit () |
void | encoderIndexHit () |
void | saveAdcParams () |
void | calibFailCb () |
void | encoderInit () |
void | errorCallback (const Error &error, bool cleared) |
bool | pidAutoTune () |
void | setUpExtEncTimer () |
Friends | |
class | TMCDebugBridge |
Additional Inherited Members | |
![]() | |
void | setInstance (uint8_t instance) |
virtual void | addCommandHandler () |
virtual void | removeCommandHandler () |
![]() | |
virtual void | setSpiConfig (SPIConfig config) |
![]() | |
virtual void | Cleanup () |
void | Delay (const TickType_t Delay) |
void | DelayUntil (const TickType_t Period) |
void | ResetDelayUntil () |
bool | Wait (ConditionVariable &Cv, Mutex &CvLock, TickType_t Timeout=portMAX_DELAY) |
![]() | |
static std::vector< uint16_t > & | getCommandHandlerIds () |
![]() | |
std::shared_ptr< Encoder > | drvEncoder = std::make_shared<Encoder>() |
![]() | |
uint16_t | selectionId |
Should only be written by ClassChooser during creation. More... | |
![]() | |
bool | restoreDelayedFlag = false |
![]() | |
uint32_t | cpr = 0 |
![]() | |
bool | commandsEnabled = true |
std::vector< CmdHandlerCommanddef > | registeredCommands |
CmdHandlerInfo | cmdHandlerInfo |
![]() | |
SPIPort & | spiPort |
SPIConfig | spiConfig |
![]() | |
static bool | startupComplete = false |
|
strongprivate |
Definition at line 49 of file TMC4671.cpp.
|
virtual |
Definition at line 81 of file TMC4671.cpp.
bool TMC4671::autohome | ( | ) |
Definition at line 762 of file TMC4671.cpp.
void TMC4671::bangInitEnc | ( | int16_t | power | ) |
Aligns ABN encoders by forcing an angle with high current and calculating the offset Will start at the current phiE to minimize any extra movements (useful if motor was turned in openloop mode before already)
power | Maximum current reached during flux ramp |
Definition at line 957 of file TMC4671.cpp.
|
virtual |
Reimplemented from SPIDevice.
Definition at line 2537 of file TMC4671.cpp.
|
private |
Definition at line 1420 of file TMC4671.cpp.
bool TMC4671::calibrateAdcOffset | ( | uint16_t | time = 500 | ) |
Calibrates the ADC by disabling the power stage and sampling a mean value. Takes time!
Definition at line 1359 of file TMC4671.cpp.
void TMC4671::calibrateAenc | ( | ) |
Moves the motor to find out analog encoder scalings and offsets
Definition at line 1046 of file TMC4671.cpp.
void TMC4671::calibrateEncoder | ( | ) |
Definition at line 648 of file TMC4671.cpp.
|
inline |
Definition at line 780 of file TMC4671.cpp.
bool TMC4671::checkAdc | ( | ) |
Samples the adc and checks if it returns a neutral value
Definition at line 374 of file TMC4671.cpp.
bool TMC4671::checkEncoder | ( | ) |
Steps the motor a few times to check if the encoder follows correctly
Definition at line 1167 of file TMC4671.cpp.
|
virtual |
[in] | cmd | The parsed command to be executed. |
[out] | replies | A vector to return one or multiple reply objects into. Replies to the interface will be generated based on the reply objects. A string reply may not contain start, end and separation markers: [,],| Other characters are allowed. |
Reimplemented from CommandHandler.
Definition at line 2890 of file TMC4671.cpp.
int16_t TMC4671::controlFluxDissipate | ( | ) |
Calculates a flux value based on the internal and external voltage difference to dissipate energy without a brake resistor
Definition at line 1710 of file TMC4671.cpp.
|
static |
Definition at line 2614 of file TMC4671.cpp.
|
virtual |
Request an emergency stop if something critical happened or the emergency button is triggered Should stop the motor immediately in a safe way.
Reimplemented from MotorDriver.
Definition at line 1680 of file TMC4671.cpp.
|
private |
uint16_t TMC4671::encodeEncHallMisc | ( | ) |
Definition at line 2631 of file TMC4671.cpp.
|
static |
Definition at line 2623 of file TMC4671.cpp.
|
private |
Definition at line 2605 of file TMC4671.cpp.
|
private |
Definition at line 1429 of file TMC4671.cpp.
uint32_t TMC4671::encToPos | ( | uint32_t | enc | ) |
Converts encoder counts to phiM
Definition at line 1935 of file TMC4671.cpp.
|
virtual |
Reimplemented from SPIDevice.
Definition at line 2540 of file TMC4671.cpp.
|
private |
Definition at line 3252 of file TMC4671.cpp.
void TMC4671::estimateABNparams | ( | ) |
Moves the rotor and estimates polarity and direction of the encoder Polarity is found by measuring the n pulse. If polarity was found to be reversed during the test direction will be reversed again to account for that
Definition at line 2277 of file TMC4671.cpp.
void TMC4671::estimateExtEnc | ( | ) |
Similar to the ABN calibration this moved the motor and measures the encoder direction
Definition at line 2330 of file TMC4671.cpp.
bool TMC4671::externalEncoderAllowed | ( | ) |
Definition at line 2006 of file TMC4671.cpp.
|
virtual |
Reimplemented from ExtiHandler.
Definition at line 2599 of file TMC4671.cpp.
bool TMC4671::findEncoderIndex | ( | int32_t | speed = 10 , |
uint16_t | power = 2500 , |
||
bool | offsetPhiM = false , |
||
bool | zeroCount = false |
||
) |
Rotates motor until the ABN index is found
Definition at line 823 of file TMC4671.cpp.
int32_t TMC4671::getActualFlux | ( | ) |
Returns measured flux
Definition at line 2471 of file TMC4671.cpp.
int32_t TMC4671::getActualTorque | ( | ) |
Returns measured torque
Definition at line 2480 of file TMC4671.cpp.
std::pair< int32_t, int32_t > TMC4671::getActualTorqueFlux | ( | ) |
Returns measured flux and torque as a pair Flux is first, torque second item
Definition at line 2461 of file TMC4671.cpp.
|
inlineoverridevirtual |
Type of this class. Mainclass, motordriver... Should be implemented by the parent class so it is not in the info struct
Reimplemented from CommandHandler.
|
virtual |
Gets the amount of counts per full rotation of the encoder
Reimplemented from Encoder.
Definition at line 1909 of file TMC4671.cpp.
uint32_t TMC4671::getEncCpr | ( | ) |
Definition at line 1573 of file TMC4671.cpp.
|
overridevirtual |
Returns the encoder of this motor driver. Either the integrated encoder or an external encoder assigned to this motor driver passed externally
Reimplemented from MotorDriver.
Definition at line 1840 of file TMC4671.cpp.
|
overridevirtual |
Returns the type of the encoder. Must override this and NOT return NONE in other classes
Reimplemented from Encoder.
Definition at line 1942 of file TMC4671.cpp.
int16_t TMC4671::getFlux | ( | ) |
Definition at line 2046 of file TMC4671.cpp.
uint8_t TMC4671::getGpioPins | ( | ) |
Reads the state of the 8 gpio pins
Definition at line 1800 of file TMC4671.cpp.
|
inlinevirtual |
Returns a description of this class
Reimplemented from CommandHandler.
|
virtual |
Command handlers always have class infos. Works well with ChoosableClass.
Implements CommandHandler.
Definition at line 87 of file TMC4671.cpp.
TMC4671Limits TMC4671::getLimits | ( | ) |
Definition at line 2137 of file TMC4671.cpp.
MotionMode TMC4671::getMotionMode | ( | ) |
Definition at line 1614 of file TMC4671.cpp.
int16_t TMC4671::getPhiE | ( | ) |
Definition at line 946 of file TMC4671.cpp.
int16_t TMC4671::getPhiE_Enc | ( | ) |
Reads phiE directly from the encoder selection instead of the current phiE selection
Definition at line 1150 of file TMC4671.cpp.
int16_t TMC4671::getPhiEfromExternalEncoder | ( | ) |
Definition at line 934 of file TMC4671.cpp.
PhiE TMC4671::getPhiEtype | ( | ) |
Definition at line 1595 of file TMC4671.cpp.
TMC4671PIDConf TMC4671::getPids | ( | ) |
Definition at line 2089 of file TMC4671.cpp.
|
overridevirtual |
Returns the encoder position as raw counts
Reimplemented from Encoder.
Definition at line 1887 of file TMC4671.cpp.
|
overridevirtual |
Returns absolute positions without offsets for absolute encoders. Otherwise it returns getPos
Reimplemented from Encoder.
Definition at line 1893 of file TMC4671.cpp.
float TMC4671::getPwmFreq | ( | ) |
Returns the PWM loop frequency in Hz Depends on hardware clock and pwm counter setting. Default 25kHz
Definition at line 2416 of file TMC4671.cpp.
uint8_t TMC4671::getSpiAddr | ( | ) |
TMC_ControlState TMC4671::getState | ( | ) |
Definition at line 776 of file TMC4671.cpp.
int32_t TMC4671::getTargetPos | ( | ) |
Definition at line 903 of file TMC4671.cpp.
int32_t TMC4671::getTargetVelocity | ( | ) |
Definition at line 919 of file TMC4671.cpp.
float TMC4671::getTemp | ( | ) |
Reads a temperature from a thermistor connected to AGPI_B Not calibrated perfectly!
Definition at line 349 of file TMC4671.cpp.
std::pair< uint32_t, std::string > TMC4671::getTmcType | ( | ) |
Returns a string with the name and version of the chip
Definition at line 1817 of file TMC4671.cpp.
int32_t TMC4671::getTmcVM | ( | ) |
Returns estimated VM in mV measured by TMC
Definition at line 231 of file TMC4671.cpp.
int16_t TMC4671::getTorque | ( | ) |
Definition at line 2036 of file TMC4671.cpp.
int32_t TMC4671::getVelocity | ( | ) |
Definition at line 922 of file TMC4671.cpp.
|
overridevirtual |
If returned true it signals that this motor driver contains its own encoder and does not require an external encoder
Reimplemented from MotorDriver.
Definition at line 1856 of file TMC4671.cpp.
bool TMC4671::hasPower | ( | ) |
Definition at line 192 of file TMC4671.cpp.
|
private |
Definition at line 2444 of file TMC4671.cpp.
bool TMC4671::initialize | ( | ) |
Sets all parameters of the driver at startup. Only has to be called once when the driver is detected restoreFlash() should be called before this to restore settings!
Definition at line 244 of file TMC4671.cpp.
void TMC4671::initializeWithPower | ( | ) |
Definition at line 385 of file TMC4671.cpp.
bool TMC4671::isSetUp | ( | ) |
Definition at line 198 of file TMC4671.cpp.
|
virtual |
Reimplemented from MotorDriver.
Definition at line 423 of file TMC4671.cpp.
|
private |
Iterative tuning function for tuning the torque mode PI values
Enter phieExt & torque mode Zero I, default P Ramp up flux P until 50% of target, then lower increments until targetflux_p is reached Increase I until oscillation is found. Back off a bit
Definition at line 666 of file TMC4671.cpp.
bool TMC4671::pingDriver | ( | ) |
Check if driver is responding
Definition at line 223 of file TMC4671.cpp.
uint32_t TMC4671::posToEnc | ( | uint32_t | pos | ) |
Definition at line 1938 of file TMC4671.cpp.
void TMC4671::rampFlux | ( | uint16_t | target, |
uint16_t | time_ms | ||
) |
Ramps flux from current value to a target value over a specified duration
Definition at line 2066 of file TMC4671.cpp.
bool TMC4671::reachedPosition | ( | uint16_t | tolerance | ) |
Definition at line 792 of file TMC4671.cpp.
StatusFlags TMC4671::readFlags | ( | bool | maskedOnly = true | ) |
Reads status flags
maskedOnly | Masks flags by previously set flag mask that would trigger an interrupt. False to read all flags |
Definition at line 2549 of file TMC4671.cpp.
uint32_t TMC4671::readReg | ( | uint8_t | reg | ) |
Definition at line 2488 of file TMC4671.cpp.
void TMC4671::registerCommands | ( | ) |
Definition at line 2842 of file TMC4671.cpp.
void TMC4671::restoreEncHallMisc | ( | uint16_t | val | ) |
Definition at line 2657 of file TMC4671.cpp.
|
overridevirtual |
Restores saved parameters Call initialize() to apply some of the settings
Reimplemented from PersistentStorage.
Definition at line 145 of file TMC4671.cpp.
|
virtual |
Implementation of your actual thread code. You must override this function.
Implements cpp_freertos::Thread.
Definition at line 427 of file TMC4671.cpp.
void TMC4671::runOpenLoop | ( | uint16_t | ud, |
uint16_t | uq, | ||
int32_t | speed, | ||
int32_t | accel, | ||
bool | torqueMode = false |
||
) |
Definition at line 1625 of file TMC4671.cpp.
|
private |
Writes ADC offsets into flash
Definition at line 135 of file TMC4671.cpp.
|
overridevirtual |
Called when the user uses the "save" command and presses the save button in the configurator Automatically called by the command parser thread for every class that inherits from PersistentStorage
Reimplemented from PersistentStorage.
Definition at line 107 of file TMC4671.cpp.
void TMC4671::setAdcOffset | ( | uint32_t | adc_I0_offset, |
uint32_t | adc_I1_offset | ||
) |
Definition at line 1951 of file TMC4671.cpp.
void TMC4671::setAdcScale | ( | uint32_t | adc_I0_scale, |
uint32_t | adc_I1_scale | ||
) |
Definition at line 1959 of file TMC4671.cpp.
void TMC4671::setAddress | ( | uint8_t | address | ) |
Definition at line 93 of file TMC4671.cpp.
void TMC4671::setBBM | ( | uint8_t | bbml, |
uint8_t | bbmh | ||
) |
Definition at line 2385 of file TMC4671.cpp.
void TMC4671::setBiquadFlux | ( | const TMC4671Biquad & | filter | ) |
Applies a biquad filter to the flux target Set nullptr to disable
Definition at line 2152 of file TMC4671.cpp.
void TMC4671::setBiquadPos | ( | const TMC4671Biquad & | filter | ) |
Applies a biquad filter to the pos target Set nullptr to disable
Definition at line 2173 of file TMC4671.cpp.
void TMC4671::setBiquadTorque | ( | const TMC4671Biquad & | filter | ) |
Applies a biquad filter to the torque target Set nullptr to disable
Definition at line 2215 of file TMC4671.cpp.
void TMC4671::setBiquadVel | ( | const TMC4671Biquad & | filter | ) |
Applies a biquad filter to the actual measured velocity Set nullptr to disable
Definition at line 2194 of file TMC4671.cpp.
void TMC4671::setBrakeLimits | ( | uint16_t | low, |
uint16_t | high | ||
) |
Sets the raw brake resistor limits. Centered at 0x7fff Set both 0 to deactivate
Definition at line 2267 of file TMC4671.cpp.
void TMC4671::setCpr | ( | uint32_t | cpr | ) |
Definition at line 1920 of file TMC4671.cpp.
|
overridevirtual |
Can pass an external encoder if driver has no integrated encoder This allows a driver to get an external encoder assigned if it requires one and has the capability of using external encoders
Reimplemented from MotorDriver.
Definition at line 1848 of file TMC4671.cpp.
void TMC4671::setEncoderIndexFlagEnabled | ( | bool | enabled, |
bool | zeroEncoder = false |
||
) |
Enables or disables the encoder index interruption on the flag pin depending on the selected encoder
Definition at line 877 of file TMC4671.cpp.
void TMC4671::setEncoderType | ( | EncoderType_TMC | type | ) |
Changes the encoder type and calls init methods for the encoder types. Setup the specific parameters (abnconf, aencconf...) first.
Definition at line 1513 of file TMC4671.cpp.
void TMC4671::setExternalEncoderAllowed | ( | bool | allow | ) |
Definition at line 1993 of file TMC4671.cpp.
void TMC4671::setFFMode | ( | FFMode | mode | ) |
Definition at line 1980 of file TMC4671.cpp.
void TMC4671::setFlux | ( | int16_t | flux | ) |
Definition at line 2040 of file TMC4671.cpp.
void TMC4671::setFluxTorque | ( | int16_t | flux, |
int16_t | torque | ||
) |
Definition at line 2049 of file TMC4671.cpp.
void TMC4671::setFluxTorqueFF | ( | int16_t | flux, |
int16_t | torque | ||
) |
Definition at line 2056 of file TMC4671.cpp.
void TMC4671::setGpioMode | ( | TMC_GpioMode | mode | ) |
Changes the mode of the 8 GPIO pins Banks A and B can be mapped independently to inputs or outputs, as a DS adc interface or by default as a secondary debug SPI port
Definition at line 1774 of file TMC4671.cpp.
void TMC4671::setGpioPins | ( | uint8_t | pins | ) |
Changes the state of gpio pins that are mapped as output lower 4 bits bank A, upper 4 bits bank B
Definition at line 1808 of file TMC4671.cpp.
void TMC4671::setHwType | ( | TMC_HW_Ver | type | ) |
Sets some constants and features depending on the hardware version of the driver
Definition at line 2692 of file TMC4671.cpp.
void TMC4671::setLimits | ( | TMC4671Limits | limits | ) |
Definition at line 2126 of file TMC4671.cpp.
void TMC4671::setMotionMode | ( | MotionMode | mode, |
bool | force = false |
||
) |
Definition at line 1603 of file TMC4671.cpp.
void TMC4671::setMotorType | ( | MotorType | motor, |
uint16_t | poles | ||
) |
Definition at line 2014 of file TMC4671.cpp.
void TMC4671::setOpenLoopSpeedAccel | ( | int32_t | speed, |
uint32_t | accel | ||
) |
Definition at line 1619 of file TMC4671.cpp.
void TMC4671::setPhiE_ext | ( | int16_t | phiE | ) |
Definition at line 930 of file TMC4671.cpp.
void TMC4671::setPhiEtype | ( | PhiE | type | ) |
Definition at line 1585 of file TMC4671.cpp.
void TMC4671::setPidPrecision | ( | TMC4671PidPrecision | setting | ) |
Definition at line 2113 of file TMC4671.cpp.
void TMC4671::setPids | ( | TMC4671PIDConf | pids | ) |
Definition at line 2080 of file TMC4671.cpp.
|
overridevirtual |
Changes position using offset from index
Reimplemented from Encoder.
Definition at line 1864 of file TMC4671.cpp.
void TMC4671::setPositionExt | ( | int32_t | pos | ) |
Definition at line 926 of file TMC4671.cpp.
void TMC4671::setPosSel | ( | PosSelection | psel | ) |
Changes the position sensor source
Definition at line 1755 of file TMC4671.cpp.
|
private |
Sets pwm mode:
0 = pwm off
1 = pwm off, HS low, LS high
2 = pwm off, HS high, LS low
3 = pwm off
4 = pwm off
5 = pwm LS only
6 = pwm HS only
7 = pwm on centered, FOC mode
Definition at line 2381 of file TMC4671.cpp.
|
private |
Definition at line 2392 of file TMC4671.cpp.
void TMC4671::setPwmFreq | ( | float | freq | ) |
Changes the PWM frequency to a desired frequency Possible values depend on the hwclock. At 25MHz the lowest possible frequency is 24.1kHz
Definition at line 2436 of file TMC4671.cpp.
|
private |
Changes PWM frequency Max value 4095, minimum 255
Definition at line 2425 of file TMC4671.cpp.
void TMC4671::setSequentialPI | ( | bool | sequential | ) |
Definition at line 1988 of file TMC4671.cpp.
bool TMC4671::setSpiAddr | ( | uint8_t | chan | ) |
void TMC4671::setStatusFlags | ( | StatusFlags | flags | ) |
Definition at line 2570 of file TMC4671.cpp.
void TMC4671::setStatusFlags | ( | uint32_t | flags | ) |
Definition at line 2566 of file TMC4671.cpp.
void TMC4671::setStatusMask | ( | StatusFlags | mask | ) |
Definition at line 2558 of file TMC4671.cpp.
void TMC4671::setStatusMask | ( | uint32_t | mask | ) |
Definition at line 2562 of file TMC4671.cpp.
|
private |
Enable or disable space vector pwm for 3 phase motors Normally active but should be disabled if the motor has no isolated star point
Definition at line 2403 of file TMC4671.cpp.
void TMC4671::setTargetPos | ( | int32_t | pos | ) |
Enables position mode and sets a target position
Definition at line 896 of file TMC4671.cpp.
void TMC4671::setTargetVelocity | ( | int32_t | vel | ) |
Enables velocity mode and sets a velocity target
Definition at line 912 of file TMC4671.cpp.
void TMC4671::setTmcPos | ( | int32_t | pos | ) |
Changes position in tmc register
Definition at line 1882 of file TMC4671.cpp.
void TMC4671::setTorque | ( | int16_t | torque | ) |
Definition at line 2030 of file TMC4671.cpp.
void TMC4671::setTorqueFilter | ( | TMC4671Biquad_conf & | conf | ) |
Changes the torque biquad filter
Definition at line 2236 of file TMC4671.cpp.
void TMC4671::setTorqueLimit | ( | uint16_t | limit | ) |
Definition at line 2107 of file TMC4671.cpp.
void TMC4671::setUdUq | ( | int16_t | ud, |
int16_t | uq | ||
) |
Definition at line 1646 of file TMC4671.cpp.
void TMC4671::setup_ABN_Enc | ( | TMC4671ABNConf | encconf | ) |
Definition at line 1289 of file TMC4671.cpp.
void TMC4671::setup_AENC | ( | TMC4671AENCConf | encconf | ) |
Definition at line 1313 of file TMC4671.cpp.
void TMC4671::setup_HALL | ( | TMC4671HALLConf | hallconf | ) |
Definition at line 1330 of file TMC4671.cpp.
|
private |
Definition at line 3218 of file TMC4671.cpp.
void TMC4671::setupFeedForwardTorque | ( | int32_t | gain, |
int32_t | constant | ||
) |
Definition at line 1967 of file TMC4671.cpp.
void TMC4671::setupFeedForwardVelocity | ( | int32_t | gain, |
int32_t | constant | ||
) |
Definition at line 1973 of file TMC4671.cpp.
void TMC4671::setUqUdLimit | ( | uint16_t | limit | ) |
Limits the PWM value
Definition at line 2102 of file TMC4671.cpp.
void TMC4671::setVelSel | ( | VelSelection | vsel, |
uint8_t | mode = 0 |
||
) |
Changes the velocity sensor source (RPM if PhiM source used)
mode | 0 = fixed frequency sampling (~4369.067Hz), 1 = PWM sync time difference measurement |
Definition at line 1764 of file TMC4671.cpp.
|
virtual |
Enable the motor driver
Reimplemented from MotorDriver.
Definition at line 1661 of file TMC4671.cpp.
void TMC4671::statusCheck | ( | ) |
Reads and resets all status flags and executes depending on status flags
Definition at line 2577 of file TMC4671.cpp.
|
virtual |
Disable the motor driver
Reimplemented from MotorDriver.
Definition at line 1650 of file TMC4671.cpp.
void TMC4671::timerElapsed | ( | TIM_HandleTypeDef * | htim | ) |
Definition at line 3205 of file TMC4671.cpp.
|
virtual |
Sets a torque in positive or negative direction For ADC linearity reasons under 25000 is recommended
Reimplemented from MotorDriver.
Definition at line 1724 of file TMC4671.cpp.
void TMC4671::updateReg | ( | uint8_t | reg, |
uint32_t | dat, | ||
uint32_t | mask, | ||
uint8_t | shift | ||
) |
Definition at line 2530 of file TMC4671.cpp.
void TMC4671::writeReg | ( | uint8_t | reg, |
uint32_t | dat | ||
) |
Definition at line 2502 of file TMC4671.cpp.
void TMC4671::writeRegAsync | ( | uint8_t | reg, |
uint32_t | dat | ||
) |
Definition at line 2514 of file TMC4671.cpp.
void TMC4671::zeroAbnUsingPhiM | ( | bool | offsetPhiE = false | ) |
Definition at line 802 of file TMC4671.cpp.
|
friend |
TMC4671ABNConf TMC4671::abnconf |
TMC4671AENCConf TMC4671::aencconf |
|
private |
TMC4671MainConfig TMC4671::conf |
|
private |
TMC4671Limits TMC4671::curLimits |
|
private |
TMC4671PIDConf TMC4671::curPids |
|
private |
|
private |
|
private |
|
private |
TMC4671FlashAddrs TMC4671::flashAddrs |
TMC4671HALLConf TMC4671::hallconf |
|
private |
|
static |
|
private |
|
private |
|
private |
|
private |
TMC4671PidPrecision TMC4671::pidPrecision |
|
private |
|
private |
|
private |
StatusFlags TMC4671::statusFlags = {0} |
StatusFlags TMC4671::statusMask = {0} |
|
private |