Auto-adjust centerpoint when idle

This commit is contained in:
Scott Bezek 2021-10-24 14:42:21 -07:00
parent 51f43cf727
commit bbc51702ff
3 changed files with 43 additions and 12 deletions

View File

@ -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

View File

@ -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);

View File

@ -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);