added files
This commit is contained in:
0x23
parent
36090af63b
commit
4cd8e6e20e
86 changed files with 74246 additions and 0 deletions
312
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
Normal file
312
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp
Normal file
|
|
@ -0,0 +1,312 @@
|
|||
#include "MT6835_encoder.h"
|
||||
#include "hardware/spi.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "pico/stdlib.h"
|
||||
|
||||
void MT6835Encoder::setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz) {
|
||||
// Set GPIO functions to SPI
|
||||
gpio_set_function(pin_sck, GPIO_FUNC_SPI);
|
||||
gpio_set_function(pin_mosi, GPIO_FUNC_SPI);
|
||||
gpio_set_function(pin_miso, GPIO_FUNC_SPI);
|
||||
|
||||
// SPI format: 8 bits, mode 3 (CPOL=1, CPHA=1)
|
||||
spi_init(spi, baudrate_hz);
|
||||
spi_set_format(spi, 8, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
|
||||
}
|
||||
|
||||
|
||||
MT6835Encoder::MT6835Encoder(spi_inst_t* spi, uint cs_pin) : spi(spi), cs_pin(cs_pin) {
|
||||
// nop
|
||||
}
|
||||
|
||||
MT6835Encoder::~MT6835Encoder() {
|
||||
// nop
|
||||
}
|
||||
|
||||
void MT6835Encoder::init(uint8_t bandwidth, uint8_t hysteresis) {
|
||||
if (cs_pin >= 0) {
|
||||
gpio_init(cs_pin);
|
||||
gpio_set_dir(cs_pin, GPIO_OUT);
|
||||
gpio_put(cs_pin, 1); // CS high
|
||||
}
|
||||
|
||||
set_bandwidth(bandwidth);
|
||||
set_hysteresis(hysteresis);
|
||||
|
||||
last_raw_angle = 0;
|
||||
abs_raw_angle = 0;
|
||||
}
|
||||
|
||||
void MT6835Encoder::reset_abs_angle(int32_t abs_raw_angle) {
|
||||
MT6835Encoder::abs_raw_angle = abs_raw_angle;
|
||||
}
|
||||
|
||||
float MT6835Encoder::read_abs_angle() {
|
||||
int32_t raw_angle = read_abs_angle_raw();
|
||||
return raw_angle * RAW_TO_ANGLE;
|
||||
}
|
||||
|
||||
int32_t MT6835Encoder::read_abs_angle_raw() {
|
||||
uint8_t data[6] = {0};
|
||||
data[0] = MT6835_OP_ANGLE << 4;
|
||||
data[1] = MT6835_REG_ANGLE1;
|
||||
// rest zero
|
||||
|
||||
spi_begin_transaction();
|
||||
spi_transfer(data, 6);
|
||||
spi_end_transaction();
|
||||
|
||||
last_status = data[4] & 0x07;
|
||||
last_crc = data[5];
|
||||
int32_t raw_angle = ((int32_t)data[2] << 13) | ((int32_t)data[3] << 5) | (data[4] >> 3);
|
||||
|
||||
if (check_crc) {
|
||||
if (last_crc != calc_crc(raw_angle, last_status)) {
|
||||
last_status |= MT6835_CRC_ERROR;
|
||||
return -1.0f; // CRC error indicator
|
||||
}
|
||||
}
|
||||
|
||||
return update_abs_raw_angle(raw_angle);
|
||||
}
|
||||
|
||||
int32_t MT6835Encoder::get_rawcounts_per_rev() {
|
||||
return MT6835_CPR;
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_status() {
|
||||
return last_status;
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_calibration_status() {
|
||||
uint8_t data[3] = {0};
|
||||
data[0] = (MT6835_OP_READ << 4) | (MT6835_REG_CAL_STATUS >> 8);
|
||||
data[1] = MT6835_REG_CAL_STATUS & 0xFF;
|
||||
|
||||
spi_begin_transaction();
|
||||
spi_transfer(data, 3);
|
||||
spi_end_transaction();
|
||||
|
||||
return data[2] >> 6;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::set_zero_from_current_position() {
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_ZERO;
|
||||
cmd.addr = 0x000;
|
||||
transfer_24(&cmd);
|
||||
abs_raw_angle = 0;
|
||||
last_raw_angle = 0;
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::write_eeprom() {
|
||||
sleep_ms(1); // wait at least 1 ms
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_PROG;
|
||||
cmd.addr = 0x000;
|
||||
transfer_24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_bandwidth() {
|
||||
MT6835Options5 opts{ .reg = read_register(MT6835_REG_OPTS5) };
|
||||
return opts.bw;
|
||||
}
|
||||
void MT6835Encoder::set_bandwidth(uint8_t bw) {
|
||||
MT6835Options5 opts{ .reg = read_register(MT6835_REG_OPTS5) };
|
||||
opts.bw = bw;
|
||||
write_register(MT6835_REG_OPTS5, opts.reg);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_hysteresis() {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
return opts.hyst;
|
||||
}
|
||||
void MT6835Encoder::set_hysteresis(uint8_t hyst) {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
opts.hyst = hyst;
|
||||
set_options3(opts);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::get_rotation_direction() {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
return opts.rot_dir;
|
||||
}
|
||||
void MT6835Encoder::set_rotation_direction(uint8_t dir) {
|
||||
MT6835Options3 opts{ .reg = get_options3().reg };
|
||||
opts.rot_dir = dir;
|
||||
set_options3(opts);
|
||||
}
|
||||
|
||||
uint16_t MT6835Encoder::get_abz_resolution() {
|
||||
uint8_t hi = read_register(MT6835_REG_ABZ_RES1);
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
return (hi << 6) | lo.abz_res_low;
|
||||
}
|
||||
void MT6835Encoder::set_abz_resolution(uint16_t res) {
|
||||
uint8_t hi = (res >> 6);
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
lo.abz_res_low = (res & 0x3F);
|
||||
write_register(MT6835_REG_ABZ_RES1, hi);
|
||||
write_register(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
}
|
||||
|
||||
bool MT6835Encoder::is_abz_enabled() {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
return lo.abz_off == 0;
|
||||
}
|
||||
void MT6835Encoder::set_abz_enabled(bool enabled) {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
lo.abz_off = enabled ? 0 : 1;
|
||||
write_register(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
}
|
||||
|
||||
bool MT6835Encoder::is_ab_swapped() {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
return lo.ab_swap == 1;
|
||||
}
|
||||
void MT6835Encoder::set_ab_swapped(bool swapped) {
|
||||
MT6835ABZRes lo{ .reg = read_register(MT6835_REG_ABZ_RES2) };
|
||||
lo.ab_swap = swapped ? 1 : 0;
|
||||
write_register(MT6835_REG_ABZ_RES2, lo.reg);
|
||||
}
|
||||
|
||||
uint16_t MT6835Encoder::get_zero_position() {
|
||||
uint8_t hi = read_register(MT6835_REG_ZERO1);
|
||||
MT6835Options0 lo{ .reg = read_register(MT6835_REG_ZERO2) };
|
||||
return (hi << 4) | lo.zero_pos_low;
|
||||
}
|
||||
void MT6835Encoder::set_zero_position(uint16_t pos) {
|
||||
uint8_t hi = (pos >> 4);
|
||||
MT6835Options0 lo{ .reg = read_register(MT6835_REG_ZERO2) };
|
||||
lo.zero_pos_low = pos & 0x0F;
|
||||
write_register(MT6835_REG_ZERO1, hi);
|
||||
write_register(MT6835_REG_ZERO2, lo.reg);
|
||||
}
|
||||
|
||||
MT6835Options1 MT6835Encoder::get_options1() {
|
||||
MT6835Options1 result{ .reg = read_register(MT6835_REG_OPTS1) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options1(MT6835Options1 opts) {
|
||||
write_register(MT6835_REG_OPTS1, opts.reg);
|
||||
}
|
||||
|
||||
MT6835Options2 MT6835Encoder::get_options2() {
|
||||
MT6835Options2 result{ .reg = read_register(MT6835_REG_OPTS2) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options2(MT6835Options2 opts) {
|
||||
MT6835Options2 val = get_options2();
|
||||
val.nlc_en = opts.nlc_en;
|
||||
val.pwm_fq = opts.pwm_fq;
|
||||
val.pwm_pol = opts.pwm_pol;
|
||||
val.pwm_sel = opts.pwm_sel;
|
||||
write_register(MT6835_REG_OPTS2, val.reg);
|
||||
}
|
||||
|
||||
MT6835Options3 MT6835Encoder::get_options3() {
|
||||
MT6835Options3 result{ .reg = read_register(MT6835_REG_OPTS3) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options3(MT6835Options3 opts) {
|
||||
MT6835Options3 val = get_options3();
|
||||
val.rot_dir = opts.rot_dir;
|
||||
val.hyst = opts.hyst;
|
||||
write_register(MT6835_REG_OPTS3, val.reg);
|
||||
}
|
||||
|
||||
MT6835Options4 MT6835Encoder::get_options4() {
|
||||
MT6835Options4 result{ .reg = read_register(MT6835_REG_OPTS4) };
|
||||
return result;
|
||||
}
|
||||
void MT6835Encoder::set_options4(MT6835Options4 opts) {
|
||||
MT6835Options4 val = get_options4();
|
||||
val.gpio_ds = opts.gpio_ds;
|
||||
val.autocal_freq = opts.autocal_freq;
|
||||
write_register(MT6835_REG_OPTS4, val.reg);
|
||||
}
|
||||
|
||||
static inline uint32_t swap_bytes(uint32_t val) {
|
||||
return __builtin_bswap32(val);
|
||||
}
|
||||
|
||||
void MT6835Encoder::transfer_24(MT6835Command* cmd) {
|
||||
uint32_t buff = swap_bytes(cmd->val);
|
||||
spi_begin_transaction();
|
||||
spi_transfer((uint8_t*)&buff, 3);
|
||||
spi_end_transaction();
|
||||
cmd->val = swap_bytes(buff);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::read_register(uint16_t reg) {
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_READ;
|
||||
cmd.addr = reg;
|
||||
transfer_24(&cmd);
|
||||
return cmd.data;
|
||||
}
|
||||
|
||||
bool MT6835Encoder::write_register(uint16_t reg, uint8_t value) {
|
||||
MT6835Command cmd{};
|
||||
cmd.cmd = MT6835_OP_WRITE;
|
||||
cmd.addr = reg;
|
||||
cmd.data = value;
|
||||
transfer_24(&cmd);
|
||||
return cmd.data == MT6835_WRITE_ACK;
|
||||
}
|
||||
|
||||
MT6835Encoder::AbsRawAngleType MT6835Encoder::update_abs_raw_angle(int32_t raw_angle) {
|
||||
if(raw_angle >= 0) {
|
||||
int32_t half_max = MT6835_CPR>>1;
|
||||
|
||||
int32_t d = raw_angle - last_raw_angle;
|
||||
if (d > half_max) d -= MT6835_CPR;
|
||||
else if (d < -half_max) d += MT6835_CPR;
|
||||
|
||||
abs_raw_angle += d;
|
||||
last_raw_angle = raw_angle;
|
||||
}
|
||||
|
||||
return abs_raw_angle;
|
||||
}
|
||||
|
||||
// Helper SPI transaction helpers for CS handling and SPI transfer
|
||||
void MT6835Encoder::spi_begin_transaction() {
|
||||
// No real beginTransaction in Pico SDK; just pull CS low if used
|
||||
if (cs_pin >= 0)
|
||||
gpio_put(cs_pin, 0);
|
||||
}
|
||||
|
||||
void MT6835Encoder::spi_transfer(uint8_t* data, size_t length) {
|
||||
// Full-duplex transfer, sending and receiving on SPI
|
||||
spi_write_read_blocking(spi, data, data, length);
|
||||
}
|
||||
|
||||
void MT6835Encoder::spi_end_transaction() {
|
||||
if (cs_pin >= 0)
|
||||
gpio_put(cs_pin, 1);
|
||||
}
|
||||
|
||||
uint8_t MT6835Encoder::calc_crc(uint32_t angle, uint8_t status) {
|
||||
uint8_t crc = 0x00;
|
||||
uint8_t input;
|
||||
|
||||
input = angle >> 13;
|
||||
crc ^= input;
|
||||
for (int k = 8; k > 0; k--)
|
||||
crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
|
||||
|
||||
input = (angle >> 5) & 0xFF;
|
||||
crc ^= input;
|
||||
for (int k = 8; k > 0; k--)
|
||||
crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
|
||||
|
||||
input = ((angle << 3) & 0xFF) | (status & 0x07);
|
||||
crc ^= input;
|
||||
for (int k = 8; k > 0; k--)
|
||||
crc = (crc & 0x80) ? (crc << 1) ^ 0x07 : crc << 1;
|
||||
|
||||
return crc;
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue