Unknow/beyon-motion
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
Alexandre Gut
2026-03-31 13:10:37 +02:00
commit 03325b9502
566 changed files with 351758 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

93
Motor_encod/AS5600L.c Normal file
View File

@@ -0,0 +1,93 @@
#include "AS5600L.h"
void as560x_init(AS5600L* as5600l, i2c_inst_t *I2C_ID, int frq , int sda, int scl ) {
i2c_init(I2C_ID, frq);
gpio_set_function(sda, GPIO_FUNC_I2C);
gpio_set_function(scl, GPIO_FUNC_I2C);
gpio_pull_up(sda);
gpio_pull_up(scl);
AS5600L_I2C instance_i2c;
instance_i2c.I2C_PORT = I2C_ID;
instance_i2c.FREQUENCE = frq;
instance_i2c.I2C_SDA_PIN = sda;
instance_i2c.I2C_SCL_PIN = scl;
as5600l->serial_instance = instance_i2c;
as5600l->abs_pos = 0;
as5600l->curr_angle = 0;
as5600l->last_angle = 0;
as5600l->start_pos = 0;
as5600l->start_pos = as560xReadAngle(as5600l);
as5600l->abs_pos = 0 - as5600l->start_pos;
}
int update_pos(AS5600L* as5600l){
// sensorData();
as5600l->curr_angle = as560xReadAngle(as5600l);
if(as5600l->last_angle > ENCODER_RESOLUTION/2 & as5600l->curr_angle < (as5600l->last_angle - ENCODER_RESOLUTION/2)){
as5600l->abs_pos = as5600l->abs_pos + ENCODER_RESOLUTION - as5600l->last_angle + as5600l->curr_angle;
}else if(as5600l->curr_angle > ENCODER_RESOLUTION/2 & as5600l->last_angle < (as5600l->curr_angle - ENCODER_RESOLUTION/2) ){
as5600l->abs_pos = as5600l->abs_pos -ENCODER_RESOLUTION - as5600l->last_angle + as5600l->curr_angle;
}else{
as5600l->abs_pos = as5600l->abs_pos - as5600l->last_angle + as5600l->curr_angle;
}
as5600l->last_angle = as5600l->curr_angle;
return as5600l->abs_pos;
}
int __bswap16(int value) {
return (value >> 8) | (value << 8);
}
void i2cError() {
printf("I2C Error\n");
}
int as560xReadReg(AS5600L* as5600l,int addr, bool wide, int mask) {
int buf;
int result = i2c_write_timeout_us(as5600l->serial_instance.I2C_PORT, AS5600L_ADDRESS, (uint8_t *)&addr, 1, true, I2C_TIMEOUT_US);
if (result <= 0) {
i2cError();
}
result = i2c_read_timeout_us(as5600l->serial_instance.I2C_PORT, AS5600L_ADDRESS, (uint8_t *)&buf, (wide ? 2 : 1), false, I2C_TIMEOUT_US);
if (result <= 0) {
i2cError();
}
if (wide) {
return __bswap16(buf) & mask;
} else {
return buf & mask;
}
}
void AS560x_print_reg16(AS5600L* as5600l,const char *formatStr, int addr, int mask) {
int result = as560xReadReg(as5600l,addr, true, mask);
printf(formatStr, result & mask);
}
void AS560x_print_reg8(AS5600L* as5600l,const char *formatStr, int addr, uint8_t mask) {
uint8_t result = (uint8_t)as560xReadReg(as5600l,addr, false, mask);
printf(formatStr, result & mask);
}
int as560xReadAngle(AS5600L* as5600l) {
return as560xReadReg(as5600l,AS560x_RAW_ANGLE_REG, true, 0xFFF) ;
}
uint8_t as560xGetStatus(AS5600L* as5600l) {
return (uint8_t)as560xReadReg(as5600l,AS560x_STATUS_REG, false, 0x38);
}
void sensorData(AS5600L* as5600l) {
AS560x_print_reg8(as5600l,"Status: %02x; ", AS560x_STATUS_REG, 0x38);
AS560x_print_reg8(as5600l,"AGC: %3x; ", 0x1a, 0xff);
AS560x_print_reg16(as5600l,"Angle: %d\n\r", AS560x_RAW_ANGLE_REG, 0xFFF);
}

47
Motor_encod/AS5600L.h Normal file
View File

