Version v1.0.1:
* Improved homing (parallel homing support, better repeatability, better geometric reference point) * Improved joint calibration procedure * Calibration data can now be stored persistently on the flash memory (no repeated calibration required) * Improved logging * added PythonAPI to control device easily New G-Code commands: * Enable/Disable motors command, including pose recovery from current position on motor enable * Dedicated joint calibration command with save to flash option * Set pose command to directly set a target pose for the servo loops, bypassing the motion controller (good for real-time control)
This commit is contained in:
0x23
parent
2cf353e7fc
commit
d9888ef369
27 changed files with 1723 additions and 784 deletions
|
|
@ -3,83 +3,92 @@
|
|||
// being distributed under the MIT liscence as well. Thank you SimpleFOC !
|
||||
// --------------------------------------------------------------------------------------
|
||||
|
||||
|
||||
#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);
|
||||
// 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
|
||||
// nop
|
||||
}
|
||||
|
||||
MT6835Encoder::~MT6835Encoder() {
|
||||
// nop
|
||||
// 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
|
||||
}
|
||||
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);
|
||||
set_rotation_direction(0); // needs to be set, otherwise might be random
|
||||
set_bandwidth(bandwidth);
|
||||
set_hysteresis(hysteresis);
|
||||
|
||||
last_raw_angle = 0;
|
||||
abs_raw_angle = 0;
|
||||
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;
|
||||
}
|
||||
|
||||
void MT6835Encoder::reset_abs_angle_period() {
|
||||
abs_raw_angle %= MT6835_CPR;
|
||||
if (abs_raw_angle < 0)
|
||||
abs_raw_angle += MT6835_CPR;
|
||||
}
|
||||
|
||||
float MT6835Encoder::read_abs_angle() {
|
||||
int32_t raw_angle = read_abs_angle_raw();
|
||||
return raw_angle * RAW_TO_ANGLE;
|
||||
int32_t raw_angle = read_abs_angle_raw();
|
||||
return raw_angle * RAW_TO_ANGLE;
|
||||
}
|
||||
|
||||
MT6835Encoder::AbsRawAngleType MT6835Encoder::read_abs_angle_raw() {
|
||||
uint8_t data[6] = {0};
|
||||
data[0] = MT6835_OP_ANGLE << 4;
|
||||
data[1] = MT6835_REG_ANGLE1;
|
||||
// rest zero
|
||||
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();
|
||||
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
|
||||
}
|
||||
}
|
||||
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);
|
||||
return update_abs_raw_angle(raw_angle);
|
||||
}
|
||||
|
||||
MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() {
|
||||
MT6835Encoder::AbsRawAngleType MT6835Encoder::get_last_abs_raw_angle() const {
|
||||
return abs_raw_angle;
|
||||
}
|
||||
|
||||
float MT6835Encoder::get_last_abs_angle() const {
|
||||
return abs_raw_angle * RAW_TO_ANGLE;
|
||||
}
|
||||
|
||||
int32_t MT6835Encoder::get_rawcounts_per_rev() {
|
||||
return MT6835_CPR;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -147,10 +147,11 @@ class 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
|
||||
|
||||
void reset_abs_angle_period(); // Brings abs angle into [0..2pi)
|
||||
float read_abs_angle(); // returns the absolute angle in radians
|
||||
float get_last_abs_angle() const; // returns the last read abs angle
|
||||
AbsRawAngleType read_abs_angle_raw(); // returns the absolute angle in raw counts
|
||||
AbsRawAngleType get_last_abs_raw_angle(); // returns the last read abs raw angle
|
||||
AbsRawAngleType get_last_abs_raw_angle() const; // returns the last read abs raw angle
|
||||
|
||||
int32_t get_rawcounts_per_rev(); // returns the number of raw counts per revolution
|
||||
|
||||
|
|
|
|||
|
|
@ -6,12 +6,14 @@
|
|||
// --------------------------------------------------------------------------------------
|
||||
|
||||
#include "TB6612_motor_driver.h"
|
||||
#include "utilities/logging.h"
|
||||
|
||||
#include <math.h>
|
||||
#include <algorithm>
|
||||
#include "hardware/pwm.h"
|
||||
#include "hardware/gpio.h"
|
||||
#include "hardware/clocks.h"
|
||||
#include "pico/time.h"
|
||||
|
||||
// Helper rounding function
|
||||
int32_t round_int32(float val) {
|
||||
|
|
@ -33,7 +35,7 @@ TB6612MotorDriver::TB6612MotorDriver(
|
|||
pwm_freq(pwm_freq), pwm_resolution(pwm_resolution)
|
||||
{
|
||||
max_pwm = (1 << pwm_resolution) - 1;
|
||||
amplitude = 0.1f * max_pwm; // Default to 10% amplitude
|
||||
set_amplitude(0.0f, false);
|
||||
}
|
||||
|
||||
void init_output_pin(uint8_t pin, bool value) {
|
||||
|
|
@ -46,17 +48,11 @@ 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
|
||||
// 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);
|
||||
|
|
@ -76,6 +72,8 @@ void TB6612MotorDriver::begin() {
|
|||
setup_pwm_pin(pin_neg_a);
|
||||
setup_pwm_pin(pin_pos_b);
|
||||
setup_pwm_pin(pin_neg_b);
|
||||
|
||||
disable();
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::enable() {
|
||||
|
|
@ -90,18 +88,63 @@ void TB6612MotorDriver::disable() {
|
|||
|
||||
void TB6612MotorDriver::set_amplitude(float amplitude, bool immediate_update) {
|
||||
amplitude = std::clamp(amplitude, 0.0f, 1.0f);
|
||||
TB6612MotorDriver::amplitude = amplitude * max_pwm;
|
||||
TB6612MotorDriver::amplitude = amplitude;
|
||||
TB6612MotorDriver::amplitude_raw = amplitude * max_pwm;
|
||||
if(immediate_update)
|
||||
set_field_angle(field_angle);
|
||||
}
|
||||
|
||||
|
||||
void TB6612MotorDriver::set_amplitude_smooth(float amplitude, int ramp_time_ms) {
|
||||
amplitude = std::clamp(amplitude, 0.0f, 1.0f);
|
||||
float start = TB6612MotorDriver::amplitude;
|
||||
int step_time = 10;
|
||||
int steps = std::max(1, ramp_time_ms / step_time);
|
||||
|
||||
for (int i = 1; i <= steps; ++i) {
|
||||
float t = float(i) / steps;
|
||||
set_amplitude(start + t * (amplitude - start), true);
|
||||
sleep_ms(step_time);
|
||||
}
|
||||
}
|
||||
|
||||
float TB6612MotorDriver::get_amplitude() const {
|
||||
return amplitude;
|
||||
}
|
||||
|
||||
void TB6612MotorDriver::rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step) {
|
||||
float start = field_angle;
|
||||
float rad_per_µs = (delta_angle >= 0.0f) ? rad_per_s*1e-6f : -rad_per_s*1e-6f;
|
||||
|
||||
// Determine how long the movement should take (in microseconds)
|
||||
uint64_t duration_us = (uint64_t)(fabs(delta_angle / rad_per_s) * 1e6f);
|
||||
uint64_t start_time_us = time_us_64();
|
||||
while (true) {
|
||||
uint64_t elapsed_us = time_us_64() - start_time_us;
|
||||
if (elapsed_us >= duration_us)
|
||||
break;
|
||||
|
||||
// Update the field
|
||||
set_field_angle(start + rad_per_µs * elapsed_us);
|
||||
|
||||
if(on_step)
|
||||
on_step();
|
||||
|
||||
sleep_us(500);
|
||||
}
|
||||
|
||||
// Ensure final angle is reached
|
||||
set_field_angle(start + delta_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));
|
||||
set_pwm(pin_pos_a, pin_neg_a, round_int32(sin_a * amplitude_raw));
|
||||
set_pwm(pin_pos_b, pin_neg_b, round_int32(cos_a * amplitude_raw));
|
||||
}
|
||||
|
||||
float TB6612MotorDriver::get_field_angle() {
|
||||
|
|
|
|||
|
|
@ -8,6 +8,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <stdint.h>
|
||||
#include <functional>
|
||||
// #include <Arduino.h>
|
||||
|
||||
class TB6612MotorDriver {
|
||||
|
|
@ -27,6 +28,12 @@ class TB6612MotorDriver {
|
|||
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
|
||||
void set_amplitude_smooth(float amplitude, int ramp_time_ms);
|
||||
float get_amplitude() const;
|
||||
|
||||
// Rotate the magnetic field by the given angle delta.
|
||||
// For longer moves, use on_step callback to update encoders.
|
||||
void rotate_field(float delta_angle, float rad_per_s, const std::function<void()>& on_step);
|
||||
|
||||
private:
|
||||
void set_pwm(uint8_t ch_pos, uint8_t ch_neg, int32_t value);
|
||||
|
|
@ -39,6 +46,8 @@ class TB6612MotorDriver {
|
|||
uint16_t pwm_freq;
|
||||
uint8_t pwm_resolution;
|
||||
uint16_t max_pwm;
|
||||
float amplitude; // scaled to 0–max_pwm
|
||||
float amplitude_raw=0.0f; // scaled to 0–max_pwm
|
||||
float amplitude=0.0f; // scaled to 0–1
|
||||
float field_angle;
|
||||
};
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue