mirror of
https://github.com/scottbez1/smartknob.git
synced 2025-11-04 17:19:40 +08:00
dead zone and end stops
This commit is contained in:
parent
d9b97f8a2a
commit
51f43cf727
@ -3,6 +3,15 @@
|
||||
#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;
|
||||
|
||||
|
||||
MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
|
||||
}
|
||||
|
||||
@ -57,24 +66,54 @@ void MotorTask::run() {
|
||||
motor.useMonitoring(Serial);
|
||||
motor.monitor_downsample = 0; // disable monitor at first - optional
|
||||
|
||||
float attract_angle = 0;
|
||||
|
||||
disableCore0WDT();
|
||||
|
||||
float current_detent_center = motor.shaft_angle;
|
||||
|
||||
int min = 0;
|
||||
int max = 255;
|
||||
int value = 0;
|
||||
|
||||
|
||||
while (1) {
|
||||
|
||||
motor.loopFOC();
|
||||
|
||||
if (fabs(detents) < 0.01) {
|
||||
motor.move(0);
|
||||
display_task_.set_angle(motor.shaft_angle);
|
||||
current_detent_center = motor.shaft_angle;
|
||||
} else {
|
||||
float detent_angle = 2*PI/detents;
|
||||
float detent_width = 2*PI/detents;
|
||||
|
||||
float error = (attract_angle - motor.shaft_angle);
|
||||
motor.move(motor.PID_velocity(error));
|
||||
// if (millis() - last > 500) {
|
||||
// last = millis();
|
||||
// current_detent_center += detent_width;
|
||||
// }
|
||||
|
||||
attract_angle = round(motor.shaft_angle/detent_angle)*detent_angle;
|
||||
display_task_.set_angle(attract_angle);
|
||||
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;
|
||||
angle_to_detent_center -= detent_width;
|
||||
value--;
|
||||
} else if (angle_to_detent_center < -detent_width * 1.2 && value < max) {
|
||||
current_detent_center -= detent_width;
|
||||
angle_to_detent_center += detent_width;
|
||||
value++;
|
||||
}
|
||||
|
||||
float dead_zone = 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) {
|
||||
motor.move(0);
|
||||
} else {
|
||||
motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone));
|
||||
}
|
||||
|
||||
display_task_.set_angle(-value * PI / 180);
|
||||
}
|
||||
motor.monitor();
|
||||
command.run();
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
#include "tlv_sensor.h"
|
||||
|
||||
static const float ALPHA = 0.1;
|
||||
static const float ALPHA = 0.05;
|
||||
|
||||
TlvSensor::TlvSensor() {}
|
||||
|
||||
@ -12,9 +12,13 @@ void TlvSensor::init() {
|
||||
}
|
||||
|
||||
float TlvSensor::getSensorAngle() {
|
||||
tlv_.updateData();
|
||||
x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
|
||||
y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
|
||||
uint32_t now = micros();
|
||||
if (now - last_update_ > 100) {
|
||||
tlv_.updateData();
|
||||
x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
|
||||
y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
|
||||
last_update_ = now;
|
||||
}
|
||||
float rad = atan2f(y_, x_);
|
||||
if (rad < 0) {
|
||||
rad += 2*PI;
|
||||
|
||||
@ -20,4 +20,5 @@ class TlvSensor : public Sensor {
|
||||
Tlv493d tlv_ = Tlv493d();
|
||||
float x_;
|
||||
float y_;
|
||||
uint32_t last_update_;
|
||||
};
|
||||
|
||||
Loading…
Reference in New Issue
Block a user