ulysse
/
PAMI_Super_Star_2025
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Changement d'angle avec une vitesse angulaire fonctionnelle.

This commit is contained in:
Ulysse Cura 2025-01-31 22:47:19 +01:00
parent 6d80d913fd
commit 4b59f76e46
1 changed files with 5 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
IO.hpp
View File

@ -17,9 +17,9 @@
#define DANCING_ACTION_DELTA_ANGLE 2 #define DANCING_ACTION_DELTA_ANGLE 2
#define GAIN_KD 50 #define GAIN_KD 50
#define ANGULAR_SPEED 10 // °/s #define ANGULAR_SPEED 120 // °/s
using std::abs; using std::abs, std::signbit;
enum class Axes { enum class Axes {
X, X,
@ -125,15 +125,15 @@ class IO {
{ {
static unsigned long prev_time {millis()}; static unsigned long prev_time {millis()};
float err_dir = dir - m_dir; float err_dir = dir - m_dir;
float angular_displacement = angular_speed * static_cast<float>(millis() / 1000 - prev_time / 1000); float angular_displacement = angular_speed * static_cast<float>(millis() - prev_time) / 1000.0f;
if(abs(err_dir) < angular_displacement) if(abs(err_dir) < abs(angular_displacement))
{ {
m_dir = dir; m_dir = dir;
} }
else else
{ {
m_dir += angular_displacement * (err_dir < 0) ? -1 : 1; m_dir += angular_displacement * ((!signbit(err_dir)) * 2 - 1);
} }
prev_time = millis(); prev_time = millis();