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

10
IO.hpp
View File

@ -17,9 +17,9 @@
#define DANCING_ACTION_DELTA_ANGLE 2
#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 {
X,
@ -125,15 +125,15 @@ class IO {
{
static unsigned long prev_time {millis()};
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;
}
else
{
m_dir += angular_displacement * (err_dir < 0) ? -1 : 1;
m_dir += angular_displacement * ((!signbit(err_dir)) * 2 - 1);
}
prev_time = millis();