@@ -0,0 +1,47 @@
#ifndef AS5600L_H
#define AS5600L_H
#include "hardware/i2c.h"
#include "pico/stdlib.h"
#include "pico/time.h"
#include <stdio.h>
#include <stdint.h>
#include <stdbool.h>
#define I2C_TIMEOUT_US (100000)
#define AS560x_STATUS_REG (0x0B)
#define AS560x_RAW_ANGLE_REG (0x0C)
#define AS5600L_ADDRESS 0x40
#define ENCODER_RESOLUTION 4096
typedef struct {
i2c_inst_t *I2C_PORT;
int FREQUENCE;
int I2C_SDA_PIN;
int I2C_SCL_PIN;
} AS5600L_I2C;
typedef struct {
AS5600L_I2C serial_instance;
int last_angle;
int curr_angle ;
int abs_pos ;
int start_pos;
} AS5600L;
void as560x_init(AS5600L* as5600l, i2c_inst_t *I2C_ID, int frq , int sda, int scl);
int update_pos(AS5600L* as5600l);
int __bswap16(int value);
void i2cError();
int as560xReadReg(AS5600L* as5600l,int addr, bool wide, int mask);
void AS560x_print_reg16(AS5600L* as5600l,const char *formatStr, int addr, int mask);
void AS560x_print_reg8(AS5600L* as5600l,const char *formatStr, int addr, uint8_t mask);
int as560xReadAngle(AS5600L* as5600l);
uint8_t as560xGetStatus(AS5600L* as5600l);
void sensorData(AS5600L* as5600l);
#endif /* AS5600L_H */

34
Motor_encod/CMakeLists.txt Normal file
View File

@@ -0,0 +1,34 @@
cmake_minimum_required(VERSION 3.12)
set(PROJECT_NAME "Motor_test")
set(PICO_SDK_PATH "E:/Beyon_Motion/pico-sdk")
set(PICO_TOOLCHAIN_PATH "C:/Users/spide/scoop/apps/gcc-arm-none-eabi/current/bin")
set(CMAKE_C_STANDARD 11)
set(CMAKE_CXX_STANDARD 17)
include(${PICO_SDK_PATH}/external/pico_sdk_import.cmake)
project(${PROJECT_NAME} C CXX ASM) # Ajout de CXX ici
pico_sdk_init()
add_executable(${PROJECT_NAME}
main.c
Tmc2209.c
Tmc_uart.c
AS5600L.c
AS5600L.h
)
target_link_libraries(${PROJECT_NAME}
pico_stdlib
hardware_i2c
hardware_uart
hardware_dma
)
pico_enable_stdio_usb(${PROJECT_NAME} 1)
pico_enable_stdio_uart(${PROJECT_NAME} 0)
pico_add_extra_outputs(${PROJECT_NAME})

627
Motor_encod/Tmc2209.c Normal file
View File

@@ -0,0 +1,627 @@
// 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 = 0;
tmc2209->rFrame[0] = 0x55;
tmc2209->rFrame[1] = 0;
tmc2209->rFrame[2] = 0;
tmc2209->rFrame[3] = 0;
tmc2209->wFrame[0] = 0x55;
tmc2209->wFrame[1] = 0;
tmc2209->wFrame[2] = 0;
tmc2209->wFrame[3] = 0;
tmc2209->wFrame[4] = 0;
tmc2209->wFrame[5] = 0;
tmc2209->wFrame[6] = 0;
tmc2209->wFrame[7] = 0;
tmc2209->result[0] = 0;
tmc2209->result[1] = 0;
tmc2209->result[2] = 0;
tmc2209->result[3] = 0;
// 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("%d ", 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];
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");
}
printf("end");
}
// 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];
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];
if (gstat & RESET) {
printf("TMC2209: The Driver has 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");
}
if (gstat & UV_CP) {
printf("TMC2209: Undervoltage on the charge pump. The driver is disabled in this case\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;
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 & ~(1 << bit);
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;
}
// Écrire la nouvelle valeur dans le registre CHOPCONF
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);
}
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);
}
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);
}
printf("chopconf : %d\n",chopconf);
write_reg_check(tmc2209, CHOPCONF, chopconf);
}
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);
}
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);
}

47
Motor_encod/Tmc2209.h Normal file
View File

@@ -0,0 +1,47 @@
// Tmc2209.h
#ifndef TMC2209_H
#define TMC2209_H
#include "pico/stdlib.h"
#include "Tmc_uart.h"
#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <math.h>
#include <string.h>
typedef struct {
TMC_UART* serial_instance;
int mtr_id;
uint8_t rFrame[4];
uint8_t wFrame[8];
uint64_t result[4];
} TMC2209;
void TMC2209_Init(TMC2209* tmc2209,TMC_UART* serial_instance); //ok
void TMC2209_Destroy(TMC2209* tmc2209); //idk
uint8_t compute_crc8_atm(const uint8_t *datagram, size_t length, uint8_t initial_value); //ok
uint64_t* read_reg(TMC2209* tmc2209, int reg) ; //ok
uint32_t read_int(TMC2209* tmc2209, int reg); //ok
bool write_reg(TMC2209* tmc2209, int reg, int val); //ok
bool write_reg_check(TMC2209* tmc2209, int reg, int val) ; //ok
void general_stat(TMC2209* tmc2209) ; //idk
void general_config(TMC2209* tmc2209); //idk
void driver_status(TMC2209* tmc2209); //idk
bool set_vactual(TMC2209* tmc2209, int value) ; //ok
void set_voltage_sense(TMC2209* tmc2209, bool enabled); //ok
bool get_voltage_sense(TMC2209* tmc2209); //ok
void set_current_flow(TMC2209* tmc2209, double run_current, double hold_current_multiplier, int hold_current_delay, double Vref);//averif
void set_iscale_analog(TMC2209* tmc2209, bool enabled); //ok
void set_interpolation(TMC2209* tmc2209, bool enabled);
void set_internal_resistor_sense(TMC2209* tmc2209, bool enabled);
void set_spread_cycle(TMC2209* tmc2209, bool enabled);
void set_microstepping_resolution(TMC2209* tmc2209, int msres); //ok
void set_microstep_resolution_regselect(TMC2209* tmc2209, bool enabled); //ok
void set_irun_ihold(TMC2209* tmc2209, double IHold, double IRun, int IHoldDelay); //ok
void clear_general_stat(TMC2209* tmc2209); //ok
int get_microstepping_resolution(TMC2209* tmc2209); //ok
#endif

36
Motor_encod/Tmc_uart.c Normal file
View File

@@ -0,0 +1,36 @@
// TMC_UART.c
#include "TMC_uart.h"
#define DEFAULT_UART_ID uart1
#define DEFAULT_BAUD_RATE 115200
#define DEFAULT_MOTOR_TX_PIN 4
#define DEFAULT_MOTOR_RX_PIN 5
TMC_UART* tmc_Uart_Init(TMC_UART* tmc_uart, uart_inst_t *UART_ID, int BAUD_RATE, int MOTOR_TX_PIN, int MOTOR_RX_PIN) {
if (BAUD_RATE == -1) BAUD_RATE = DEFAULT_BAUD_RATE;
if (MOTOR_TX_PIN == -1) MOTOR_TX_PIN = DEFAULT_MOTOR_TX_PIN;
if (MOTOR_RX_PIN == -1) MOTOR_RX_PIN = DEFAULT_MOTOR_RX_PIN;
uart_init(UART_ID, BAUD_RATE);
gpio_set_function(MOTOR_TX_PIN, GPIO_FUNC_UART);
gpio_set_function(MOTOR_RX_PIN, GPIO_FUNC_UART);
uart_set_hw_flow(UART_ID, false, false);
uart_set_format(UART_ID, 8, 1, UART_PARITY_NONE);
uart_set_fifo_enabled(UART_ID, false);
tmc_uart->UART_ID = UART_ID;
tmc_uart->BAUD_RATE = BAUD_RATE;
tmc_uart->MOTOR_TX_PIN = MOTOR_TX_PIN;
tmc_uart->MOTOR_RX_PIN = MOTOR_RX_PIN;
return tmc_uart;
}
void TMC_UART_Write(TMC_UART* tmc_uart, int data) {
uart_putc(tmc_uart->UART_ID, data);
}
void TMC_UART_Destroy(TMC_UART* tmc_uart) {
// Aucune opération nécessaire
}

18
Motor_encod/Tmc_uart.h Normal file
View File

@@ -0,0 +1,18 @@
// TMC_UART.h
#ifndef TMC_UART_H
#define TMC_UART_H
#include <stdio.h>
#include "pico/stdlib.h"
typedef struct {
uart_inst_t *UART_ID;
int BAUD_RATE;
int MOTOR_TX_PIN;
int MOTOR_RX_PIN;
} TMC_UART;
TMC_UART* tmc_Uart_Init(TMC_UART* tmc_uart, uart_inst_t *UART_ID, int BAUD_RATE, int MOTOR_TX_PIN, int MOTOR_RX_PIN);
void tmc_Uart_Destroy(TMC_UART* TMC_UART);
#endif

148
Motor_encod/main.c Normal file
View File

