fafo/OpenMicroManipulator
17
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

added files

Browse source
This commit is contained in:
0x23 2025-08-28 11:52:28 +02:00
parent 36090af63b
commit 4cd8e6e20e
86 changed files with 74246 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

79
firmware/MotionControllerRP/src/hardware/MT6701_encoder.cpp Normal file
View file

@ -0,0 +1,79 @@
#include "MT6701_encoder.h"
MT6701Encoder::MT6701Encoder(TwoWire& wire, uint8_t i2c_addr)
: wire(wire), address(i2c_addr) {
}
void MT6701Encoder::init() {
wire.begin();
last_raw_angle = 0;
abs_raw_angle = 0;
}
int32_t MT6701Encoder::read_abs_angle_raw() {
wire.beginTransmission(address);
wire.write(0x03); // ANGLE_H register
wire.endTransmission(false);
wire.requestFrom(address, (uint8_t)2);
if (wire.available() < 2) return -1;
uint8_t angle_h = wire.read();
uint8_t angle_l = wire.read();
int32_t raw_angle = (angle_h << 6) | (angle_l >> 2);
return update_abs_raw_angle(raw_angle);
}
float MT6701Encoder::read_abs_angle() {
int32_t raw = read_abs_angle_raw();
return float(raw) * RAW_TO_RAD;
}
void MT6701Encoder::set_hysteresis(uint8_t hyst) {
if (hyst > 7) return;
uint8_t hyst2 = (hyst >> 2) & 0x01;
uint8_t hyst10 = hyst & 0x03;
// --- Register 0x32 ---
wire.beginTransmission(address);
wire.write(0x32);
wire.endTransmission(false);
wire.requestFrom(address, (uint8_t)1);
uint8_t reg32 = wire.read();
reg32 = (reg32 & 0x7F) | (hyst2 << 7);
wire.beginTransmission(address);
wire.write(0x32);
wire.write(reg32);
wire.endTransmission();
// --- Register 0x34 ---
wire.beginTransmission(address);
wire.write(0x34);
wire.endTransmission(false);
wire.requestFrom(address, (uint8_t)1);
uint8_t reg34 = wire.read();
reg34 = (reg34 & 0x3F) | (hyst10 << 6);
wire.beginTransmission(address);
wire.write(0x34);
wire.write(reg34);
wire.endTransmission();
}
MT6701Encoder::AbsRawAngleType MT6701Encoder::update_abs_raw_angle(int32_t raw_angle) {
if (raw_angle >= 0) {
int32_t half_max = CPR >> 1;
int d = raw_angle - last_raw_angle;
if (d > half_max) d -= CPR;
else if (d < -half_max) d += CPR;
abs_raw_angle += d;
last_raw_angle = raw_angle;
}
return abs_raw_angle;
}

27
firmware/MotionControllerRP/src/hardware/MT6701_encoder.h Normal file
View file

@ -0,0 +1,27 @@
#pragma once
#include <Wire.h>
class MT6701Encoder {
public:
using AbsRawAngleType = int32_t;
MT6701Encoder(TwoWire& wire, uint8_t i2c_addr=0x06);
void init();
int32_t read_abs_angle_raw();
float read_abs_angle();
void set_hysteresis(uint8_t hyst); // 0–7
AbsRawAngleType update_abs_raw_angle(int32_t raw_angle);
private:
TwoWire& wire;
uint8_t address;
AbsRawAngleType abs_raw_angle = 0;
int32_t last_raw_angle = -1;
static constexpr int CPR = 16384; // 14-bit resolution
static constexpr float RAW_TO_RAD = 2*PI / CPR;
};

312
firmware/MotionControllerRP/src/hardware/MT6835_encoder.cpp Normal file
View 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;
}

212
firmware/MotionControllerRP/src/hardware/MT6835_encoder.h Normal file
View file

