Unknow/beyon-motion
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
beyon-motion/Clean_TMC2209/Tmc2209.c

831 lines
27 KiB
C
Raw Permalink Normal View History

Initial commit
2026-03-31 13:10:37 +02:00
// Tmc2209.c
#include "Tmc2209.h"
#define portMAX_DELAY 10
#define UART_BUFFER_SIZE 64 // Taille du tampon UART, à ajuster selon vos besoins
// Adresses des registres
#define GCONF 0x00
#define GSTAT 0x01
#define IFCNT 0x02
#define IOIN 0x06
#define IHOLD_IRUN 0x10
#define TSTEP 0x12
#define TCOOLTHRS 0x14
#define SGTHRS 0x40
#define SG_RESULT 0x41
#define MSCNT 0x6A
#define CHOPCONF 0x6C
#define DRVSTATUS 0x6F
#define VACTUAL 0x22
// Bits associés à GCONF
#define I_SCALE_ANALOG (1 << 0)
#define INTERNAL_RSENSE (1 << 1)
#define EN_SPREADCYCLE (1 << 2)
#define SHAFT (1 << 3)
#define INDEX_OTPW (1 << 4)
#define INDEX_STEP (1 << 5)
#define MSTEP_REG_SELECT (1 << 7)
// Bits associés à GSTAT
#define RESET (1 << 0)
#define DRV_ERR (1 << 1)
#define UV_CP (1 << 2)
// Bits associés à CHOPCONF
#define VSENSE (1 << 17)
#define MSRES0 (1 << 24)
#define MSRES1 (1 << 25)
#define MSRES2 (1 << 26)
#define MSRES3 (1 << 27)
#define INTPOL (1 << 28)
// Bits associés à IOIN
#define IO_ENN (1 << 0)
#define IO_STEP (1 << 7)
#define IO_SPREAD (1 << 8)
#define IO_DIR (1 << 9)
// Bits associés à DRVSTATUS
#define STST (1U << 31)
#define STEALTH (1U << 30)
#define CS_ACTUAL (31U << 16)
#define T157 (1 << 11)
#define T150 (1 << 10)
#define T143 (1 << 9)
#define T120 (1 << 8)
#define OLB (1 << 7)
#define OLA (1 << 6)
#define S2VSB (1 << 5)
#define S2VSA (1 << 4)
#define S2GB (1 << 3)
#define S2GA (1 << 2)
#define OT (1 << 1)
#define OTPW (1 << 0)
// Bits associés à IHOLD_IRUN
#define IHOLD (31 << 0)
#define IRUN (31 << 8)
#define IHOLDDELAY (15 << 16)
// Bits associés à SGTHRS
#define SGTHRS_VAL (255 << 0)
// Autres constantes
#define MRES_256 0
#define MRES_128 1
#define MRES_64 2
#define MRES_32 3
#define MRES_16 4
#define MRES_8 5
#define MRES_4 6
#define MRES_2 7
#define MRES_1 8
void TMC2209_Init(TMC2209* tmc2209,TMC_UART* serial_instance) {
tmc2209->serial_instance = serial_instance;
tmc2209->mtr_id = 0x00;
tmc2209->rFrame[0] = 0x55;
tmc2209->rFrame[1] = 0x00;
tmc2209->rFrame[2] = 0x00;
tmc2209->rFrame[3] = 0x00;
tmc2209->wFrame[0] = 0x55;
tmc2209->wFrame[1] = 0x00;
tmc2209->wFrame[2] = 0x00;
tmc2209->wFrame[3] = 0x00;
tmc2209->wFrame[4] = 0x00;
tmc2209->wFrame[5] = 0x00;
tmc2209->wFrame[6] = 0x00;
tmc2209->wFrame[7] = 0x00;
tmc2209->result[0] = 0x00;
tmc2209->result[1] = 0x00;
tmc2209->result[2] = 0x00;
tmc2209->result[3] = 0x00;
// return true;
}
uint8_t compute_crc8_atm(const uint8_t *datagram, size_t length, uint8_t initial_value) {
uint8_t crc = initial_value;
size_t i;
int j;
for (i = 0; i < length; i++) {
uint8_t byte = datagram[i];
for (j = 0; j < 8; j++) {
if ((crc >> 7) ^ (byte & 0x01)) {
crc = ((crc << 1) ^ 0x07) & 0xFF;
} else {
crc = (crc << 1) & 0xFF;
}
byte >>= 1;
}
}
return crc;
}
uint64_t *read_reg(TMC2209* tmc2209, int reg) {
tmc2209->rFrame[0] = 0x55;
tmc2209->rFrame[1] = tmc2209->mtr_id;
tmc2209->rFrame[2] = reg;
tmc2209->rFrame[3] = compute_crc8_atm(tmc2209->rFrame, 3, 0);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[0]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[1]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[2]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->rFrame[3]);
uint64_t temp_result[12] = {0x00};
int i = 0;
uint32_t start_time = time_us_32(); // Temps de départ en microsecondes
while (time_us_32() - start_time < 2000 && i < 12) { // Lire pendant 2 ms
if (uart_is_readable(tmc2209->serial_instance->UART_ID)) {
uint64_t tempchar = uart_getc(tmc2209->serial_instance->UART_ID);
temp_result[i] = tempchar;
i++;
}
}
if (temp_result[11] == 0 ){
tmc2209->result[0] = temp_result[6];
tmc2209->result[1] = temp_result[7];
tmc2209->result[2] = temp_result[8];
tmc2209->result[3] = temp_result[9];
}else{
tmc2209->result[0] = temp_result[7];
tmc2209->result[1] = temp_result[8];
tmc2209->result[2] = temp_result[9];
tmc2209->result[3] = temp_result[10];
}
sleep_ms(3);
return tmc2209->result;
}
uint32_t read_int(TMC2209* tmc2209, int reg) {
uint8_t data[4]; // Array to store the 4-byte response
for (int tries = 0; tries < 10; tries++) {
uint64_t* rtn = read_reg(tmc2209, reg);
if (rtn == NULL) {
printf("TMC2209: read_reg failed to return data\n");
continue;
}
if (sizeof(*rtn) >= sizeof(data)) {
for (int i = 0; i < sizeof(data); i++) {
data[i] = (uint8_t)(*rtn >> (8 * (sizeof(*rtn) - i - 1)));
}
} else {
printf("TMC2209: expected at least 4 bytes, got %zu Bytes\n", sizeof(*rtn));
continue;
}
uint32_t result_int = (data[0] << 24) | (data[1] << 16) | (data[2] << 8) | data[3];
if (!(result_int & 0x80000000)) {
return result_int;
} else {
printf("TMC2209: error in register response: 0x%08X\n", result_int);
}
}
printf("TMC2209: failed to read register after 10 tries\n");
printf("TMC2209: is Stepper Powersupply switched on?\n");
exit(EXIT_FAILURE);
}
bool write_reg(TMC2209* tmc2209, int reg, int val) {
tmc2209->wFrame[0] = 0x55;
tmc2209->wFrame[1] = tmc2209->mtr_id;
tmc2209->wFrame[2] = reg | 0x80;
tmc2209->wFrame[3] = (val >> 24) & 0xFF;
tmc2209->wFrame[4] = (val >> 16) & 0xFF;
tmc2209->wFrame[5] = (val >> 8) & 0xFF;
tmc2209->wFrame[6] = val & 0xFF;
tmc2209->wFrame[7] = compute_crc8_atm(tmc2209->wFrame, sizeof(tmc2209->wFrame) - 1, 0);
// printf("val : %d\n",val);
// printf("Frames :");
// for (int i = 0; i < 8; i++) {
// printf("%X ", tmc2209->wFrame[i]);
// }
// printf("\n");
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[0]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[1]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[2]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[3]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[4]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[5]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[6]);
uart_putc(tmc2209->serial_instance->UART_ID, tmc2209->wFrame[7]);
sleep_ms(3);
return true;
}
bool write_reg_check(TMC2209* tmc2209, int reg, int val) {
int ifcnt12 = read_int(tmc2209, IFCNT);
unsigned int ifcnt1 = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
write_reg(tmc2209, reg, val);
int ifcnt22 = read_int(tmc2209, IFCNT);
unsigned int ifcnt2 = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (ifcnt1 >= ifcnt2) {
printf("TMC2209: writing not successful!\n");
printf("reg: %X val: %d\n", reg, val);
printf("ifcnt: %d %d\n", ifcnt1, ifcnt2);
return false;
}
return true;
}
void TMC2209_destroy(TMC2209* tmc2209) {
// free(tmc2209);
}
void driver_status(TMC2209* tmc2209) {
printf("TMC2209: ---\n");
printf("TMC2209: DRIVER STATUS:\n");
read_int(tmc2209, DRVSTATUS);
unsigned int drvstatus = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
printf("Drvstatus : %d \n",drvstatus);
if (drvstatus & STST) {
printf("TMC2209: Info: motor is standing still\n");
} else {
printf("TMC2209: Info: motor is running\n");
}
if (drvstatus & STEALTH) {
printf("TMC2209: Info: motor is running on StealthChop\n");
} else {
printf("TMC2209: Info: motor is running on SpreadCycle\n");
}
int cs_act = drvstatus & CS_ACTUAL;
cs_act >>= 16;
printf("TMC2209: CS actual: %d\n", cs_act);
if (drvstatus & OLB) {
printf("TMC2209: Warning: Open load detected on phase B\n");
}
if (drvstatus & OLA) {
printf("TMC2209: Warning: Open load detected on phase A\n");
}
if (drvstatus & S2VSB) {
printf("TMC2209: Error: Short on low-side MOSFET detected on phase B. The driver becomes disabled\n");
}
if (drvstatus & S2GA) {
printf("TMC2209: Error: Short on low-side MOSFET detected on phase A. The driver becomes disabled\n");
}
if (drvstatus & S2GB) {
printf("TMC2209: Error: Short to GND detected on phase B. The driver becomes disabled.\n");
}
if (drvstatus & S2GA) {
printf("TMC2209: Error: Short to GND detected on phase A. The driver becomes disabled.\n");
}
if (drvstatus & OT) {
printf("TMC2209: Error: Driver Overheating!\n");
}
if (drvstatus & OTPW) {
printf("TMC2209: Warning: Driver Overheating Prewarning!\n");
}
}
// Fonction general_config
void general_config(TMC2209* tmc2209) {
printf("TMC2209: ---\n");
printf("TMC2209: GENERAL CONFIG\n");
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
printf("Gconf : %d \n",gconf);
if (gconf & I_SCALE_ANALOG) {
printf("TMC2209: Driver is using voltage supplied to VREF as current reference\n");
} else {
printf("TMC2209: Driver is using internal reference derived from 5VOUT\n");
}
if (gconf & INTERNAL_RSENSE) {
printf("TMC2209: Internal sense resistors. Use current supplied into VREF as reference.\n");
printf("TMC2209: VREF pin internally is driven to GND in this mode.\n");
printf("TMC2209: This will most likely destroy your driver!!!\n");
exit(EXIT_FAILURE);
} else {
printf("TMC2209: Operation with external sense resistors\n");
}
if (gconf & EN_SPREADCYCLE) {
printf("TMC2209: SpreadCycle mode enabled\n");
} else {
printf("TMC2209: StealthChop PWM mode enabled\n");
}
if (gconf & SHAFT) {
printf("TMC2209: Inverse motor direction\n");
} else {
printf("TMC2209: normal motor direction\n");
}
if (gconf & INDEX_OTPW) {
printf("TMC2209: INDEX pin outputs overtemperature prewarning flag\n");
} else {
printf("TMC2209: INDEX shows the first microstep position of sequencer\n");
}
if (gconf & INDEX_STEP) {
printf("TMC2209: INDEX output shows step pulses from internal pulse generator\n");
} else {
printf("TMC2209: INDEX output as selected by index_otpw\n");
}
}
// Fonction general_stat
void general_stat(TMC2209* tmc2209) {
printf("TMC2209: ---\n");
printf("TMC2209: GENERAL STAT\n");
read_int(tmc2209, GSTAT);
unsigned int gstat = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
printf("Gstat : %d \n",gstat);
if (gstat & RESET) {
printf("TMC2209: The driver has been reset since the last read access to GSTAT\n");
} else {
printf("TMC2209: The driver has not been reset since the last read access to GSTAT\n");
}
if (gstat & DRV_ERR) {
printf("TMC2209: The driver has been shut down due to overtemperature or short circuit detection since the last read access\n");
} else {
printf("TMC2209: The driver has not been shut down due to overtemperature or short circuit detection since the last read access\n");
}
if (gstat & UV_CP) {
printf("TMC2209: Undervoltage detected on the charge pump. The driver is disabled in this case\n");
} else {
printf("TMC2209: No undervoltage detected on the charge pump. The driver is not disabled in this case\n");
}
printf("end Gstat\n");
}
bool set_vactual(TMC2209* tmc2209, int value) {
return write_reg_check(tmc2209, VACTUAL, value);
}
unsigned int set_bit(unsigned int value, unsigned int bit) {
unsigned int temp = value | bit;
// printf("value %d ,set_bit temp : %d\n" ,value,temp);
return temp;
}
// Fonction pour définir un bit spécifique à 0
unsigned int clear_bit(unsigned int value, unsigned int bit) {
unsigned int temp = value & ~(bit);
// printf("value %d ,clear_bit temp : %d\n" ,value,temp);
return temp;
}
void clear_general_stat(TMC2209* tmc2209) {
read_int(tmc2209, GSTAT);
unsigned int gstat= (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
gstat = set_bit(gstat, RESET);
gstat = set_bit(gstat, DRV_ERR);
write_reg_check(tmc2209, GSTAT, gstat);
}
void set_voltage_sense(TMC2209* tmc2209, bool enabled) {
read_int(tmc2209, CHOPCONF); // Lire la valeur actuelle du registre CHOPCONF
unsigned int chopconf= (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (enabled) {
chopconf |= VSENSE;
} else {
chopconf &= ~VSENSE;
}
if (!write_reg_check(tmc2209, CHOPCONF, chopconf)) {
printf("Erreur lors de l'écriture de la valeur dans le registre CHOPCONF\n");
}
}
bool get_voltage_sense(TMC2209* tmc2209) {
read_int(tmc2209, CHOPCONF);
unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return (chopconf & VSENSE) != 0;
}
void set_current_flow(TMC2209* tmc2209,double run_current, double hold_current_multiplier, int hold_current_delay, double Vref) {
double CS_IRun = 0.0;
double Rsense = 0.11;
double Vfs = 0.0;
Vref = 5.4;
hold_current_multiplier = 0.5;
hold_current_delay = 10;
run_current = 1800;
int voltage_sense = get_voltage_sense(tmc2209);
if (voltage_sense) {
Vfs = 0.180 * Vref / 2.5;
} else {
Vfs = 0.325 * Vref / 2.5;
}
CS_IRun = 32.0 * 1.41421 * run_current / 1000.0 * (Rsense + 0.02) / Vfs - 1;
CS_IRun = fmin(CS_IRun, 31);
CS_IRun = fmax(CS_IRun, 0);
double CS_IHold = hold_current_multiplier * CS_IRun;
CS_IRun = round(CS_IRun);
CS_IHold = round(CS_IHold);
// printf("CS_IHold_rounded %.3f, CS_IRun_rounded %.3f, hold_current_delay %d\n" , CS_IHold, CS_IRun, hold_current_delay);
set_irun_ihold(tmc2209,CS_IHold, CS_IRun, hold_current_delay);
}
int get_iscale_analog(TMC2209* tmc2209) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return (gconf & I_SCALE_ANALOG) ;
}
void set_iscale_analog(TMC2209* tmc2209, bool enabled) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (enabled) {
gconf = set_bit(gconf, I_SCALE_ANALOG);
} else {
gconf = clear_bit(gconf, I_SCALE_ANALOG);
}
write_reg_check(tmc2209, GCONF, gconf);
}
int get_interpolation(TMC2209* tmc2209) {
read_int(tmc2209, CHOPCONF);
unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return (chopconf & INTPOL) ;
}
void set_interpolation(TMC2209* tmc2209, bool enabled) {
read_int(tmc2209, CHOPCONF);
unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (enabled) {
chopconf = set_bit(chopconf, INTPOL);
} else {
chopconf = clear_bit(chopconf, INTPOL);
}
write_reg_check(tmc2209, CHOPCONF, chopconf);
}
int get_internal_resistor_sense(TMC2209* tmc2209) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return (gconf & INTERNAL_RSENSE) ;
}
void set_internal_resistor_sense(TMC2209* tmc2209, bool enabled) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (enabled) {
gconf = set_bit(gconf, INTERNAL_RSENSE);
} else {
gconf = clear_bit(gconf, INTERNAL_RSENSE);
}
write_reg_check(tmc2209, GCONF, gconf);
}
int get_spread_cycle(TMC2209* tmc2209) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return (gconf & EN_SPREADCYCLE) ;
}
void set_spread_cycle(TMC2209* tmc2209, bool enabled) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (enabled) {
gconf = set_bit(gconf, EN_SPREADCYCLE);
} else {
gconf = clear_bit(gconf, EN_SPREADCYCLE);
}
write_reg_check(tmc2209, GCONF, gconf);
}
void set_microstepping_resolution(TMC2209* tmc2209, int msres) {
read_int(tmc2209, CHOPCONF);
unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
// printf("chopconf %d \n",chopconf);
int msresdezimal = 8 - (int)log2(msres);
chopconf &= 4043309055;
chopconf |= msresdezimal << 24;
write_reg_check(tmc2209, CHOPCONF, chopconf);
set_microstep_resolution_regselect(tmc2209, true);
}
int get_microstepping_resolution(TMC2209* tmc2209) {
read_int(tmc2209, CHOPCONF);
unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
// printf("chopconf : %d\n", chopconf);
int msresdezimal = chopconf & (MSRES0 | MSRES1 | MSRES2 | MSRES3);
msresdezimal = msresdezimal >> 24;
msresdezimal = 8 - msresdezimal;
int micro_stepping_resolution = (int)pow(2, msresdezimal);
return micro_stepping_resolution;
}
void set_microstep_resolution_regselect(TMC2209* tmc2209, bool enabled) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (enabled) {
gconf = set_bit(gconf, MSTEP_REG_SELECT);
} else {
gconf = clear_bit(gconf, MSTEP_REG_SELECT);
}
// printf("gconf : %d \n",gconf);
write_reg_check(tmc2209, GCONF, gconf);
}
void set_irun_ihold(TMC2209* tmc2209, double IHold, double IRun, int IHoldDelay) {
// Convertir les valeurs doubles en entiers
int ihold_irun = 0;
int ih = (int)IHold;
int ir = (int)IRun;
ihold_irun |= ih << 0;
// printf("ihold_irun1 : %d\n",ihold_irun);
ihold_irun |= ir << 8;
// printf("ihold_irun2 : %d\n",ihold_irun);
ihold_irun |= IHoldDelay << 16;
// printf("ihold_irun3 : %d\n",ihold_irun);
write_reg_check(tmc2209, IHOLD_IRUN, ihold_irun);
}
int get_direction_shart(TMC2209* tmc2209) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return (gconf & SHAFT) ;
}
void ioin(TMC2209* tmc2209) {
printf("TMC2209: ---\n");
printf("TMC2209: INPUTS\n");
read_int(tmc2209, IOIN);
unsigned int ioin = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (ioin & IO_SPREAD) {
printf("TMC2209: spread is high\n");
} else {
printf("TMC2209: spread is low\n");
}
if (ioin & IO_DIR) {
printf("TMC2209: dir is high\n");
} else {
printf("TMC2209: dir is low\n");
}
if (ioin & IO_STEP) {
printf("TMC2209: step is high\n");
} else {
printf("TMC2209: step is low\n");
}
if (ioin & IO_ENN) {
printf("TMC2209: en is high\n");
} else {
printf("TMC2209: en is low\n");
}
}
void chopper_control(TMC2209* tmc2209, int direction) {
printf("TMC2209: ---\n");
printf("TMC2209: CHOPPER CONTROL\n");
read_int(tmc2209, CHOPCONF);
unsigned int chopconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (chopconf & INTPOL) {
printf("TMC2209: interpolation to 256 microsteps\n");
}
if (chopconf & VSENSE) {
printf("TMC2209: 1: High sensitivity, low sense resistor voltage\n");
} else {
printf("TMC2209: 0: Low sensitivity, high sense resistor voltage\n");
}
}
void set_direction_shart(TMC2209* tmc2209, int direction) {
read_int(tmc2209, GCONF);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
int dir = get_direction_shart(tmc2209);
// printf("dir : %d , dir 8 : %d\n",dir,dir / 8);
if ((dir / 8) != direction) {
if (direction) {
gconf = set_bit(gconf, SHAFT);
} else {
gconf = clear_bit(gconf, SHAFT);
}
// printf("gconf : %d \n",gconf);
write_reg_check(tmc2209, GCONF, gconf);
sleep_us(100);
}
}
int get_stallguard(TMC2209* tmc2209) {
read_int(tmc2209, SG_RESULT);
unsigned int stg = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return stg ;
}
void set_stallguard_threshold(TMC2209* tmc2209, int threshold){
write_reg_check(tmc2209,SGTHRS, threshold);
}
void set_coolstep_threshold(TMC2209* tmc2209, int threshold){
write_reg_check(tmc2209,TCOOLTHRS, threshold);
}
void set_stallguard_callback(TMC2209* tmc2209, int stallguard_pin, int threshold, int callback, int min_speed){
set_stallguard_threshold(tmc2209,threshold);
set_coolstep_threshold(tmc2209,min_speed);
gpio_set_function(stallguard_pin, GPIO_FUNC_SIO);
gpio_pull_up(stallguard_pin);
gpio_set_irq_enabled_with_callback(stallguard_pin, GPIO_IRQ_EDGE_RISE, true, callback);
}
int get_tstep(TMC2209* tmc2209) {
read_int(tmc2209, TSTEP);
unsigned int gconf = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
return gconf ;
}
int get_microstep_counter(TMC2209* tmc2209, bool in_steps, int offset) {
read_int(tmc2209, MSCNT);
unsigned int mscnt = (unsigned int)tmc2209->result[0] << 24 |
(unsigned int)tmc2209->result[1] << 16 |
(unsigned int)tmc2209->result[2] << 8 |
(unsigned int)tmc2209->result[3];
if (!in_steps) {
return mscnt;
}
int micro_stepping_resolution = get_microstepping_resolution(tmc2209); // Remplacez par la résolution réelle du micro-pas
return round((4 * micro_stepping_resolution) - ((mscnt - 64) * (micro_stepping_resolution * 4) / 1024) - 1) + offset;
}
Reference in New Issue Copy Permalink