projects/Smartknob
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Firmware updates - sensors, calibration, View support, etc (#9)

Browse Source 
- Modify TLV493d library to expose frame counter in order to check for lockup, and implement auto-reset in tlv_sensor in case of lockup
 - Implement MT6701 SimpleFOC sensor
 - Make display optional
 - Add optional LED, strain, ALS support
 - Connect ALS to LED and display brightness
 - Hardcoded strain gauge thresholds and haptic feedback
This commit is contained in:
Scott Bezek
2022-03-10 19:05:49 -08:00
committed by GitHub
parent 9e2725f850
commit b47fcf7da4
26 changed files with 2101 additions and 110 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
firmware/src/display_task.cpp
View File

@@ -1,18 +1,20 @@
#if SK_DISPLAY
#include "display_task.h"
#include "semaphore_guard.h"
#include "font/roboto_light_60.h"
DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 2048, 1, task_core} {
semaphore_ = xSemaphoreCreateMutex();
assert(semaphore_ != NULL);
xSemaphoreGive(semaphore_);
DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 4048, 1, task_core} {
knob_state_queue_ = xQueueCreate(1, sizeof(KnobState));
assert(knob_state_queue_ != NULL);
mutex_ = xSemaphoreCreateMutex();
assert(mutex_ != NULL);
}
DisplayTask::~DisplayTask() {
if (semaphore_ != NULL) {
vSemaphoreDelete(semaphore_);
}
vQueueDelete(knob_state_queue_);
vSemaphoreDelete(mutex_);
}
static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_t *b)
@@ -73,16 +75,23 @@ static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_
}
void DisplayTask::run() {
delay(100);
tft_.begin();
tft_.invertDisplay(1);
tft_.setRotation(0);
tft_.fillScreen(TFT_PURPLE);
tft_.fillScreen(TFT_DARKGREEN);
ledcSetup(LEDC_CHANNEL_LCD_BACKLIGHT, 5000, 16);
ledcAttachPin(PIN_LCD_BACKLIGHT, LEDC_CHANNEL_LCD_BACKLIGHT);
ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, UINT16_MAX);
spr_.setColorDepth(16);
if (spr_.createSprite(TFT_WIDTH, TFT_HEIGHT) == nullptr) {
Serial.println("ERROR: sprite allocation failed!");
tft_.fillScreen(TFT_RED);
} else {
Serial.println("Sprite created!");
tft_.fillScreen(TFT_PURPLE);
}
spr_.setTextColor(0xFFFF, TFT_BLACK);
@@ -100,9 +109,8 @@ void DisplayTask::run() {
spr_.setTextDatum(CC_DATUM);
spr_.setTextColor(TFT_WHITE);
while(1) {
{
SemaphoreGuard lock(semaphore_);
state = state_;
if (xQueueReceive(knob_state_queue_, &state, portMAX_DELAY) == pdFALSE) {
continue;
}
spr_.fillSprite(TFT_BLACK);
@@ -112,9 +120,9 @@ void DisplayTask::run() {
}
spr_.setFreeFont(&Roboto_Light_60);
spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - 30, 1);
spr_.setFreeFont(&Roboto_Thin_24);
int32_t line_y = TFT_HEIGHT / 2 + 20;
spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - VALUE_OFFSET, 1);
spr_.setFreeFont(&DESCRIPTION_FONT);
int32_t line_y = TFT_HEIGHT / 2 + DESCRIPTION_Y_OFFSET;
char* start = state.config.descriptor;
char* end = start + strlen(state.config.descriptor);
while (start < end) {
@@ -139,6 +147,9 @@ void DisplayTask::run() {
spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(left_bound), TFT_HEIGHT/2 - RADIUS * sinf(left_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(left_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(left_bound), TFT_WHITE);
spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(right_bound), TFT_HEIGHT/2 - RADIUS * sinf(right_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(right_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(right_bound), TFT_WHITE);
}
if (DRAW_ARC) {
spr_.drawCircle(TFT_WIDTH/2, TFT_HEIGHT/2, RADIUS, TFT_DARKGREY);
}
float adjusted_sub_position = state.sub_position_unit * state.config.position_width_radians;
if (state.config.num_positions > 0) {
@@ -171,11 +182,22 @@ void DisplayTask::run() {
}
spr_.pushSprite(0, 0);
{
SemaphoreGuard lock(mutex_);
ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, brightness_);
}
delay(2);
}
}
void DisplayTask::setData(KnobState state) {
SemaphoreGuard lock(semaphore_);
state_ = state;
QueueHandle_t DisplayTask::getKnobStateQueue() {
return knob_state_queue_;
}
void DisplayTask::setBrightness(uint16_t brightness) {
SemaphoreGuard lock(mutex_);
brightness_ = brightness;
}
#endif

18
firmware/src/display_task.h
View File

@@ -1,5 +1,7 @@
#pragma once
#if SK_DISPLAY
#include <Arduino.h>
#include <TFT_eSPI.h>
@@ -13,7 +15,9 @@ class DisplayTask : public Task<DisplayTask> {
DisplayTask(const uint8_t task_core);
~DisplayTask();
void setData(KnobState state);
QueueHandle_t getKnobStateQueue();
void setBrightness(uint16_t brightness);
protected:
void run();
@@ -24,7 +28,17 @@ class DisplayTask : public Task<DisplayTask> {
/** Full-size sprite used as a framebuffer */
TFT_eSprite spr_ = TFT_eSprite(&tft_);
SemaphoreHandle_t semaphore_;
QueueHandle_t knob_state_queue_;
KnobState state_;
SemaphoreHandle_t mutex_;
uint16_t brightness_;
};
#else
class DisplayTask {};
#endif

188
firmware/src/interface_task.cpp
View File

@@ -1,10 +1,36 @@
#include <AceButton.h>
#if SK_LEDS
#include <FastLED.h>
#endif
#if SK_STRAIN
#include <HX711.h>
#endif
#if SK_ALS
#include <Adafruit_VEML7700.h>
#endif
#include "interface_task.h"
#include "util.h"
using namespace ace_button;
#define COUNT_OF(A) (sizeof(A) / sizeof(A[0]))
#if SK_LEDS
CRGB leds[NUM_LEDS];
#endif
#if SK_STRAIN
HX711 scale;
#endif
#if SK_ALS
Adafruit_VEML7700 veml = Adafruit_VEML7700();
#endif
static KnobConfig configs[] = {
// int32_t num_positions;
// int32_t position;
@@ -81,7 +107,7 @@ static KnobConfig configs[] = {
32,
0,
8.225806452 * PI / 180,
1,
2,
1,
1.1,
"Coarse values\nStrong detents",
@@ -97,25 +123,146 @@ static KnobConfig configs[] = {
},
};
InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task) : Task{"Interface", 2048, 1, task_core}, motor_task_(motor_task) {
InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) : Task("Interface", 4048, 1, task_core), motor_task_(motor_task), display_task_(display_task) {
#if SK_DISPLAY
assert(display_task != nullptr);
#endif
}
InterfaceTask::~InterfaceTask() {}
void InterfaceTask::run() {
AceButton button(36);
pinMode(36, INPUT);
button.getButtonConfig()->setIEventHandler(this);
#if PIN_BUTTON_NEXT >= 34
pinMode(PIN_BUTTON_NEXT, INPUT);
#else
pinMode(PIN_BUTTON_NEXT, INPUT_PULLUP);
#endif
AceButton button_next((uint8_t) PIN_BUTTON_NEXT);
button_next.getButtonConfig()->setIEventHandler(this);
#if PIN_BUTTON_PREV > -1
#if PIN_BUTTON_PREV >= 34
pinMode(PIN_BUTTON_PREV, INPUT);
#else
pinMode(PIN_BUTTON_PREV, INPUT_PULLUP);
#endif
AceButton button_prev((uint8_t) PIN_BUTTON_PREV);
button_prev.getButtonConfig()->setIEventHandler(this);
#endif
#if SK_LEDS
FastLED.addLeds<SK6812, PIN_LED_DATA, GRB>(leds, NUM_LEDS);
#endif
#if PIN_SDA >= 0 && PIN_SCL >= 0
Wire.begin(PIN_SDA, PIN_SCL);
Wire.setClock(400000);
#endif
#if SK_STRAIN
scale.begin(38, 2);
#endif
#if SK_ALS
if (veml.begin()) {
veml.setGain(VEML7700_GAIN_2);
veml.setIntegrationTime(VEML7700_IT_400MS);
} else {
Serial.println("ALS sensor not found!");
}
#endif
motor_task_.setConfig(configs[0]);
// How far button is pressed, in range [0, 1]
float press_value_unit = 0;
// Interface loop:
while (1) {
button.check();
button_next.check();
#if PIN_BUTTON_PREV > -1
button_prev.check();
#endif
if (Serial.available()) {
int v = Serial.read();
if (v == ' ') {
nextConfig();
changeConfig(true);
}
}
#if SK_ALS
const float LUX_ALPHA = 0.005;
static float lux_avg;
float lux = veml.readLux();
lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
static uint32_t last_als;
if (millis() - last_als > 1000) {
Serial.print("millilux: "); Serial.println(lux*1000);
last_als = millis();
}
#endif
#if SK_STRAIN
// TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
const int32_t lower = 950000;
const int32_t upper = 1800000;
if (scale.wait_ready_timeout(100)) {
int32_t reading = scale.read();
// Ignore readings that are way out of expected bounds
if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
static uint32_t last_reading_display;
if (millis() - last_reading_display > 1000) {
Serial.print("HX711 reading: ");
Serial.println(reading);
last_reading_display = millis();
}
long value = CLAMP(reading, lower, upper);
press_value_unit = 1. * (value - lower) / (upper - lower);
static bool pressed;
if (!pressed && press_value_unit > 0.75) {
motor_task_.playHaptic(true);
pressed = true;
changeConfig(true);
} else if (pressed && press_value_unit < 0.25) {
motor_task_.playHaptic(false);
pressed = false;
}
}
} else {
Serial.println("HX711 not found.");
#if SK_LEDS
for (uint8_t i = 0; i < NUM_LEDS; i++) {
leds[i] = CRGB::Red;
}
FastLED.show();
#endif
}
#endif
uint16_t brightness = UINT16_MAX;
// TODO: brightness scale factor should be configurable (depends on reflectivity of surface)
#if SK_ALS
brightness = (uint16_t)CLAMP(lux_avg * 13000, (float)1280, (float)UINT16_MAX);
#endif
#if SK_DISPLAY
display_task_->setBrightness(brightness); // TODO: apply gamma correction
#endif
#if SK_LEDS
for (uint8_t i = 0; i < NUM_LEDS; i++) {
leds[i].setHSV(200 * press_value_unit, 255, brightness >> 8);
// Gamma adjustment
leds[i].r = dim8_video(leds[i].r);
leds[i].g = dim8_video(leds[i].g);
leds[i].b = dim8_video(leds[i].b);
}
FastLED.show();
#endif
delay(10);
}
}
@@ -123,15 +270,34 @@ void InterfaceTask::run() {
void InterfaceTask::handleEvent(AceButton* button, uint8_t event_type, uint8_t button_state) {
switch (event_type) {
case AceButton::kEventPressed:
nextConfig();
if (button->getPin() == PIN_BUTTON_NEXT) {
changeConfig(true);
}
#if PIN_BUTTON_PREV > -1
if (button->getPin() == PIN_BUTTON_PREV) {
changeConfig(false);
}
#endif
break;
case AceButton::kEventReleased:
break;
}
}
void InterfaceTask::nextConfig() {
current_config_ = (current_config_ + 1) % COUNT_OF(configs);
Serial.printf("Changing config to %d:\n%s\n", current_config_, configs[current_config_].descriptor);
void InterfaceTask::changeConfig(bool next) {
if (next) {
current_config_ = (current_config_ + 1) % COUNT_OF(configs);
} else {
if (current_config_ == 0) {
current_config_ = COUNT_OF(configs) - 1;
} else {
current_config_ --;
}
}
Serial.print("Changing config to ");
Serial.print(current_config_);
Serial.print(" -- ");
Serial.println(configs[current_config_].descriptor);
motor_task_.setConfig(configs[current_config_]);
}

7
firmware/src/interface_task.h
View File

@@ -3,6 +3,7 @@
#include <AceButton.h>
#include <Arduino.h>
#include "display_task.h"
#include "motor_task.h"
#include "task.h"
@@ -10,7 +11,7 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
friend class Task<InterfaceTask>; // Allow base Task to invoke protected run()
public:
InterfaceTask(const uint8_t task_core, MotorTask& motor_task);
InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task);
~InterfaceTask();
void handleEvent(ace_button::AceButton* button, uint8_t event_type, uint8_t button_state) override;
@@ -20,7 +21,9 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
private:
MotorTask& motor_task_;
DisplayTask* display_task_;
int current_config_ = 0;
void nextConfig();
void changeConfig(bool next);
};

53
firmware/src/main.cpp
View File

@@ -1,31 +1,66 @@
#include <Arduino.h>
#include <FastLED.h>
#include <SimpleFOC.h>
#include <TFT_eSPI.h>
#include "display_task.h"
#include "interface_task.h"
#include "motor_task.h"
#include "tlv_sensor.h"
DisplayTask display_task = DisplayTask(1);
MotorTask motor_task = MotorTask(0, display_task);
InterfaceTask interface_task = InterfaceTask(1, motor_task);
#if SK_DISPLAY
static DisplayTask display_task = DisplayTask(0);
static DisplayTask* display_task_p = &display_task;
#else
static DisplayTask* display_task_p = nullptr;
#endif
static MotorTask motor_task = MotorTask(1);
CRGB leds[1];
InterfaceTask interface_task = InterfaceTask(0, motor_task, display_task_p);
static QueueHandle_t knob_state_debug_queue;
void setup() {
Serial.begin(115200);
motor_task.begin();
interface_task.begin();
#if SK_DISPLAY
display_task.begin();
vTaskDelete(nullptr);
// Connect display to motor_task's knob state feed
motor_task.addListener(display_task.getKnobStateQueue());
#endif
// Create a queue and register it with motor_task to print knob state to serial (see loop() below)
knob_state_debug_queue = xQueueCreate(1, sizeof(KnobState));
assert(knob_state_debug_queue != NULL);
motor_task.addListener(knob_state_debug_queue);
// Free up the loop task
vTaskDelete(NULL);
}
static KnobState state = {};
uint32_t last_debug;
void loop() {
assert(false);
// Print any new state, at most 5 times per second
if (millis() - last_debug > 200 && xQueueReceive(knob_state_debug_queue, &state, portMAX_DELAY) == pdTRUE) {
Serial.println(state.current_position);
last_debug = millis();
}
static uint32_t last_stack_debug;
if (millis() - last_stack_debug > 1000) {
Serial.println("Stack high water:");
Serial.printf("main: %d\n", uxTaskGetStackHighWaterMark(NULL));
#if SK_DISPLAY
Serial.printf("display: %d\n", uxTaskGetStackHighWaterMark(display_task.getHandle()));
#endif
Serial.printf("motor: %d\n", uxTaskGetStackHighWaterMark(motor_task.getHandle()));
Serial.printf("interface: %d\n", uxTaskGetStackHighWaterMark(interface_task.getHandle()));
last_stack_debug = millis();
}
}

368
firmware/src/motor_task.cpp
View File

@@ -1,13 +1,10 @@
#include <SimpleFOC.h>
#include <sensors/MagneticSensorI2C.h>
#include "motor_task.h"
#include "mt6701_sensor.h"
#include "tlv_sensor.h"
template <typename T> T CLAMP(const T& value, const T& low, const T& high)
{
return value < low ? low : (value > high ? high : value);
}
#include "util.h"
static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
static const float DEAD_ZONE_RAD = 1 * _PI / 180;
@@ -19,8 +16,8 @@ static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
static const float IDLE_CORRECTION_RATE_ALPHA = 0.0005;
MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
queue_ = xQueueCreate(1, sizeof(KnobConfig));
MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 1200, 1, task_core) {
queue_ = xQueueCreate(5, sizeof(Command));
assert(queue_ != NULL);
}
@@ -28,11 +25,15 @@ MotorTask::~MotorTask() {}
// BLDC motor & driver instance
BLDCMotor motor = BLDCMotor(7);
BLDCDriver6PWM driver = BLDCDriver6PWM(27, 26, 25, 33, 32, 13);
TlvSensor tlv = TlvSensor();
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);
@@ -40,18 +41,41 @@ 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();
Wire.begin();
Wire.setClock(400000);
tlv.init();
#if SENSOR_TLV
encoder.init(Wire, false);
#endif
#if SENSOR_MT6701
encoder.init();
// motor.LPF_angle = LowPassFilter(0.05);
#endif
// motor.LPF_current_q = {0.01};
motor.linkDriver(&driver);
motor.controller = MotionControlType::torque;
motor.voltage_limit = 5;
motor.linkSensor(&tlv);
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.
@@ -66,16 +90,191 @@ void MotorTask::run() {
motor.init();
tlv.update();
encoder.update();
delay(10);
// 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;
motor.initFOC(zero_electric_offset, Direction::CCW);
motor.initFOC(zero_electric_offset, foc_direction);
bool calibrate = false;
Serial.println("Press Y to run calibration");
uint32_t t = millis();
while (millis() - t < 3000) {
if (Serial.read() == 'Y') {
calibrate = true;
break;
}
delay(10);
}
if (calibrate) {
motor.controller = MotionControlType::angle_openloop;
motor.pole_pairs = 1;
motor.initFOC(0, Direction::CW);
float a = 0;
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
motor.move(a);
delay(1);
}
float start_sensor = encoder.getAngle();
for (; a < 3 * _2PI; a += 0.01) {
encoder.update();
motor.move(a);
delay(1);
}
for (uint8_t i = 0; i < 200; i++) {
encoder.update();
delay(1);
}
float end_sensor = encoder.getAngle();
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();
// TODO: check for no motor movement!
Serial.print("Sensor measures positive for positive motor rotation: ");
if (end_sensor > start_sensor) {
Serial.println("YES, Direction=CW");
motor.initFOC(0, Direction::CW);
} else {
Serial.println("NO, Direction=CCW");
motor.initFOC(0, Direction::CCW);
}
// Rotate many 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;
motor.move(a);
Serial.println("Going to electrical zero...");
float destination = a + _2PI;
for (; a < destination; a += 0.03) {
encoder.update();
motor.move(a);
delay(1);
}
Serial.println("pause...");
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
delay(1);
}
Serial.println("Measuring...");
start_sensor = motor.sensor_direction * encoder.getAngle();
destination = a + electrical_revolutions * _2PI;
for (; a < destination; a += 0.03) {
encoder.update();
motor.move(a);
delay(1);
}
for (uint16_t i = 0; i < 1000; i++) {
encoder.update();
motor.move(a);
delay(1);
}
end_sensor = motor.sensor_direction * encoder.getAngle();
motor.voltage_limit = 0;
motor.move(a);
if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
Serial.println("ERROR: motor did not reach target!");
while(1) {}
}
float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
Serial.println(electrical_per_mechanical);
int measured_pole_pairs = (int)round(electrical_per_mechanical);
Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
delay(1000);
// Measure mechanical angle at every electrical zero for several revolutions
motor.voltage_limit = 5;
motor.move(a);
float offset_x = 0;
float offset_y = 0;
float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
float destination2 = (floor(a / _2PI)) * _2PI;
for (; a < destination1; a += 0.4) {
motor.move(a);
delay(100);
for (uint8_t i = 0; i < 100; i++) {
encoder.update();
delay(1);
}
float real_electrical_angle = _normalizeAngle(a);
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
float offset_angle = measured_electrical_angle - real_electrical_angle;
offset_x += cosf(offset_angle);
offset_y += sinf(offset_angle);
Serial.print(degrees(real_electrical_angle));
Serial.print(", ");
Serial.print(degrees(measured_electrical_angle));
Serial.print(", ");
Serial.println(degrees(_normalizeAngle(offset_angle)));
}
for (; a > destination2; a -= 0.4) {
motor.move(a);
delay(100);
for (uint8_t i = 0; i < 100; i++) {
encoder.update();
delay(1);
}
float real_electrical_angle = _normalizeAngle(a);
float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle() - 0);
float offset_angle = measured_electrical_angle - real_electrical_angle;
offset_x += cosf(offset_angle);
offset_y += sinf(offset_angle);
Serial.print(degrees(real_electrical_angle));
Serial.print(", ");
Serial.print(degrees(measured_electrical_angle));
Serial.print(", ");
Serial.println(degrees(_normalizeAngle(offset_angle)));
}
motor.voltage_limit = 0;
motor.move(a);
float avg_offset_angle = atan2f(offset_y, offset_x);
// Apply settings
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.println(motor.zero_electric_angle);
Serial.print(" direction: ");
if (motor.sensor_direction == Direction::CW) {
Serial.println("CW");
} else {
Serial.println("CCW");
}
Serial.printf(" pole pairs: %d\n", motor.pole_pairs);
delay(2000);
}
Serial.println(motor.zero_electric_angle);
command.add('M', &doMotor, "foo");
@@ -96,33 +295,59 @@ void MotorTask::run() {
uint32_t last_idle_start = 0;
uint32_t last_debug = 0;
uint32_t last_display_update = 0;
uint32_t last_publish = 0;
while (1) {
motor.loopFOC();
if (xQueueReceive(queue_, &config, 0) == pdTRUE) {
Serial.println("Got new config");
current_detent_center = motor.shaft_angle;
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.04;
const float derivative_upper_strength = config.detent_strength_unit * 0;
const float derivative_position_width_lower = 5 * PI / 180;
const float derivative_position_width_upper = 10 * PI / 180;
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)
);
// 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);
@@ -147,6 +372,9 @@ void MotorTask::run() {
}
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;
@@ -168,27 +396,63 @@ void MotorTask::run() {
if (fabsf(motor.shaft_velocity) > 20) {
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 {
motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment));
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_display_update > 10) {
display_task_.setData({
if (millis() - last_publish > 10) {
publish({
.current_position = config.position,
.sub_position_unit = -angle_to_detent_center / config.position_width_radians,
.config = config,
});
last_display_update = millis();
last_publish = millis();
}
motor.monitor();
// command.run();
delay(1);
}
}
void MotorTask::setConfig(const KnobConfig& config) {
xQueueOverwrite(queue_, &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);
}
}

31
firmware/src/motor_task.h
View File

@@ -1,24 +1,49 @@
#pragma once
#include <Arduino.h>
#include <vector>
#include "knob_data.h"
#include "task.h"
#include "display_task.h"
enum class CommandType {
CONFIG,
HAPTIC,
};
struct HapticData {
bool press;
};
struct Command {
CommandType command_type;
union CommandData {
KnobConfig config;
HapticData haptic;
};
CommandData data;
};
class MotorTask : public Task<MotorTask> {
friend class Task<MotorTask>; // Allow base Task to invoke protected run()
public:
MotorTask(const uint8_t task_core, DisplayTask& display_task);
MotorTask(const uint8_t task_core);
~MotorTask();
void setConfig(const KnobConfig& config);
void playHaptic(bool press);
void addListener(QueueHandle_t queue);
protected:
void run();
private:
DisplayTask& display_task_;
QueueHandle_t queue_;
std::vector<QueueHandle_t> listeners_;
void publish(const KnobState& state);
};

118
firmware/src/mt6701_sensor.cpp Normal file
View File

@@ -0,0 +1,118 @@
#include "mt6701_sensor.h"
#include "driver/spi_master.h"
static const float ALPHA = 0.4;
static uint8_t tableCRC6[64] = {
0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09,
0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11,
0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39,
0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21,
0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A,
0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32,
0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A,
0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02
};
/*32-bit input data, right alignment, Calculation over 18 bits (mult. of 6) */
static uint8_t CRC6_43_18bit (uint32_t w_InputData)
{
uint8_t b_Index = 0;
uint8_t b_CRC = 0;
b_Index = (uint8_t )(((uint32_t)w_InputData >> 12u) & 0x0000003Fu);
b_CRC = (uint8_t )(((uint32_t)w_InputData >> 6u) & 0x0000003Fu);
b_Index = b_CRC ^ tableCRC6[b_Index];
b_CRC = (uint8_t )((uint32_t)w_InputData & 0x0000003Fu);
b_Index = b_CRC ^ tableCRC6[b_Index];
b_CRC = tableCRC6[b_Index];
return b_CRC;
}
MT6701Sensor::MT6701Sensor() {}
void MT6701Sensor::init() {
pinMode(PIN_MT_CSN, OUTPUT);
digitalWrite(PIN_MT_CSN, HIGH);
spi_bus_config_t tx_bus_config = {
.mosi_io_num = -1,
.miso_io_num = PIN_MT_DATA,
.sclk_io_num = PIN_MT_CLOCK,
.quadwp_io_num = -1,
.quadhd_io_num = -1,
.max_transfer_sz = 1000,
};
esp_err_t ret = spi_bus_initialize(HSPI_HOST, &tx_bus_config, 1);
ESP_ERROR_CHECK(ret);
spi_device_interface_config_t tx_device_config = {
.command_bits=0,
.address_bits=0,
.dummy_bits=0,
.mode=1,
.duty_cycle_pos=0,
.cs_ena_pretrans=4,
.cs_ena_posttrans=0,
.clock_speed_hz=4000000,
.input_delay_ns=0,
.spics_io_num=PIN_MT_CSN,
.flags = 0,
.queue_size=1,
.pre_cb=NULL,
.post_cb=NULL,
};
ret=spi_bus_add_device(HSPI_HOST, &tx_device_config, &spi_device_);
ESP_ERROR_CHECK(ret);
spi_transaction_.flags = SPI_TRANS_USE_RXDATA;
spi_transaction_.length = 24;
spi_transaction_.rxlength = 24;
spi_transaction_.tx_buffer = NULL;
spi_transaction_.rx_buffer = NULL;
}
float MT6701Sensor::getSensorAngle() {
uint32_t now = micros();
if (now - last_update_ > 100) {
esp_err_t ret=spi_device_polling_transmit(spi_device_, &spi_transaction_);
assert(ret==ESP_OK);
uint32_t spi_32 = (spi_transaction_.rx_data[0] << 16) | (spi_transaction_.rx_data[1] << 8) | spi_transaction_.rx_data[2];
uint32_t angle_spi = spi_32 >> 10;
uint8_t field_status = (spi_32 >> 6) & 0x3;
uint8_t push_status = (spi_32 >> 8) & 0x1;
uint8_t loss_status = (spi_32 >> 9) & 0x1;
uint8_t received_crc = spi_32 & 0x3F;
uint8_t calculated_crc = CRC6_43_18bit(spi_32 >> 6);
if (received_crc == calculated_crc) {
float new_angle = (float)angle_spi * 2 * PI / 16384;
float new_x = cosf(new_angle);
float new_y = sinf(new_angle);
x_ = new_x * ALPHA + x_ * (1-ALPHA);
y_ = new_y * ALPHA + y_ * (1-ALPHA);
} else {
Serial.printf("Bad CRC. expected %d, actual %d\n", calculated_crc, received_crc);
}
last_update_ = now;
}
float rad = -atan2f(y_, x_);
if (rad < 0) {
rad += 2*PI;
}
return rad;
}

27
firmware/src/mt6701_sensor.h Normal file
View File

@@ -0,0 +1,27 @@
#pragma once
#include <SimpleFOC.h>
#include "driver/spi_master.h"
class MT6701Sensor : public Sensor {
public:
MT6701Sensor();
// initialize the sensor hardware
void init();
// Get current shaft angle from the sensor hardware, and
// return it as a float in radians, in the range 0 to 2PI.
// - This method is pure virtual and must be implemented in subclasses.
// Calling this method directly does not update the base-class internal fields.
// Use update() when calling from outside code.
float getSensorAngle();
private:
spi_device_handle_t spi_device_;
spi_transaction_t spi_transaction_ = {};
float x_;
float y_;
uint32_t last_update_;
};

34
firmware/src/tlv_sensor.cpp
View File

@@ -1,11 +1,13 @@
#include "tlv_sensor.h"
static const float ALPHA = 0.04;
static const float ALPHA = 1;
TlvSensor::TlvSensor() {}
void TlvSensor::init() {
tlv_.begin();
void TlvSensor::init(TwoWire* wire, bool invert) {
wire_ = wire;
invert_ = invert;
tlv_.begin(*wire);
tlv_.setAccessMode(Tlv493d::AccessMode_e::MASTERCONTROLLEDMODE);
tlv_.disableInterrupt();
tlv_.disableTemp();
@@ -13,13 +15,35 @@ void TlvSensor::init() {
float TlvSensor::getSensorAngle() {
uint32_t now = micros();
if (now - last_update_ > 100) {
if (now - last_update_ > 50) {
tlv_.updateData();
frame_counts_[cur_frame_count_index_] = tlv_.getExpectedFrameCount();
cur_frame_count_index_++;
if (cur_frame_count_index_ >= sizeof(frame_counts_)) {
cur_frame_count_index_ = 0;
}
x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
last_update_ = now;
bool all_same = true;
uint8_t match_frame = frame_counts_[0];
for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
if (frame_counts_[i] != match_frame) {
all_same = false;
break;
}
}
if (all_same) {
Serial.println("LOCKED!");
init(wire_, invert_);
// Force unique frame counts to avoid reset loop
for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
frame_counts_[i] = i;
}
}
}
float rad = atan2f(y_, x_);
float rad = (invert_ ? -1 : 1) * atan2f(y_, x_);
if (rad < 0) {
rad += 2*PI;
}

7
firmware/src/tlv_sensor.h
View File

@@ -8,7 +8,7 @@ class TlvSensor : public Sensor {
TlvSensor();
// initialize the sensor hardware
void init();
void init(TwoWire* wire, bool invert);
// Get current shaft angle from the sensor hardware, and
// return it as a float in radians, in the range 0 to 2PI.
@@ -21,4 +21,9 @@ class TlvSensor : public Sensor {
float x_;
float y_;
uint32_t last_update_;
TwoWire* wire_;
bool invert_;
uint8_t frame_counts_[3] = {};
uint8_t cur_frame_count_index_ = 0;
};

7
firmware/src/util.h Normal file
View File

@@ -0,0 +1,7 @@
#pragma once
template <typename T> T CLAMP(const T& value, const T& low, const T& high)
{
return value < low ? low : (value > high ? high : value);
}