@ -0,0 +1,212 @@
#pragma once
#include <stdint.h>
#include "hardware/spi.h"
#include "hardware/gpio.h"
#define MT6835_CPR (1<<21)
#define MT6835_OP_READ 0b0011
#define MT6835_OP_WRITE 0b0110
#define MT6835_OP_PROG 0b1100
#define MT6835_OP_ZERO 0b0101
#define MT6835_OP_ANGLE 0b1010
#define MT6835_CMD_MASK 0b111100000000000000000000
#define MT6835_ADDR_MASK 0b000011111111111100000000
#define MT6835_DATA_MASK 0b000000000000000011111111
#define MT6835_STATUS_OVERSPEED 0x01
#define MT6835_STATUS_WEAKFIELD 0x02
#define MT6835_STATUS_UNDERVOLT 0x04
#define MT6835_CRC_ERROR 0x08
#define MT6835_WRITE_ACK 0x55
#define MT6835_REG_USERID 0x001
#define MT6835_REG_ANGLE1 0x003
#define MT6835_REG_ANGLE2 0x004
#define MT6835_REG_ANGLE3 0x005
#define MT6835_REG_ANGLE4 0x006
#define MT6835_REG_ABZ_RES1 0x007
#define MT6835_REG_ABZ_RES2 0x008
#define MT6835_REG_ZERO1 0x009
#define MT6835_REG_ZERO2 0x00A
#define MT6835_REG_OPTS0 0x00A
#define MT6835_REG_OPTS1 0x00B
#define MT6835_REG_OPTS2 0x00C
#define MT6835_REG_OPTS3 0x00D
#define MT6835_REG_OPTS4 0x00E
#define MT6835_REG_OPTS5 0x011
// NLC table, 192 bytes
#define MT6835_REG_NLC_BASE 0x013
#define MT6835_REG_CAL_STATUS 0x113
//*** DATATYPES **********************************************************************************/
union MT6835ABZRes {
struct {
uint8_t ab_swap:1;
uint8_t abz_off:1;
uint8_t abz_res_low:6;
};
uint8_t reg;
};
union MT6835Options0 {
struct {
uint8_t z_pul_wid:3;
uint8_t z_edge:1;
uint8_t zero_pos_low:4;
};
uint8_t reg;
};
union MT6835Options1 {
struct {
uint8_t uvw_res:4;
uint8_t uvw_off:1;
uint8_t uvw_mux:1;
uint8_t z_phase:2;
};
uint8_t reg;
};
union MT6835Options2 {
struct {
uint8_t pwm_sel:3;
uint8_t pwm_pol:1;
uint8_t pwm_fq:1;
uint8_t nlc_en:1;
uint8_t reserved:2;
};
uint8_t reg;
};
union MT6835Options3 {
struct {
uint8_t hyst:3;
uint8_t rot_dir:1;
uint8_t reserved:4;
};
uint8_t reg;
};
union MT6835Options4 {
struct {
uint8_t reserved:4;
uint8_t autocal_freq:3;
uint8_t gpio_ds:1;
};
uint8_t reg;
};
union MT6835Options5 {
struct {
uint8_t bw:3;
uint8_t reserved:5;
};
uint8_t reg;
};
union MT6835Command {
struct {
uint32_t unused:8;
uint32_t data:8;
uint32_t addr:12;
uint32_t cmd:4;
};
uint32_t val;
};
//*** CLASS ************************************************************************************/
class MT6835Encoder {
public:
typedef int32_t AbsRawAngleType;
public:
static constexpr float RAW_TO_ANGLE = (2.0f*3.14159265358979323846f)/float(MT6835_CPR);
// use this to setup a HW spi. It can then be used by multiple instances of MT6835
static void setup_spi(spi_inst_t* spi, uint pin_sck, uint pin_mosi, uint pin_miso, int32_t baudrate_hz);
// Constructor: pass SPI instance (spi0 or spi1), CS pin
MT6835Encoder(spi_inst_t *spi, uint cs_pin);
virtual ~MT6835Encoder();
void init(uint8_t bandwidth=0x5, uint8_t hysteresis=0x4);
void reset_abs_angle(int32_t abs_raw_angle=0); // resets the total revolutions of abs angle
float read_abs_angle(); // returns the absolute angle in radians
AbsRawAngleType read_abs_angle_raw(); // returns the absolute angle in raw counts
int32_t get_rawcounts_per_rev(); // returns the number of raw counts per revolution
uint8_t get_bandwidth();
void set_bandwidth(uint8_t bw);
uint8_t get_hysteresis();
void set_hysteresis(uint8_t hyst);
uint8_t get_rotation_direction();
void set_rotation_direction(uint8_t dir);
uint16_t get_abz_resolution();
void set_abz_resolution(uint16_t res);
bool is_abz_enabled();
void set_abz_enabled(bool enabled);
bool is_ab_swapped();
void set_ab_swapped(bool swapped);
uint16_t get_zero_position();
void set_zero_position(uint16_t pos);
MT6835Options1 get_options1();
void set_options1(MT6835Options1 opts);
MT6835Options2 get_options2();
void set_options2(MT6835Options2 opts);
MT6835Options3 get_options3();
void set_options3(MT6835Options3 opts);
MT6835Options4 get_options4();
void set_options4(MT6835Options4 opts);
uint8_t get_status();
uint8_t get_calibration_status();
bool set_zero_from_current_position();
bool write_eeprom(); // takes ~6s to complete after calling
bool check_crc = false;
private:
spi_inst_t *spi;
uint cs_pin;
uint8_t last_status = 0;
uint8_t last_crc = 0;
int32_t last_raw_angle = 0;
AbsRawAngleType abs_raw_angle = 0;
AbsRawAngleType update_abs_raw_angle(int32_t raw_angle);
void spi_begin_transaction();
void spi_transfer(uint8_t* data, size_t length);
void spi_end_transaction();
void transfer_24(MT6835Command *out_value);
uint8_t read_register(uint16_t reg);
bool write_register(uint16_t reg, uint8_t value);
uint8_t calc_crc(uint32_t angle, uint8_t status);
};

