dead zone and end stops

This commit is contained in:
Scott Bezek 2021-10-23 20:47:24 -07:00
parent d9b97f8a2a
commit 51f43cf727
3 changed files with 55 additions and 11 deletions

View File

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

View File

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

View File

@ -20,4 +20,5 @@ class TlvSensor : public Sensor {
Tlv493d tlv_ = Tlv493d();
float x_;
float y_;
uint32_t last_update_;
};