94 lines
3.0 KiB
C
94 lines
3.0 KiB
C
|
#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);
|
||
|
|
}
|
||
|
|
|
||
|
|
|