mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-09-26 23:09:27 +08:00
Auto-adjust centerpoint when idle
This commit is contained in:
parent
51f43cf727
commit
bbc51702ff
@ -20,8 +20,9 @@ monitor_flags =
|
||||
lib_deps =
|
||||
TFT_eSPI@2.3.59
|
||||
fastled/FastLED @ ^3.4.0
|
||||
askuric/Simple FOC @ ^2.2
|
||||
infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
|
||||
askuric/Simple FOC @ ^2.2
|
||||
infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
|
||||
bxparks/AceButton @ ^1.9.1
|
||||
|
||||
build_flags =
|
||||
-Os
|
||||
|
@ -1,12 +1,15 @@
|
||||
#include <AceButton.h>
|
||||
#include <Arduino.h>
|
||||
#include <TFT_eSPI.h>
|
||||
#include <FastLED.h>
|
||||
#include <SimpleFOC.h>
|
||||
#include <TFT_eSPI.h>
|
||||
|
||||
#include "display_task.h"
|
||||
#include "motor_task.h"
|
||||
#include "tlv_sensor.h"
|
||||
|
||||
using namespace ace_button;
|
||||
|
||||
DisplayTask display_task = DisplayTask(1);
|
||||
MotorTask motor_task = MotorTask(0, display_task);
|
||||
|
||||
|
@ -3,13 +3,20 @@
|
||||
#include "motor_task.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);
|
||||
}
|
||||
|
||||
static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
|
||||
static const float DEAD_ZONE_RAD = 3 * _PI / 180;
|
||||
static const float DEAD_ZONE_RAD = 1 * _PI / 180;
|
||||
|
||||
static const float IDLE_VELOCITY_EWMA_ALPHA = 0.001;
|
||||
static const float IDLE_VELOCITY_RAD_PER_SEC = 0.05;
|
||||
static const uint32_t IDLE_CORRECTION_DELAY_MILLIS = 500;
|
||||
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) {
|
||||
@ -66,7 +73,7 @@ void MotorTask::run() {
|
||||
motor.useMonitoring(Serial);
|
||||
motor.monitor_downsample = 0; // disable monitor at first - optional
|
||||
|
||||
disableCore0WDT();
|
||||
// disableCore0WDT();
|
||||
|
||||
float current_detent_center = motor.shaft_angle;
|
||||
|
||||
@ -74,11 +81,35 @@ void MotorTask::run() {
|
||||
int max = 255;
|
||||
int value = 0;
|
||||
|
||||
float idle_check_velocity_ewma = 0;
|
||||
uint32_t last_idle_start = 0;
|
||||
uint32_t last_debug = 0;
|
||||
|
||||
while (1) {
|
||||
|
||||
motor.loopFOC();
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
if (fabs(detents) < 0.01) {
|
||||
motor.move(0);
|
||||
display_task_.set_angle(motor.shaft_angle);
|
||||
@ -86,11 +117,6 @@ void MotorTask::run() {
|
||||
} else {
|
||||
float detent_width = 2*PI/detents;
|
||||
|
||||
// if (millis() - last > 500) {
|
||||
// last = millis();
|
||||
// current_detent_center += detent_width;
|
||||
// }
|
||||
|
||||
float angle_to_detent_center = motor.shaft_angle - current_detent_center;
|
||||
if (angle_to_detent_center > detent_width * 1.2 && value > min) {
|
||||
current_detent_center += detent_width;
|
||||
@ -102,15 +128,16 @@ void MotorTask::run() {
|
||||
value++;
|
||||
}
|
||||
|
||||
float dead_zone = CLAMP(
|
||||
float dead_zone_adjustment = CLAMP(
|
||||
angle_to_detent_center,
|
||||
fmaxf(-detent_width*DEAD_ZONE_DETENT_PERCENT, -DEAD_ZONE_RAD),
|
||||
fminf(detent_width*DEAD_ZONE_DETENT_PERCENT, DEAD_ZONE_RAD));
|
||||
|
||||
if (fabsf(motor.shaft_velocity) > 20) {
|
||||
// Don't apply torque if velocity is too high (helps avoid feedback loop)
|
||||
motor.move(0);
|
||||
} else {
|
||||
motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone));
|
||||
motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment));
|
||||
}
|
||||
|
||||
display_task_.set_angle(-value * PI / 180);
|
||||
|
Loading…
Reference in New Issue
Block a user