114
firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.cpp Normal file
View file

@ -0,0 +1,114 @@
#include "TB6612_motor_driver.h"
#include <math.h>
#include <algorithm>
#include "hardware/pwm.h"
#include "hardware/gpio.h"
#include "hardware/clocks.h"
// Helper rounding function
int32_t round_int32(float val) {
return (val >= 0.0f) ? (int32_t)(val + 0.5f) : (int32_t)(val - 0.5f);
}
TB6612MotorDriver::TB6612MotorDriver(
uint8_t pin_en_a, uint8_t pin_pos_a, uint8_t pin_neg_a, uint8_t pin_pwm_a,
uint8_t pin_en_b, uint8_t pin_pos_b, uint8_t pin_neg_b, uint8_t pin_pwm_b,
uint8_t ch_pos_a, uint8_t ch_neg_a,
uint8_t ch_pos_b, uint8_t ch_neg_b,
uint16_t pwm_freq,
uint8_t pwm_resolution
)
: pin_en_a(pin_en_a), pin_pos_a(pin_pos_a), pin_neg_a(pin_neg_a), pin_pwm_a(pin_pwm_a),
pin_en_b(pin_en_b), pin_pos_b(pin_pos_b), pin_neg_b(pin_neg_b), pin_pwm_b(pin_pwm_b),
ch_pos_a(ch_pos_a), ch_neg_a(ch_neg_a),
ch_pos_b(ch_pos_b), ch_neg_b(ch_neg_b),
pwm_freq(pwm_freq), pwm_resolution(pwm_resolution)
{
max_pwm = (1 << pwm_resolution) - 1;
amplitude = 0.1f * max_pwm; // Default to 10% amplitude
}
void init_output_pin(uint8_t pin, bool value) {
gpio_init(pin);
gpio_set_dir(pin, GPIO_OUT);
gpio_put(pin, value);
}
void TB6612MotorDriver::begin() {
init_output_pin(pin_en_a, false);
init_output_pin(pin_en_b, false);
// enable pwm pins, see TB6612 documentation for how the PWM pins work,
// for slow decay mode they are constantly enabled
init_output_pin(pin_pwm_a, true);
init_output_pin(pin_pwm_b, true);
// not needed since pwm pins are configured below
// pinMode(pin_pos_a, OUTPUT);
// pinMode(pin_neg_a, OUTPUT);
// pinMode(pin_pos_b, OUTPUT);
// pinMode(pin_neg_b, OUTPUT);
// Read system clock dynamically
uint32_t sys_clk = clock_get_hz(clk_sys);
float clkdiv = (float)sys_clk / (pwm_freq * max_pwm);
// Setup helper
auto setup_pwm_pin = [&](uint8_t pin) {
gpio_set_function(pin, GPIO_FUNC_PWM);
uint slice = pwm_gpio_to_slice_num(pin);
uint chan = pwm_gpio_to_channel(pin); // 0 for A, 1 for B
pwm_config config = pwm_get_default_config();
pwm_config_set_clkdiv(&config, clkdiv);
pwm_config_set_wrap(&config, max_pwm);
pwm_init(slice, &config, true);
};
setup_pwm_pin(pin_pos_a);
setup_pwm_pin(pin_neg_a);
setup_pwm_pin(pin_pos_b);
setup_pwm_pin(pin_neg_b);
}
void TB6612MotorDriver::enable() {
gpio_put(pin_en_a, 1);
gpio_put(pin_en_b, 1);
}
void TB6612MotorDriver::disable() {
gpio_put(pin_en_a, 0);
gpio_put(pin_en_b, 0);
}
void TB6612MotorDriver::set_amplitude(float amplitude, bool immediate_update) {
amplitude = std::clamp(amplitude, 0.0f, 1.0f);
TB6612MotorDriver::amplitude = amplitude * max_pwm;
if(immediate_update)
set_field_angle(field_angle);
}
void TB6612MotorDriver::set_field_angle(float angle_rad) {
field_angle = angle_rad;
float sin_a = sin(angle_rad);
float cos_a = cos(angle_rad);
set_pwm(pin_pos_a, pin_neg_a, round_int32(sin_a * amplitude));
set_pwm(pin_pos_b, pin_neg_b, round_int32(cos_a * amplitude));
}
float TB6612MotorDriver::get_field_angle() {
return field_angle;
}
void TB6612MotorDriver::set_pwm(uint8_t pin_pos, uint8_t pin_neg, int32_t value) {
value = std::min(std::max(value, -(int32_t)max_pwm), (int32_t)max_pwm);
if (value >= 0) {
pwm_set_gpio_level(pin_pos, max_pwm - value);
pwm_set_gpio_level(pin_neg, max_pwm);
} else {
pwm_set_gpio_level(pin_pos, max_pwm);
pwm_set_gpio_level(pin_neg, max_pwm + value);
}
}

37
firmware/MotionControllerRP/src/hardware/TB6612_motor_driver.h Normal file
View file

@ -0,0 +1,37 @@
#pragma once
#include <stdint.h>
// #include <Arduino.h>
class TB6612MotorDriver {
public:
TB6612MotorDriver(
uint8_t pin_en_a, uint8_t pin_pos_a, uint8_t pin_neg_a, uint8_t pin_pwm_a,
uint8_t pin_en_b, uint8_t pin_pos_b, uint8_t pin_neg_b, uint8_t pin_pwm_b,
uint8_t ch_pos_a=0, uint8_t ch_neg_a=1,
uint8_t ch_pos_b=2, uint8_t ch_neg_b=3,
uint16_t pwm_freq = 20000, // might not be hit exactly and may be rounded to nearby freq.
uint8_t pwm_resolution = 12
);
void begin();
void enable();
void disable();
void set_field_angle(float angle_rad);
float get_field_angle();
void set_amplitude(float amplitude, bool immediate_update); // Input in range 0.0–1.0
private:
void set_pwm(uint8_t ch_pos, uint8_t ch_neg, int32_t value);
uint8_t pin_en_a, pin_pos_a, pin_neg_a, pin_pwm_a;
uint8_t pin_en_b, pin_pos_b, pin_neg_b, pin_pwm_b;
uint8_t ch_pos_a, ch_neg_a;
uint8_t ch_pos_b, ch_neg_b;
uint16_t pwm_freq;
uint8_t pwm_resolution;
uint16_t max_pwm;
float amplitude; // scaled to 0–max_pwm
float field_angle;
};