@@ -0,0 +1,148 @@
#include <stdio.h>
#include "pico/stdlib.h"
#include "pico/time.h"
#include "hardware/uart.h"
#include "hardware/irq.h" // Inclure le fichier d'en-tête pour les interruptions
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "Tmc_uart.h"
#include "Tmc2209.h"
#include "AS5600L.h"
#include "unistd.h"
#define I2C_PORT i2c0
#define I2C_SDA_PIN 8
#define I2C_SCL_PIN 9
int main() {
stdio_init_all();
TMC_UART uart_instance;
TMC2209 Tmc2209;
AS5600L as5600l;
// Initialisation de l'UART avec les paramètres par défaut
tmc_Uart_Init(&uart_instance, uart1, -1, -1, -1);
TMC2209_Init(&Tmc2209,&uart_instance);
as560x_init(&as5600l,I2C_PORT,100000,I2C_SDA_PIN,I2C_SCL_PIN);
int abs = 0;
abs = update_pos;
sleep_ms(4000);
// driver_status(&uart_instance);
// general_config(&uart_instance);
// clear_general_stat(&uart_instance);
printf("UART initialisé avec succès\n");
int32_t resolution = get_microstepping_resolution(&Tmc2209);
printf("Microstepping resolution begin : %ld\n", resolution);
sleep_ms(10);
set_microstepping_resolution(&Tmc2209,256);
sleep_ms(10);
resolution = get_microstepping_resolution(&Tmc2209);
printf("Microstepping resolution after new go to 1/256: %ld\n", resolution);
sleep_ms(10);
// set_microstepping_resolution(&Tmc2209,16);
// sleep_ms(10);
// resolution = get_microstepping_resolution(&Tmc2209);
// printf("Microstepping resolution after renew go to 1/16: %ld\n", resolution);
// printf(get_microstepping_resolution(&Tmc2209));
// clear_general_stat(&Tmc2209);
// sleep_ms(2000);
// set_voltage_sense(&Tmc2209, false);
// sleep_ms(2000);
// set_current_flow(&Tmc2209, 1800, 0.5, 10, 5.4);
// sleep_ms(2000);
// set_iscale_analog(&Tmc2209, false);
// sleep_ms(2000);
// set_interpolation(&Tmc2209, true);
// sleep_ms(2000);
// set_internal_resistor_sense(&Tmc2209, false);
// sleep_ms(2000);
// set_spread_cycle(&Tmc2209, true);
// sleep_ms(2000);
// set_microstepping_resolution(&Tmc2209,256);
// sleep_ms(2000);
// set_microstep_resolution_regselect(&Tmc2209, true);
// int32_t resolution = get_microstepping_resolution(&Tmc2209);
// printf("Microstepping resolution begin : %ld\n", resolution);
const uint LED_PIN = 7;
gpio_init(LED_PIN);
gpio_set_dir(LED_PIN, GPIO_OUT);
gpio_put(LED_PIN, 0);
sleep_ms(1000);
int max = 1600;
for (int i = 100; i <= max; i += 100) {
set_vactual(&Tmc2209,i*256);
abs = update_pos(&as5600l);
printf("Position absolue : %d\n",abs);
}
uint32_t start_time = time_us_32();
uint32_t current_time = start_time;
while (current_time - start_time < 10000000) { // 10 secondes en microsecondes
abs = update_pos(&as5600l);
printf("Position absolue : %d\n", abs);
// Mise à jour du temps actuel
current_time = time_us_32();
}
for (int i = max; i >= 100; i -= 100) {
set_vactual(&Tmc2209,i*256);
abs = update_pos(&as5600l);
printf("Position absolue : %d\n",abs);
}
set_vactual(&Tmc2209,0);
sleep_ms(10000);
gpio_put(LED_PIN, 1);
// int32_t resolution;
// general_config(&Tmc2209);
// while (true) {
// }
// for (int i = 1000; i <= 200000; i += 10) {
// set_vactual(&Tmc2209, i);
// sleep_ms(2);
// }
// for (int i = 20000; i >= 10000; i -= 10) {
// set_vactual(&Tmc2209, i);
// sleep_ms(2);
// }
// sleep_ms(1000);
// printf("end\n");
// // read_reg(&Tmc2209,0x6C);
// printf(driver_status(&Tmc2209));
// sleep_ms(1000);
// // char data[] = "Hello, UART!\n";
// // for (int i = 0; data[i] != '\0'; ++i) {
// // TMC_UART_Write(&Tmc2209, data[i]);
// // }
// }
return 0;
}