Initial commit

2026-03-31 13:10:37 +02:00
commit 03325b9502
566 changed files with 351758 additions and 0 deletions
--- a/AS5600L/AS5600L.c
+++ b/AS5600L/AS5600L.c
@@ -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);
+}
+
+