Firmware cleanup

This commit is contained in:
Scott Bezek 2022-10-04 22:21:51 -07:00
parent 3bee19df93
commit b286cfabe1
4 changed files with 209 additions and 269 deletions

View File

@ -29,13 +29,8 @@ build_flags =
[env:view]
extends = base_config
; platform = https://github.com/platformio/platform-espressif32.git#feature/arduino-upstream
; platform_packages =
; framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32#master
board = esp32doit-devkit-v1
lib_deps =
; askuric/Simple FOC @ 2.2.1
; bxparks/AceButton @ 1.9.1
${base_config.lib_deps}
bodmer/TFT_eSPI@2.4.25
fastled/FastLED @ 3.5.0
@ -103,54 +98,3 @@ build_flags =
; GPIO >= 34 are input only
; (SOC_GPIO_VALID_GPIO_MASK & ~(0ULL | _FL_BIT(34) | _FL_BIT(35) | _FL_BIT(36) | _FL_BIT(37) | _FL_BIT(38) | _FL_BIT(39)))
-DSOC_GPIO_VALID_OUTPUT_GPIO_MASK=0x30EFFFFFF
[env:handheld_tdisplay]
extends = base_config
board = esp32doit-devkit-v1
lib_deps =
${base_config.lib_deps}
bodmer/TFT_eSPI@2.4.25
build_flags =
${base_config.build_flags}
-DSK_DISPLAY=1
-DSK_STRAIN=0
-DSK_LEDS=0
-DPIN_UH=17
-DPIN_UL=2
-DPIN_VH=13
-DPIN_VL=32
-DPIN_WH=33
-DPIN_WL=25
-DPIN_BUTTON_NEXT=35
-DPIN_BUTTON_PREV=0
-DPIN_SDA=-1
-DPIN_SCL=-1
-DSENSOR_MT6701=1
-DPIN_MT_DATA=21
-DPIN_MT_CLOCK=22
-DPIN_MT_CSN=12
-DPIN_LCD_BACKLIGHT=4
-DDESCRIPTION_FONT=FreeSans9pt7b
-DDESCRIPTION_Y_OFFSET=80
-DVALUE_OFFSET=0
-DDRAW_ARC=1
-DUSER_SETUP_LOADED=1
-DST7789_DRIVER=1
-DCGRAM_OFFSET=1
-DTFT_WIDTH=135
-DTFT_HEIGHT=240
-DTFT_MISO=-1
-DTFT_MOSI=19
-DTFT_SCLK=18
-DTFT_CS=5
-DTFT_DC=16
-DTFT_RST=23
-DTFT_BL=-1
-DLOAD_GLCD=1
-DLOAD_GFXFF=1
-DSPI_FREQUENCY=40000000

View File

@ -37,7 +37,7 @@ void setup() {
motor_task.addListener(knob_state_debug_queue);
// Free up the loop task
// Free up the Arduino loop task
vTaskDelete(NULL);
}

View File

