try to fix submodule
This commit is contained in:
147
firmware/lib/Arduino-FOC-drivers/src/drivers/tmc6200/README.md
Normal file
147
firmware/lib/Arduino-FOC-drivers/src/drivers/tmc6200/README.md
Normal file
@@ -0,0 +1,147 @@
|
||||
|
||||
# TMC6200 SimpleFOC Driver
|
||||
|
||||
by [@YaseenTwati](https://github.com/YaseenTwati)
|
||||
|
||||
The TMC6200 is a Universal high voltage BLDC/PMSM/Servo MOSFET 3-halfbridge gate-driver with in line motor current
|
||||
sensing. External MOSFETs for up to 100A motor current.
|
||||
|
||||
See https://www.trinamic.com/fileadmin/assets/Products/ICs_Documents/TMC6200_datasheet_Rev1.05.pdf for more information.
|
||||
|
||||
## Hardware setup
|
||||
|
||||
To use the TMC6200 you will have to connect the following:
|
||||
|
||||
- GND
|
||||
- VCC_IO - 3.3V or 5V
|
||||
- SPE - pull up to VCC to enable SPI mode
|
||||
- SPI MOSI ( SDI )
|
||||
- SPI MISO ( SDO )
|
||||
- SPI CLK
|
||||
- SPI nCS
|
||||
- DRV_EN - connect to digital out (or pull up to VCC)
|
||||
- UH - connect to motor PWM pin
|
||||
- VH - connect to motor PWM pin
|
||||
- WH - connect to motor PWM pin
|
||||
- UL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation
|
||||
- VL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation
|
||||
- WL - connect to motor PWM pin for 6-PWM, or to digital out (or pull up to VCC) for 3-PWM operation
|
||||
|
||||
Optionally, but useful:
|
||||
|
||||
- FAULT - digital in, active high
|
||||
|
||||
For current sensing:
|
||||
|
||||
- CURU - connect to analog in
|
||||
- CURV - connect to analog in
|
||||
- CURW - connect to analog in
|
||||
|
||||
## Usage
|
||||
|
||||
Usage is quite easy, especially if you already know SimpleFOC. See also the [examples](https://github.com/simplefoc/Arduino-FOC-drivers/examples/drivers/drv8316/)
|
||||
|
||||
```c++
|
||||
#include "Arduino.h"
|
||||
#include <SimpleFOC.h>
|
||||
#include "SimpleFOCDrivers.h"
|
||||
#include "drivers/tmc6200/TMC6200.hpp"
|
||||
|
||||
BLDCMotor motor = BLDCMotor(15);
|
||||
TMC6200Driver6PWM driver = DRV8316Driver6PWM(UH, UL, VH, VL, WH, WL, nCS, DRV_EN);
|
||||
|
||||
//... normal simpleFOC init code...
|
||||
```
|
||||
|
||||
Or, for 3-PWM:
|
||||
|
||||
```c++
|
||||
#include "Arduino.h"
|
||||
#include <SimpleFOC.h>
|
||||
#include "SimpleFOCDrivers.h"
|
||||
#include "drivers/tmc6200/TMC6200.hpp"
|
||||
|
||||
BLDCMotor motor = BLDCMotor(15);
|
||||
TMC6200Driver3PWM driver = TMC6200Driver3PWM(UH, MOTOR_VH, MOTOR_WH, nCS, DRV_EN);
|
||||
|
||||
void setup() {
|
||||
|
||||
pinMode(UL, OUTPUT);
|
||||
pinMode(VL, OUTPUT);
|
||||
pinMode(WL, OUTPUT);
|
||||
|
||||
digitalWrite(WL, HIGH);
|
||||
digitalWrite(UL, HIGH);
|
||||
digitalWrite(VL, HIGH);
|
||||
|
||||
//... normal simpleFOC init code...
|
||||
}
|
||||
|
||||
```
|
||||
|
||||
### Validating the SPI Connection
|
||||
You can validate the SPI connection by checking the value of VERSION field in IOIN register. The value should be 0x10.
|
||||
|
||||
```c++
|
||||
if(driver.getInputs().VERSION != TMC6200_VERSION){
|
||||
// something is wrong with the spi connection
|
||||
}
|
||||
```
|
||||
|
||||
### Current Sensing
|
||||
The gain of the internal current amplifiers can be set to 5, 10 or 20 through `setCurrentSenseGain()`
|
||||
|
||||
```c++
|
||||
driver.setCurrentSenseGain(TMC6200_AmplificationGain::_5);
|
||||
//driver.setCurrentSenseGain(TMC6200_AmplificationGain::_10);
|
||||
//driver.setCurrentSenseGain(TMC6200_AmplificationGain::_20);
|
||||
```
|
||||
The sense amplifiers can also be turned off ( they are on by default ), through `setCurrentSenseAmplifierState()`
|
||||
|
||||
```c++
|
||||
driver.setCurrentSenseAmplifierState(false);
|
||||
```
|
||||
### Driver Strength
|
||||
The strength of the mosfet drivers can be controlled through `setDriverStrength()`
|
||||
|
||||
```c++
|
||||
driver.setDriverStrength(TMC6200_DRVStrength::Weak);
|
||||
//driver.setDriverStrength(TMC6200_DRVStrength::WeakTC); // (medium above OTPW level)
|
||||
//driver.setDriverStrength(TMC6200_DRVStrength::Medium);
|
||||
//driver.setDriverStrength(TMC6200_DRVStrength::Strong);
|
||||
```
|
||||
|
||||
### Handling Faults
|
||||
The fault line will go high if a fault occurs such as a short, an interrupt can be used to handle it.
|
||||
Note that some faults will disable the driver and will require the DRV_EN to be cycled to clear the fault.
|
||||
|
||||
```c++
|
||||
// somewhere in setup
|
||||
attachInterrupt(digitalPinToInterrupt(FAULT), handleFault, RISING);
|
||||
```
|
||||
|
||||
```c++
|
||||
void handleFault()
|
||||
{
|
||||
// you can read the status register to see what happened
|
||||
TMC6200GStatus status = driver.getStatus();
|
||||
Serial.print("hasUShorted: "); Serial.println(status.hasUShorted());
|
||||
Serial.print("hasVShorted: "); Serial.println(status.hasVShorted());
|
||||
Serial.print("hasWShorted: "); Serial.println(status.hasWShorted());
|
||||
Serial.print("isUShortedToGround: "); Serial.println(status.isUShortedToGround());
|
||||
Serial.print("isUShortedToSupply: "); Serial.println(status.isUShortedToSupply());
|
||||
Serial.print("isVShortedToGround: "); Serial.println(status.isVShortedToGround());
|
||||
Serial.print("isVShortedToSupply: "); Serial.println(status.isVShortedToSupply());
|
||||
Serial.print("isWShortedToGround: "); Serial.println(status.isWShortedToGround());
|
||||
Serial.print("isWShortedToSupply: "); Serial.println(status.isWShortedToSupply());
|
||||
Serial.print("isOverTemperaturePreWarning: "); Serial.println(status.isOverTemperaturePreWarning());
|
||||
Serial.print("isChargePumpUnderVoltage: "); Serial.println(status.isChargePumpUnderVoltage());
|
||||
|
||||
// the driver must be cycled to clear the fault
|
||||
digitalWrite(DRV_EN, LOW);
|
||||
delayMicrosockets(someSmallDelay);
|
||||
digitalWrite(DRV_EN, HIGH);
|
||||
}
|
||||
```
|
||||
|
||||
The driver provides other features such as controlling the tolerences of short detection and the BBM cycle time and so on, setting the options can be conveniently done via the provided setter methods. All documented registers and options are available via the driver, and the option values can be accessed via enums.
|
||||
221
firmware/lib/Arduino-FOC-drivers/src/drivers/tmc6200/TMC6200.cpp
Normal file
221
firmware/lib/Arduino-FOC-drivers/src/drivers/tmc6200/TMC6200.cpp
Normal file
@@ -0,0 +1,221 @@
|
||||
#include "TMC6200.hpp"
|
||||
|
||||
void TMC6200Driver::init(SPIClass *_spi) {
|
||||
spi = _spi;
|
||||
settings = SPISettings(500000, MSBFIRST, SPI_MODE3);
|
||||
|
||||
pinMode(csPin, OUTPUT);
|
||||
digitalWrite(csPin, HIGH);
|
||||
}
|
||||
|
||||
// TMC6200_GCONF ------
|
||||
|
||||
void TMC6200Driver::setDriverState(bool state) {
|
||||
TMC6200_GCONF gConf;
|
||||
gConf.reg = readRegister(TMC6200_GCONF_REG);
|
||||
|
||||
gConf.DISABLE = state ? 0 : 1;
|
||||
writeRegister(TMC6200_GCONF_REG, gConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setPWMMode(TMC6200_PWMMode pwmMode) {
|
||||
TMC6200_GCONF gConf;
|
||||
gConf.reg = readRegister(TMC6200_GCONF_REG);
|
||||
|
||||
gConf.SINGLE_LINE = pwmMode;
|
||||
writeRegister(TMC6200_GCONF_REG, gConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setFaultDirect(TMC6200_FaultDirect faultDirect) {
|
||||
TMC6200_GCONF gConf;
|
||||
gConf.reg = readRegister(TMC6200_GCONF_REG);
|
||||
|
||||
gConf.FAULT_DIRECT = faultDirect;
|
||||
writeRegister(TMC6200_GCONF_REG, gConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setCurrentSenseAmplifierState(bool state) {
|
||||
TMC6200_GCONF gConf;
|
||||
gConf.reg = readRegister(TMC6200_GCONF_REG);
|
||||
|
||||
gConf.AMPLIFIER_OFF = state ? 0 : 1;
|
||||
writeRegister(TMC6200_GCONF_REG, gConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain) {
|
||||
TMC6200_GCONF gConf;
|
||||
gConf.reg = readRegister(TMC6200_GCONF_REG);
|
||||
|
||||
gConf.AMPLIFICATION = amplificationGain;
|
||||
writeRegister(TMC6200_GCONF_REG, gConf.reg);
|
||||
|
||||
gConf.reg = readRegister(TMC6200_GCONF_REG);
|
||||
}
|
||||
|
||||
// TMC6200_DRV_CONF ------
|
||||
|
||||
void TMC6200Driver::setOverTemperatureThreshold(TMC6200_OTSelect otLevel) {
|
||||
TMC6200_DRV_CONF drvConf;
|
||||
drvConf.reg = readRegister(TMC6200_DRV_CONF_REG);
|
||||
|
||||
drvConf.OT_SELECT = otLevel;
|
||||
writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setDriverStrength(TMC6200_DRVStrength strength) {
|
||||
TMC6200_DRV_CONF drvConf;
|
||||
drvConf.reg = readRegister(TMC6200_DRV_CONF_REG);
|
||||
|
||||
drvConf.DRV_STRENGTH = strength;
|
||||
writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setBBMCycles(uint8_t clockCycles) {
|
||||
TMC6200_DRV_CONF drvConf;
|
||||
drvConf.reg = readRegister(TMC6200_DRV_CONF_REG);
|
||||
|
||||
drvConf.BBMCLKS = clockCycles;
|
||||
writeRegister(TMC6200_DRV_CONF_REG, drvConf.reg);
|
||||
}
|
||||
|
||||
// TMC6200_SHORT_CONF ------
|
||||
|
||||
void TMC6200Driver::setShortDelay(TMC6200_ShortDelay shortDelay) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.SHORT_DELAY = shortDelay;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::shortFilter(TMC6200_ShortFilter shortFilter) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.SHORT_FILTER = shortFilter;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setShortToSupplySensitivityLevel(uint8_t level) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.S2VS_LEVEL = level;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setShortToGroundSensitivityLevel(uint8_t level) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.S2G_LEVEL = level;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setShortRetries(uint8_t retries) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.RETRY = retries;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setParallelProtect(TMC6200_ParallelProtect parallelProtect) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.PROTECT_PARALLEL = parallelProtect;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setShortToGroundDetectionState(bool state) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.DISABLE_S2G = state ? 0 : 1;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
void TMC6200Driver::setShortToSupplyDetectionState(bool state) {
|
||||
TMC6200_SHORT_CONF shortConf;
|
||||
shortConf.reg = readRegister(TMC6200_SHORT_CONF_REG);
|
||||
|
||||
shortConf.DISABLE_S2VS = state ? 0 : 1;
|
||||
writeRegister(TMC6200_SHORT_CONF_REG, shortConf.reg);
|
||||
}
|
||||
|
||||
// TMC6200_IOIN ------
|
||||
|
||||
TMC6200_IOIN TMC6200Driver::getInputs() {
|
||||
TMC6200_IOIN ioin;
|
||||
ioin.reg = readRegister(TMC6200_IOIN_REG);
|
||||
|
||||
return ioin;
|
||||
}
|
||||
|
||||
// TMC6200_GSTAT ------
|
||||
|
||||
TMC6200GStatus TMC6200Driver::getStatus() {
|
||||
TMC6200_GSTAT gstat;
|
||||
gstat.reg = readRegister(TMC6200_GSTAT_REG);
|
||||
|
||||
return {gstat};
|
||||
}
|
||||
|
||||
void TMC6200Driver::setStatus(TMC6200_GSTAT status) {
|
||||
writeRegister(TMC6200_GSTAT_REG, status.reg);
|
||||
}
|
||||
|
||||
uint32_t TMC6200Driver::readRegister(uint8_t addr) {
|
||||
digitalWrite(csPin, LOW);
|
||||
spi->beginTransaction(settings);
|
||||
|
||||
// Address
|
||||
spi->transfer(TMC_ADDRESS(addr));
|
||||
|
||||
// 32bit Data
|
||||
uint32_t value = 0;
|
||||
value |= (spi->transfer(0x00) << 24);
|
||||
value |= (spi->transfer(0x00) << 16);
|
||||
value |= (spi->transfer(0x00) << 8);
|
||||
value |= (spi->transfer(0x00) << 0);
|
||||
|
||||
spi->end();
|
||||
digitalWrite(csPin, HIGH);
|
||||
|
||||
return value;
|
||||
}
|
||||
|
||||
void TMC6200Driver::writeRegister(uint8_t addr, uint32_t data) {
|
||||
digitalWrite(csPin, LOW);
|
||||
spi->beginTransaction(settings);
|
||||
|
||||
// Address
|
||||
spi->transfer(addr | TMC_WRITE_BIT);
|
||||
|
||||
// 32bit Data
|
||||
spi->transfer(0xFF & (data >> 24));
|
||||
spi->transfer(0xFF & (data >> 16));
|
||||
spi->transfer(0xFF & (data >> 8));
|
||||
spi->transfer(0xFF & (data >> 0));
|
||||
|
||||
spi->end();
|
||||
digitalWrite(csPin, HIGH);
|
||||
}
|
||||
|
||||
// --------------------
|
||||
|
||||
void TMC6200Driver3PWM::init(SPIClass *_spi) {
|
||||
TMC6200Driver::init(_spi);
|
||||
delayMicroseconds(1);
|
||||
TMC6200Driver::setPWMMode(TMC6200_PWMMode::SingleLine);
|
||||
BLDCDriver3PWM::init();
|
||||
};
|
||||
|
||||
|
||||
void TMC6200Driver6PWM::init(SPIClass *_spi) {
|
||||
TMC6200Driver::init(_spi);
|
||||
delayMicroseconds(1);
|
||||
TMC6200Driver::setPWMMode(TMC6200_PWMMode::Individual);
|
||||
BLDCDriver6PWM::init();
|
||||
};
|
||||
171
firmware/lib/Arduino-FOC-drivers/src/drivers/tmc6200/TMC6200.hpp
Normal file
171
firmware/lib/Arduino-FOC-drivers/src/drivers/tmc6200/TMC6200.hpp
Normal file
@@ -0,0 +1,171 @@
|
||||
#pragma once
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <SPI.h>
|
||||
#include <drivers/BLDCDriver3PWM.h>
|
||||
#include <drivers/BLDCDriver6PWM.h>
|
||||
#include "TMC6200_Registers.hpp"
|
||||
|
||||
#define TMC6200_VERSION 0x10
|
||||
|
||||
enum TMC6200_PWMMode {
|
||||
Individual = 0, // 6PWM
|
||||
SingleLine = 1, // 3PWM
|
||||
};
|
||||
|
||||
enum TMC6200_AmplificationGain {
|
||||
_5 = 0,
|
||||
_10 = 1,
|
||||
_Also_10 = 2, // maybe just remove this
|
||||
_20 = 3,
|
||||
};
|
||||
|
||||
enum TMC6200_ShortDelay {
|
||||
_750nS = 0,
|
||||
_1500nS = 1,
|
||||
};
|
||||
|
||||
enum TMC6200_ShortFilter {
|
||||
_100nS = 0,
|
||||
_1uS = 1,
|
||||
_2uS = 2,
|
||||
_3uS = 3,
|
||||
};
|
||||
|
||||
enum TMC6200_ParallelProtect {
|
||||
Detected = 0,
|
||||
All = 1,
|
||||
};
|
||||
|
||||
enum TMC6200_FaultDirect {
|
||||
AtLeastOneBridgeDown = 0,
|
||||
EachProtectiveAction = 1,
|
||||
};
|
||||
|
||||
enum TMC6200_OTSelect {
|
||||
_150 = 0b00,
|
||||
_143 = 0b01,
|
||||
_136 = 0b10,
|
||||
_120 = 0b11,
|
||||
};
|
||||
|
||||
enum TMC6200_DRVStrength {
|
||||
Weak = 0b00,
|
||||
WeakTC = 0b01, // (medium above OTPW level)
|
||||
Medium = 0b10,
|
||||
Strong = 0b11,
|
||||
};
|
||||
|
||||
class TMC6200GStatus {
|
||||
public:
|
||||
TMC6200GStatus(TMC6200_GSTAT status) : status(status) {};
|
||||
|
||||
bool isReset() const { return status.RESET == 0b1; };
|
||||
|
||||
bool isOverTemperaturePreWarning() const { return status.DRV_OTPW == 0b1; };
|
||||
|
||||
bool isOverTemperature() const { return status.DRV_OT == 0b1; };
|
||||
|
||||
bool isChargePumpUnderVoltage() const { return status.UV_CP == 0b1; };
|
||||
|
||||
bool hasUShorted() const { return status.SHORT_DET_U == 0b1; };
|
||||
|
||||
bool hasVShorted() const { return status.SHORT_DET_V == 0b1; };
|
||||
|
||||
bool hasWShorted() const { return status.SHORT_DET_W == 0b1; };
|
||||
|
||||
bool isUShortedToGround() const { return status.S2GU == 0b1; };
|
||||
|
||||
bool isUShortedToSupply() const { return status.S2VSU == 0b1; };
|
||||
|
||||
bool isVShortedToGround() const { return status.S2GV == 0b1; };
|
||||
|
||||
bool isVShortedToSupply() const { return status.S2VSV == 0b1; };
|
||||
|
||||
bool isWShortedToGround() const { return status.S2GW == 0b1; };
|
||||
|
||||
bool isWShortedToSupply() const { return status.S2VSW == 0b1; };
|
||||
|
||||
TMC6200_GSTAT status;
|
||||
};
|
||||
|
||||
class TMC6200Driver {
|
||||
|
||||
public:
|
||||
TMC6200Driver(int csPin) : csPin(csPin), spi(&SPI), settings(1000000, MSBFIRST, SPI_MODE3) {};
|
||||
|
||||
virtual ~TMC6200Driver() {};
|
||||
|
||||
virtual void init(SPIClass *_spi);
|
||||
|
||||
void setDriverState(bool state);
|
||||
|
||||
void setPWMMode(TMC6200_PWMMode pwmMode);
|
||||
|
||||
void setFaultDirect(TMC6200_FaultDirect faultDirect);
|
||||
|
||||
void setCurrentSenseGain(TMC6200_AmplificationGain amplificationGain);
|
||||
|
||||
void setOverTemperatureThreshold(TMC6200_OTSelect otLevel);
|
||||
|
||||
void setDriverStrength(TMC6200_DRVStrength strength);
|
||||
|
||||
void setCurrentSenseAmplifierState(bool state);
|
||||
|
||||
void setShortDelay(TMC6200_ShortDelay shortDelay);
|
||||
|
||||
void shortFilter(TMC6200_ShortFilter shortFilter);
|
||||
|
||||
void setShortToSupplySensitivityLevel(uint8_t level);
|
||||
|
||||
void setShortToGroundSensitivityLevel(uint8_t level);
|
||||
|
||||
void setShortRetries(uint8_t retries);
|
||||
|
||||
void setParallelProtect(TMC6200_ParallelProtect parallelProtect);
|
||||
|
||||
void setShortToGroundDetectionState(bool state);
|
||||
|
||||
void setShortToSupplyDetectionState(bool state);
|
||||
|
||||
void setBBMCycles(uint8_t clockCycles);
|
||||
|
||||
void setStatus(TMC6200_GSTAT status);
|
||||
|
||||
TMC6200GStatus getStatus();
|
||||
|
||||
TMC6200_IOIN getInputs();
|
||||
|
||||
private:
|
||||
uint32_t readRegister(uint8_t addr);
|
||||
|
||||
void writeRegister(uint8_t addr, uint32_t data);
|
||||
|
||||
int csPin;
|
||||
SPIClass *spi;
|
||||
SPISettings settings;
|
||||
};
|
||||
|
||||
|
||||
class TMC6200Driver3PWM : public TMC6200Driver, public BLDCDriver3PWM {
|
||||
|
||||
public:
|
||||
TMC6200Driver3PWM(int phA, int phB, int phC, int cs, int en = NOT_SET) :
|
||||
TMC6200Driver(cs), BLDCDriver3PWM(phA, phB, phC, en) {};
|
||||
|
||||
~TMC6200Driver3PWM() override = default;
|
||||
|
||||
virtual void init(SPIClass *_spi = &SPI) override;
|
||||
};
|
||||
|
||||
|
||||
class TMC6200Driver6PWM : public TMC6200Driver, public BLDCDriver6PWM {
|
||||
|
||||
public:
|
||||
TMC6200Driver6PWM(int phA_h, int phA_l, int phB_h, int phB_l, int phC_h, int phC_l, int cs, int en = NOT_SET) :
|
||||
TMC6200Driver(cs), BLDCDriver6PWM(phA_h, phA_l, phB_h, phB_l, phC_h, phC_l, en) {};
|
||||
|
||||
~TMC6200Driver6PWM() override = default;
|
||||
|
||||
virtual void init(SPIClass *_spi = &SPI) override;
|
||||
};
|
||||
@@ -0,0 +1,111 @@
|
||||
#pragma once
|
||||
|
||||
// ------
|
||||
|
||||
#define TMC_WRITE_BIT 0x80
|
||||
#define TMC_ADDRESS_MASK 0x7F
|
||||
#define TMC_ADDRESS(x) ((x) & (TMC_ADDRESS_MASK))
|
||||
|
||||
// ------
|
||||
|
||||
#define TMC6200_GCONF_REG 0x00 // RW
|
||||
#define TMC6200_GSTAT_REG 0x01 // RW + WC
|
||||
#define TMC6200_IOIN_REG 0x04 // R
|
||||
|
||||
#define TMC6200_OTP_PROG_REG 0x06 // not used
|
||||
#define TMC6200_OTP_READ_REG 0x07 // not used
|
||||
#define TMC6200_FACTORY_CONF_REG 0x08 // not used
|
||||
|
||||
#define TMC6200_SHORT_CONF_REG 0x09 // RW
|
||||
#define TMC6200_DRV_CONF_REG 0x0A // RW
|
||||
|
||||
typedef union {
|
||||
struct {
|
||||
uint32_t DISABLE: 1;
|
||||
uint32_t SINGLE_LINE: 1;
|
||||
uint32_t FAULT_DIRECT: 1;
|
||||
uint32_t : 1;
|
||||
uint32_t AMPLIFICATION: 2;
|
||||
uint32_t AMPLIFIER_OFF: 1;
|
||||
uint32_t TMC6200_TEST_MODE: 1;
|
||||
uint32_t : 24;
|
||||
};
|
||||
uint32_t reg;
|
||||
} TMC6200_GCONF;
|
||||
|
||||
typedef union {
|
||||
struct {
|
||||
uint32_t RESET: 1;
|
||||
uint32_t DRV_OTPW: 1;
|
||||
uint32_t DRV_OT: 1;
|
||||
uint32_t UV_CP: 1;
|
||||
|
||||
uint32_t SHORT_DET_U: 1;
|
||||
uint32_t S2GU: 1;
|
||||
uint32_t S2VSU: 1;
|
||||
uint32_t : 1;
|
||||
|
||||
uint32_t SHORT_DET_V: 1;
|
||||
uint32_t S2GV: 1;
|
||||
uint32_t S2VSV: 1;
|
||||
uint32_t : 1;
|
||||
|
||||
uint32_t SHORT_DET_W: 1;
|
||||
uint32_t S2GW: 1;
|
||||
uint32_t S2VSW: 1;
|
||||
uint32_t : 1;
|
||||
};
|
||||
uint32_t reg;
|
||||
} TMC6200_GSTAT;
|
||||
|
||||
typedef union {
|
||||
struct {
|
||||
unsigned UL: 1;
|
||||
uint32_t UH: 1;
|
||||
uint32_t VL: 1;
|
||||
uint32_t VH: 1;
|
||||
uint32_t WL: 1;
|
||||
uint32_t WH: 1;
|
||||
uint32_t DRV_EN: 1;
|
||||
uint32_t _reserved1: 1;
|
||||
uint32_t OTPW: 1;
|
||||
uint32_t OT136C: 1;
|
||||
uint32_t OT143C: 1;
|
||||
uint32_t OT150C: 1;
|
||||
uint32_t _reserved2: 12;
|
||||
uint32_t VERSION: 8;
|
||||
};
|
||||
|
||||
uint32_t reg;
|
||||
} TMC6200_IOIN;
|
||||
|
||||
typedef union {
|
||||
struct {
|
||||
uint32_t S2VS_LEVEL: 4;
|
||||
uint32_t : 4;
|
||||
uint32_t S2G_LEVEL: 4;
|
||||
uint32_t : 4;
|
||||
uint32_t SHORT_FILTER: 2;
|
||||
uint32_t : 2;
|
||||
uint32_t SHORT_DELAY: 1;
|
||||
uint32_t : 4;
|
||||
uint32_t RETRY: 2;
|
||||
uint32_t : 2;
|
||||
uint32_t PROTECT_PARALLEL: 1;
|
||||
uint32_t DISABLE_S2G: 1;
|
||||
uint32_t DISABLE_S2VS: 1;
|
||||
uint32_t : 2;
|
||||
};
|
||||
uint32_t reg;
|
||||
} TMC6200_SHORT_CONF;
|
||||
|
||||
typedef union {
|
||||
struct {
|
||||
uint32_t BBMCLKS: 5;
|
||||
uint32_t : 11;
|
||||
uint32_t OT_SELECT: 2;
|
||||
uint32_t DRV_STRENGTH: 2;
|
||||
uint32_t : 12;
|
||||
};
|
||||
uint32_t reg;
|
||||
} TMC6200_DRV_CONF;
|
||||
Reference in New Issue
Block a user