diff --git a/firmware/platformio.ini b/firmware/platformio.ini index 8b29000..d42f78c 100644 --- a/firmware/platformio.ini +++ b/firmware/platformio.ini @@ -18,8 +18,7 @@ monitor_flags = --echo --filter=esp32_exception_decoder lib_deps = - TFT_eSPI@2.3.70 - fastled/FastLED @ ^3.4.0 + bodmer/TFT_eSPI@2.4.25 askuric/Simple FOC @ ^2.2 infineon/TLV493D-Magnetic-Sensor @ ^1.0.3 bxparks/AceButton @ ^1.9.1 diff --git a/firmware/src/display_task.cpp b/firmware/src/display_task.cpp index eb8be33..5e62f0e 100644 --- a/firmware/src/display_task.cpp +++ b/firmware/src/display_task.cpp @@ -3,7 +3,7 @@ #include "font/roboto_light_60.h" -DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 8192, 1, task_core} { +DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 2048, 1, task_core} { semaphore_ = xSemaphoreCreateMutex(); assert(semaphore_ != NULL); xSemaphoreGive(semaphore_); @@ -78,13 +78,16 @@ void DisplayTask::run() { tft_.setRotation(0); spr_.setColorDepth(16); - spr_.createSprite(TFT_WIDTH, TFT_HEIGHT); - spr_.setFreeFont(&Roboto_Light_60); + if (spr_.createSprite(TFT_WIDTH, TFT_HEIGHT) == nullptr) { + Serial.println("ERROR: sprite allocation failed!"); + } spr_.setTextColor(0xFFFF, TFT_BLACK); KnobState state; const int RADIUS = TFT_WIDTH / 2; + const uint16_t FILL_COLOR = spr_.color565(90, 18, 151); + const uint16_t DOT_COLOR = spr_.color565(80, 100, 200); int32_t pointer_center_x = TFT_WIDTH / 2; int32_t pointer_center_y = TFT_HEIGHT / 2; @@ -94,42 +97,75 @@ void DisplayTask::run() { spr_.setTextDatum(CC_DATUM); spr_.setTextColor(TFT_WHITE); while(1) { - { SemaphoreGuard lock(semaphore_); state = state_; } spr_.fillSprite(TFT_BLACK); - if (state.num_positions > 1) { - int32_t height = state.current_position * TFT_HEIGHT / (state.num_positions - 1); - spr_.fillRect(0, TFT_HEIGHT - height, TFT_WIDTH, height, spr_.color565(109, 20, 176)); + if (state.config.num_positions > 1) { + int32_t height = state.current_position * TFT_HEIGHT / (state.config.num_positions - 1); + spr_.fillRect(0, TFT_HEIGHT - height, TFT_WIDTH, height, FILL_COLOR); } - spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2, 1); + 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; + char* start = state.config.descriptor; + char* end = start + strlen(state.config.descriptor); + while (start < end) { + char* newline = strchr(start, '\n'); + if (newline == nullptr) { + newline = end; + } + + char buf[sizeof(state.config.descriptor)] = {}; + strncat(buf, start, min(sizeof(buf) - 1, (size_t)(newline - start))); + spr_.drawString(String(buf), TFT_WIDTH / 2, line_y, 1); + start = newline + 1; + line_y += spr_.fontHeight(1); + } float left_bound = PI / 2; - if (state.num_positions > 0) { - float range_radians = (state.num_positions - 1) * state.position_width_radians; + if (state.config.num_positions > 0) { + float range_radians = (state.config.num_positions - 1) * state.config.position_width_radians; left_bound = PI / 2 + range_radians / 2; float right_bound = PI / 2 - range_radians / 2; 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); } - float adjusted_sub_position = state.sub_position_unit; - if (state.num_positions > 0) { + float adjusted_sub_position = state.sub_position_unit * state.config.position_width_radians; + if (state.config.num_positions > 0) { if (state.current_position == 0 && state.sub_position_unit < 0) { - adjusted_sub_position = -logf(1 - state.sub_position_unit); - } else if (state.current_position == state.num_positions - 1 && state.sub_position_unit > 0) { - adjusted_sub_position = logf(1 + state.sub_position_unit); + adjusted_sub_position = -logf(1 - state.sub_position_unit * state.config.position_width_radians / 5 / PI * 180) * 5 * PI / 180; + } else if (state.current_position == state.config.num_positions - 1 && state.sub_position_unit > 0) { + adjusted_sub_position = logf(1 + state.sub_position_unit * state.config.position_width_radians / 5 / PI * 180) * 5 * PI / 180; } } - float angle = left_bound - (state.current_position + adjusted_sub_position) * state.position_width_radians; - spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(angle), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(angle), 5, TFT_BLUE); + float raw_angle = left_bound - state.current_position * state.config.position_width_radians; + float adjusted_angle = raw_angle - adjusted_sub_position; + if (state.config.num_positions > 0 && ((state.current_position == 0 && state.sub_position_unit < 0) || (state.current_position == state.config.num_positions - 1 && state.sub_position_unit > 0))) { + + spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(raw_angle), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(raw_angle), 5, DOT_COLOR); + if (raw_angle < adjusted_angle) { + for (float r = raw_angle; r <= adjusted_angle; r += 2 * PI / 180) { + spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(r), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(r), 2, DOT_COLOR); + } + spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(adjusted_angle), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(adjusted_angle), 2, DOT_COLOR); + } else { + for (float r = raw_angle; r >= adjusted_angle; r -= 2 * PI / 180) { + spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(r), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(r), 2, DOT_COLOR); + } + spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(adjusted_angle), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(adjusted_angle), 2, DOT_COLOR); + } + } else { + spr_.fillCircle(TFT_WIDTH/2 + (RADIUS - 10) * cosf(adjusted_angle), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(adjusted_angle), 5, DOT_COLOR); + } spr_.pushSprite(0, 0); delay(2); diff --git a/firmware/src/interface_task.cpp b/firmware/src/interface_task.cpp index 89fab04..88b6cb2 100644 --- a/firmware/src/interface_task.cpp +++ b/firmware/src/interface_task.cpp @@ -7,71 +7,80 @@ using namespace ace_button; static KnobConfig configs[] = { { - .num_positions = 0, - .position = 0, - .position_width_radians = 10 * PI / 180, - .detent_strength_unit = 0, - .snap_point = 1.1, + 0, + 0, + 10 * PI / 180, + 0, + 1.1, + "Unbounded\nNo detents", }, { - .num_positions = 11, - .position = 0, - .position_width_radians = 10 * PI / 180, - .detent_strength_unit = 0, - .snap_point = 1.1, + 11, + 0, + 10 * PI / 180, + 0, + 1.1, + "Bounded 0-10\nNo detents", }, { - .num_positions = 73, - .position = 0, - .position_width_radians = 10 * PI / 180, - .detent_strength_unit = 0, - .snap_point = 1.1, + 73, + 0, + 10 * PI / 180, + 0, + 1.1, + "Multi-rev\nNo detents", }, { - .num_positions = 2, - .position = 0, - .position_width_radians = 60 * PI / 180, - .detent_strength_unit = 1, - .snap_point = 1.1, + 2, + 0, + 45 * PI / 180, + 1, + 0.6, // Note the snap point is slightly past the midpoint (0.5); compare to normal detents which use a snap point *past* the next value (i.e. > 1) + "On/off\nStrong detent", }, { - .num_positions = 2, - .position = 0, - .position_width_radians = 60 * PI / 180, - .detent_strength_unit = 1, - .snap_point = 0.6, + 1, + 0, + 60 * PI / 180, + 0.01, + 1.1, + "Return-to-center", }, { - .num_positions = 256, - .position = 127, - .position_width_radians = 1 * PI / 180, - .detent_strength_unit = 0, - .snap_point = 1.1, + 256, + 127, + 1 * PI / 180, + 0, + 1.1, + "Fine values\nNo detents", }, { - .num_positions = 256, - .position = 127, - .position_width_radians = 1 * PI / 180, - .detent_strength_unit = 1, - .snap_point = 1.1, + 256, + 127, + 1 * PI / 180, + 1, + 1.1, + "Fine values\nWith detents", }, { - .num_positions = 32, - .position = 0, - .position_width_radians = 8.225806452 * PI / 180, - .detent_strength_unit = 1, - .snap_point = 1.1, + 32, + 0, + 8.225806452 * PI / 180, + 1, + 1.1, + "Coarse values\nStrong detents", }, { - .num_positions = 32, - .position = 0, - .position_width_radians = 8.225806452 * PI / 180, - .detent_strength_unit = 0.1, - .snap_point = 1.1, + 32, + 0, + 8.225806452 * PI / 180, + 0.1, + 1.1, + "Coarse values\nWeak detents", }, }; -InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task) : Task{"Interface", 8192, 1, task_core}, motor_task_(motor_task) { +InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task) : Task{"Interface", 2048, 1, task_core}, motor_task_(motor_task) { } InterfaceTask::~InterfaceTask() {} @@ -108,6 +117,6 @@ void InterfaceTask::handleEvent(AceButton* button, uint8_t event_type, uint8_t b void InterfaceTask::nextConfig() { current_config_ = (current_config_ + 1) % COUNT_OF(configs); - Serial.printf("Changing config to %d\n", current_config_); + Serial.printf("Changing config to %d:\n%s\n", current_config_, configs[current_config_].descriptor); motor_task_.setConfig(configs[current_config_]); } diff --git a/firmware/src/knob_data.h b/firmware/src/knob_data.h index 77d92fb..cbca461 100644 --- a/firmware/src/knob_data.h +++ b/firmware/src/knob_data.h @@ -8,11 +8,11 @@ struct KnobConfig { float position_width_radians; float detent_strength_unit; float snap_point; + char descriptor[50]; }; struct KnobState { - int32_t num_positions; int32_t current_position; float sub_position_unit; - float position_width_radians; + KnobConfig config; }; diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 1d4ecf6..30295ef 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -18,9 +18,9 @@ CRGB leds[1]; void setup() { Serial.begin(115200); - display_task.begin(); motor_task.begin(); interface_task.begin(); + display_task.begin(); vTaskDelete(nullptr); } diff --git a/firmware/src/motor_task.cpp b/firmware/src/motor_task.cpp index ad42b9c..80cc217 100644 --- a/firmware/src/motor_task.cpp +++ b/firmware/src/motor_task.cpp @@ -69,7 +69,7 @@ void MotorTask::run() { tlv.update(); delay(10); - motor.initFOC(6, Direction::CW); + motor.initFOC(-0.6, Direction::CCW); Serial.println(motor.zero_electric_angle); command.add('M', &doMotor, "foo"); @@ -90,6 +90,8 @@ void MotorTask::run() { uint32_t last_idle_start = 0; uint32_t last_debug = 0; + uint32_t last_display_update = 0; + while (1) { motor.loopFOC(); @@ -138,21 +140,23 @@ void MotorTask::run() { 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 = out_of_bounds ? 10 : 3; motor.PID_velocity.P = out_of_bounds ? 4 : config.detent_strength_unit * 4; - motor.PID_velocity.D = config.detent_strength_unit * 0.04; + motor.PID_velocity.D = config.detent_strength_unit * 0.02; if (fabsf(motor.shaft_velocity) > 20) { - // Don't apply torque if velocity is too high (helps avoid feedback loop) + // 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)); } - display_task_.setData({ - .num_positions = config.num_positions, - .current_position = config.position, - .sub_position_unit = -angle_to_detent_center / config.position_width_radians, - .position_width_radians = config.position_width_radians, - }); + if (millis() - last_display_update > 10) { + display_task_.setData({ + .current_position = config.position, + .sub_position_unit = -angle_to_detent_center / config.position_width_radians, + .config = config, + }); + last_display_update = millis(); + } motor.monitor(); // command.run(); diff --git a/firmware/src/tlv_sensor.cpp b/firmware/src/tlv_sensor.cpp index 4ece47c..73821a2 100644 --- a/firmware/src/tlv_sensor.cpp +++ b/firmware/src/tlv_sensor.cpp @@ -1,6 +1,6 @@ #include "tlv_sensor.h" -static const float ALPHA = 0.05; +static const float ALPHA = 0.04; TlvSensor::TlvSensor() {}