@ -1,11 +1,24 @@
#include <SimpleFOC.h>
#include <sensors/MagneticSensorI2C.h>
#include "motor_task.h"
#if SENSOR_MT6701
#include "mt6701_sensor.h"
#endif
#if SENSOR_TLV
#include "tlv_sensor.h"
#endif
#include "util.h"
// ####
// Hardware-specific motor calibration constants.
// Run calibration once at startup, then update these constants with the calibration results.
static const float ZERO_ELECTRICAL_OFFSET = 2.77;
static const Direction FOC_DIRECTION = Direction::CW;
static const int MOTOR_POLE_PAIRS = 7;
// ####
static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
static const float DEAD_ZONE_RAD = 1 * _PI / 180;
@ -24,38 +37,16 @@ MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 2048, 1, task_core
MotorTask::~MotorTask() {}
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(1);
BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
#if SENSOR_TLV
TlvSensor encoder = TlvSensor();
#elif SENSOR_MT6701
MT6701Sensor encoder = MT6701Sensor();
#endif
// MagneticSensorI2C tlv = MagneticSensorI2C(AS5600_I2C);
Commander command = Commander(Serial);
void doMotor(char* cmd) { command.motor(&motor, cmd); }
void MotorTask::run() {
// Hardware-specific configuration:
// TODO: make this easier to configure
// Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
// SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
// it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
// So this value is based on experimentation.
// TODO: dig into SimpleFOC calibration and find/fix the issue
// float zero_electric_offset = -0.6; // original proto
//float zero_electric_offset = 0.4; // handheld 1
// float zero_electric_offset = -0.8; // handheld 2
// float zero_electric_offset = 2.93; //0.15; // 17mm test
// float zero_electric_offset = 0.66; // 15mm handheld
float zero_electric_offset = 7.34;
Direction foc_direction = Direction::CW;
motor.pole_pairs = 7;
driver.voltage_power_supply = 5;
driver.init();
@ -66,9 +57,7 @@ void MotorTask::run() {
#if SENSOR_MT6701
encoder.init();
// motor.LPF_angle = LowPassFilter(0.05);
#endif
// motor.LPF_current_q = {0.01};
motor.linkDriver(&driver);
@ -77,23 +66,21 @@ void MotorTask::run() {
motor.velocity_limit = 10000;
motor.linkSensor(&encoder);
// Not actually using the velocity loop; but I'm using those PID variables
// because SimpleFOC studio supports updating them easily over serial for tuning.
// Not actually using the velocity loop built into SimpleFOC; but I'm using those PID variables
// to run PID for torque (and SimpleFOC studio supports updating them easily over serial for tuning)
motor.PID_velocity.P = 4;
motor.PID_velocity.I = 0;
motor.PID_velocity.D = 0.04;
motor.PID_velocity.output_ramp = 10000;
motor.PID_velocity.limit = 10;
// motor.useMonitoring(Serial);
motor.init();
encoder.update();
delay(10);
motor.initFOC(zero_electric_offset, foc_direction);
motor.pole_pairs = MOTOR_POLE_PAIRS;
motor.initFOC(ZERO_ELECTRICAL_OFFSET, FOC_DIRECTION);
bool calibrate = false;
@ -107,13 +94,198 @@ void MotorTask::run() {
delay(10);
}
if (calibrate) {
this->calibrate();
}
Serial.println(motor.zero_electric_angle);
motor.monitor_downsample = 0; // disable monitor at first - optional
// disableCore0WDT();
float current_detent_center = motor.shaft_angle;
KnobConfig config = {
.num_positions = 2,
.position = 0,
.position_width_radians = 60 * _PI / 180,
.detent_strength_unit = 0,
};
float idle_check_velocity_ewma = 0;
uint32_t last_idle_start = 0;
uint32_t last_publish = 0;
while (1) {
motor.loopFOC();
// Check queue for pending requests from other tasks
Command command;
if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
switch (command.command_type) {
case CommandType::CONFIG: {
// Change haptic input mode
config = command.data.config;
Serial.println("Got new config");
current_detent_center = motor.shaft_angle;
#if SK_INVERT_ROTATION
current_detent_center = -motor.shaft_angle;
#endif
// Update derivative factor of torque controller based on detent width.
// If the D factor is large on coarse detents, the motor ends up making noise because the P&D factors amplify the noise from the sensor.
// This is a piecewise linear function so that fine detents (small width) get a higher D factor and coarse detents get a small D factor.
// Fine detents need a nonzero D factor to artificially create "clicks" each time a new value is reached (the P factor is small
// for fine detents due to the smaller angular errors, and the existing P factor doesn't work well for very small angle changes (easy to
// get runaway due to sensor noise & lag)).
// TODO: consider eliminating this D factor entirely and just "play" a hardcoded haptic "click" (e.g. a quick burst of torque in each
// direction) whenever the position changes when the detent width is too small for the P factor to work well.
const float derivative_lower_strength = config.detent_strength_unit * 0.08;
const float derivative_upper_strength = config.detent_strength_unit * 0.02;
const float derivative_position_width_lower = radians(3);
const float derivative_position_width_upper = radians(8);
const float raw = derivative_lower_strength + (derivative_upper_strength - derivative_lower_strength)/(derivative_position_width_upper - derivative_position_width_lower)*(config.position_width_radians - derivative_position_width_lower);
motor.PID_velocity.D = CLAMP(
raw,
min(derivative_lower_strength, derivative_upper_strength),
max(derivative_lower_strength, derivative_upper_strength)
);
break;
}
case CommandType::HAPTIC: {
// Play a hardcoded haptic "click"
float strength = command.data.haptic.press ? 5 : 1.5;
motor.move(strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
}
motor.move(-strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
}
motor.move(0);
motor.loopFOC();
break;
}
}
}
// If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
if (fabsf(idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) {
last_idle_start = 0;
} else {
if (last_idle_start == 0) {
last_idle_start = millis();
}
}
if (last_idle_start > 0 && millis() - last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(motor.shaft_angle - current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) {
current_detent_center = motor.shaft_angle * IDLE_CORRECTION_RATE_ALPHA + current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA);
}
// Check where we are relative to the current nearest detent; update our position if we've moved far enough to snap to another detent
float angle_to_detent_center = motor.shaft_angle - current_detent_center;
#if SK_INVERT_ROTATION
angle_to_detent_center = -motor.shaft_angle - current_detent_center;
#endif
if (angle_to_detent_center > config.position_width_radians * config.snap_point && (config.num_positions <= 0 || config.position > 0)) {
current_detent_center += config.position_width_radians;
angle_to_detent_center -= config.position_width_radians;
config.position--;
} else if (angle_to_detent_center < -config.position_width_radians * config.snap_point && (config.num_positions <= 0 || config.position < config.num_positions - 1)) {
current_detent_center -= config.position_width_radians;
angle_to_detent_center += config.position_width_radians;
config.position++;
}
float dead_zone_adjustment = CLAMP(
angle_to_detent_center,
fmaxf(-config.position_width_radians*DEAD_ZONE_DETENT_PERCENT, -DEAD_ZONE_RAD),
fminf(config.position_width_radians*DEAD_ZONE_DETENT_PERCENT, DEAD_ZONE_RAD));
bool out_of_bounds = config.num_positions > 0 && ((angle_to_detent_center > 0 && config.position == 0) || (angle_to_detent_center < 0 && config.position == config.num_positions - 1));
motor.PID_velocity.limit = 10; //out_of_bounds ? 10 : 3;
motor.PID_velocity.P = out_of_bounds ? config.endstop_strength_unit * 4 : config.detent_strength_unit * 4;
// Apply motor torque based on our angle to the nearest detent (detent strength, etc is handled by the PID_velocity parameters)
if (fabsf(motor.shaft_velocity) > 60) {
// Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
motor.move(0);
} else {
float torque = motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment);
#if SK_INVERT_ROTATION
torque = -torque;
#endif
motor.move(torque);
}
// Publish current status to other registered tasks periodically
if (millis() - last_publish > 10) {
publish({
.current_position = config.position,
.sub_position_unit = -angle_to_detent_center / config.position_width_radians,
.config = config,
});
last_publish = millis();
}
motor.monitor();
delay(1);
}
}
void MotorTask::setConfig(const KnobConfig& config) {
Command command = {
.command_type = CommandType::CONFIG,
.data = {
.config = config,
}
};
xQueueSend(queue_, &command, portMAX_DELAY);
}
void MotorTask::playHaptic(bool press) {
Command command = {
.command_type = CommandType::HAPTIC,
.data = {
.haptic = {
.press = press,
},
}
};
xQueueSend(queue_, &command, portMAX_DELAY);
}
void MotorTask::addListener(QueueHandle_t queue) {
listeners_.push_back(queue);
}
void MotorTask::publish(const KnobState& state) {
for (auto listener : listeners_) {
xQueueOverwrite(listener, &state);
}
}
void MotorTask::calibrate() {
// SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
// it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
// So this value is based on experimentation.
// TODO: dig into SimpleFOC calibration and find/fix the issue
Serial.println("\n\n\nStarting calibration, please do not touch to motor until complete!");
motor.controller = MotionControlType::angle_openloop;
motor.pole_pairs = 1;
motor.initFOC(0, Direction::CW);
float a = 0;
// #### Determine direction motor rotates relative to angle sensor
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
motor.move(a);
@ -136,10 +308,6 @@ void MotorTask::run() {
motor.voltage_limit = 0;
motor.move(a);
// Serial.println("Did motor turn counterclockwise? Press Y to continue, otherwise change motor wiring and restart");
// while (Serial.read() != 'Y') {
// delay(10);
// }
Serial.println();
@ -154,7 +322,9 @@ void MotorTask::run() {
motor.initFOC(0, Direction::CCW);
}
// Rotate many electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
// #### Determine pole-pairs
// Rotate 20 electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
uint8_t electrical_revolutions = 20;
Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
motor.voltage_limit = 5;
@ -166,7 +336,7 @@ void MotorTask::run() {
motor.move(a);
delay(1);
}
Serial.println("pause...");
Serial.println("pause..."); // Let momentum settle...
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
delay(1);
@ -204,7 +374,7 @@ void MotorTask::run() {
delay(1000);
// #### Determine mechanical offset to electrical zero
// Measure mechanical angle at every electrical zero for several revolutions
motor.voltage_limit = 5;
motor.move(a);
@ -257,202 +427,22 @@ void MotorTask::run() {
float avg_offset_angle = atan2f(offset_y, offset_x);
// Apply settings
// #### Apply settings
// TODO: save to non-volatile storage
motor.pole_pairs = measured_pole_pairs;
motor.zero_electric_angle = avg_offset_angle + _3PI_2;
motor.voltage_limit = 5;
motor.controller = MotionControlType::torque;
Serial.print("\n\nRESULTS:\n zero electric angle: ");
Serial.print("\n\nRESULTS:\n Update these constants at the top of " __FILE__ "\n ZERO_ELECTRICAL_OFFSET: ");
Serial.println(motor.zero_electric_angle);
Serial.print(" direction: ");
Serial.print(" FOC_DIRECTION: ");
if (motor.sensor_direction == Direction::CW) {
Serial.println("CW");
Serial.println("Direction::CW");
} else {
Serial.println("CCW");
Serial.println("Direction::CCW");
}
Serial.printf(" pole pairs: %d\n", motor.pole_pairs);
Serial.printf(" MOTOR_POLE_PAIRS: %d\n", motor.pole_pairs);
delay(2000);
}
Serial.println(motor.zero_electric_angle);
command.add('M', &doMotor, "foo");
// command.add('D', &doDetents, "Detents");
motor.monitor_downsample = 0; // disable monitor at first - optional
// disableCore0WDT();
float current_detent_center = motor.shaft_angle;
KnobConfig config = {
.num_positions = 2,
.position = 0,
.position_width_radians = 60 * _PI / 180,
.detent_strength_unit = 0,
};
float idle_check_velocity_ewma = 0;
uint32_t last_idle_start = 0;
uint32_t last_debug = 0;
uint32_t last_publish = 0;
while (1) {
motor.loopFOC();
Command command;
if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
switch (command.command_type) {
case CommandType::CONFIG: {
config = command.data.config;
Serial.println("Got new config");
current_detent_center = motor.shaft_angle;
#if SK_INVERT_ROTATION
current_detent_center = -motor.shaft_angle;
#endif
// Update derivative factor of torque controller based on detent width.
// If the D factor is large on coarse detents, the motor ends up making noise because the P&D factors amplify the noise from the sensor.
// This is a piecewise linear function so that fine detents (small width) get a higher D factor and coarse detents get a small D factor.
// Fine detents need a nonzero D factor to artificially create "clicks" each time a new value is reached (the P factor is small
// for fine detents due to the smaller angular errors, and the existing P factor doesn't work well for very small angle changes (easy to
// get runaway due to sensor noise & lag)).
// TODO: consider eliminating this D factor entirely and just "play" a hardcoded haptic "click" (e.g. a quick burst of torque in each
// direction) whenever the position changes when the detent width is too small for the P factor to work well.
const float derivative_lower_strength = config.detent_strength_unit * 0.08;
const float derivative_upper_strength = config.detent_strength_unit * 0.02;
const float derivative_position_width_lower = radians(3);
const float derivative_position_width_upper = radians(8);
const float raw = derivative_lower_strength + (derivative_upper_strength - derivative_lower_strength)/(derivative_position_width_upper - derivative_position_width_lower)*(config.position_width_radians - derivative_position_width_lower);
motor.PID_velocity.D = CLAMP(
raw,
min(derivative_lower_strength, derivative_upper_strength),
max(derivative_lower_strength, derivative_upper_strength)
);
break;
}
case CommandType::HAPTIC: {
float strength = command.data.haptic.press ? 5 : 1.5;
motor.move(strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
}
motor.move(-strength);
for (uint8_t i = 0; i < 3; i++) {
motor.loopFOC();
delay(1);
}
motor.move(0);
motor.loopFOC();
break;
}
}
}
idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
if (fabsf(idle_check_velocity_ewma) > IDLE_VELOCITY_RAD_PER_SEC) {
last_idle_start = 0;
} else {
if (last_idle_start == 0) {
last_idle_start = millis();
}
}
// If we are not moving and we're close to the center (but not exactly there), slowly adjust the centerpoint to match the current position
if (last_idle_start > 0 && millis() - last_idle_start > IDLE_CORRECTION_DELAY_MILLIS && fabsf(motor.shaft_angle - current_detent_center) < IDLE_CORRECTION_MAX_ANGLE_RAD) {
current_detent_center = motor.shaft_angle * IDLE_CORRECTION_RATE_ALPHA + current_detent_center * (1 - IDLE_CORRECTION_RATE_ALPHA);
// if (millis() - last_debug > 100) {
// last_debug = millis();
// Serial.print("Moving detent center. ");
// Serial.print(current_detent_center);
// Serial.print(" ");
// Serial.println(motor.shaft_angle);
// }
}
float angle_to_detent_center = motor.shaft_angle - current_detent_center;
#if SK_INVERT_ROTATION
angle_to_detent_center = -motor.shaft_angle - current_detent_center;
#endif
if (angle_to_detent_center > config.position_width_radians * config.snap_point && (config.num_positions <= 0 || config.position > 0)) {
current_detent_center += config.position_width_radians;
angle_to_detent_center -= config.position_width_radians;
config.position--;
} else if (angle_to_detent_center < -config.position_width_radians * config.snap_point && (config.num_positions <= 0 || config.position < config.num_positions - 1)) {
current_detent_center -= config.position_width_radians;
angle_to_detent_center += config.position_width_radians;
config.position++;
}
float dead_zone_adjustment = CLAMP(
angle_to_detent_center,
fmaxf(-config.position_width_radians*DEAD_ZONE_DETENT_PERCENT, -DEAD_ZONE_RAD),
fminf(config.position_width_radians*DEAD_ZONE_DETENT_PERCENT, DEAD_ZONE_RAD));
bool out_of_bounds = config.num_positions > 0 && ((angle_to_detent_center > 0 && config.position == 0) || (angle_to_detent_center < 0 && config.position == config.num_positions - 1));
motor.PID_velocity.limit = 10; //out_of_bounds ? 10 : 3;
motor.PID_velocity.P = out_of_bounds ? config.endstop_strength_unit * 4 : config.detent_strength_unit * 4;
if (fabsf(motor.shaft_velocity) > 60) {
// Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
motor.move(0);
} else {
float torque = motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment);
#if SK_INVERT_ROTATION
torque = -torque;
#endif
motor.move(torque);
}
if (millis() - last_publish > 10) {
publish({
.current_position = config.position,
.sub_position_unit = -angle_to_detent_center / config.position_width_radians,
.config = config,
});
last_publish = millis();
}
motor.monitor();
// command.run();
delay(1);
}
}
void MotorTask::setConfig(const KnobConfig& config) {
Command command = {
.command_type = CommandType::CONFIG,
.data = {
.config = config,
}
};
xQueueSend(queue_, &command, portMAX_DELAY);
}
void MotorTask::playHaptic(bool press) {
Command command = {
.command_type = CommandType::HAPTIC,
.data = {
.haptic = {
.press = press,
},
}
};
xQueueSend(queue_, &command, portMAX_DELAY);
}
void MotorTask::addListener(QueueHandle_t queue) {
listeners_.push_back(queue);
}
void MotorTask::publish(const KnobState& state) {
for (auto listener : listeners_) {
xQueueOverwrite(listener, &state);
}
}

View File

@ -1,6 +1,7 @@
#pragma once
#include <Arduino.h>
#include <SimpleFOC.h>
#include <vector>
#include "knob_data.h"
@ -45,5 +46,10 @@ class MotorTask : public Task<MotorTask> {
std::vector<QueueHandle_t> listeners_;
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(1);
BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
void publish(const KnobState& state);
void calibrate();
};