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

Machine à état fonctionnelle !

This commit is contained in:
Ulysse Cura 2025-01-31 19:30:16 +01:00
parent f8b7938e7c
commit 6d80d913fd
2 changed files with 32 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
IO.hpp
View File

@ -276,6 +276,8 @@ class IO {
//m1_speed = static_cast<int>(m_speed + 2 * correction);
}
Serial.printf(">Actual Angle: %f\n", actual_angle);
m_motors[0].setSpeed(m1_speed);
m_motors[1].setSpeed(m2_speed);
}

50
Main.ino
View File

@ -7,7 +7,7 @@
#include "Ressources.hpp"
// Unit tests activation
#define UNIT_TESTS
//#define UNIT_TESTS
/*
Etapes :
@ -65,7 +65,11 @@ void setup()
Serial.println("Initalisation Finished");
my_IO.motorControlOn();
my_IO.motorControlOff();
my_IO.setDir(0);
my_IO.setSpeed(0.0f);
//my_IO.startDancingAction(15);
#ifdef UNIT_TESTS
Serial.println("UNIT_TESTS");
@ -78,18 +82,19 @@ void loop()
{
my_IO.update();
//Serial.printf("Is Selected Color Blue : %d\n", my_IO.isSelectedColorBlue());
//Serial.printf("Is Tirette Pulled : %d\n", my_IO.isTirettePulled());
Serial.printf("Is Ta Mere Selected : %d\n", my_IO.isSelectedColorBlue());
Serial.printf("Is Tirette Pulled : %d\n", my_IO.isTirettePulled());
my_IO.setDir(0);
my_IO.setSpeed(2048.0f);
my_IO.startDancingAction();
}
#else
void loop()
{
static State actual_state {State::ForwardToRamp};
static State actual_state {State::WaitingForTirette};
my_IO.update();
switch(actual_state)
{
@ -103,59 +108,64 @@ void loop()
case State::WaitingTimer:
static unsigned long initial_time = millis();
if(millis() - initial_time >= 90000)
//if(millis() - initial_time >= 90000)
if(millis() - initial_time >= 2000)
{
actual_state = State::ForwardToRamp;
}
break;
case State::ForwardToRamp:
my_IO.forward();
my_IO.motorControlOn();
my_IO.setSpeed(2048.0f);
if(my_IO.getAngle(Axes::Y) > 9)
if(my_IO.getAngle(Axes::Y) < -5)
{
actual_state = State::ForwardToScene;
}
break;
case State::ForwardToScene:
IO::motors.forward();
my_IO.setSpeed(2048.0f);
if(IO::gyro.getAngle(Axes::Y) < 2)
if(my_IO.getAngle(Axes::Y) > -5.0f)
{
actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
//actual_state = my_IO.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
actual_state = State::Dancing;
}
break;
case State::Turn90Blue:
IO::motors.setDir(90);
my_IO.setDirWithAngularSpeed(90.0f);
if(IO::gyro.getAngle() > 90)
if(my_IO.getAngle(Axes::Z) > 90.0f)
{
actual_state = State::ForwardToSceneEdge;
}
break;
case State::Turn90Yellow:
IO::motors.setAngle(-90);
my_IO.setDirWithAngularSpeed(-90);
if(IO::gyro.getAngle() < -90)
if(my_IO.getAngle(Axes::Z) < -90)
{
actual_state = State::ForwardToSceneEdge;
}
break;
case State::ForwardToSceneEdge:
IO::motors.forward();
my_IO.setSpeed(2048);
if(IO::gyro.getAngle() < 0)
if(my_IO.getAngle(Axes::Y) < 2)
{
actual_state = State::Dancing;
}
break;
case State::Dancing:
IO::ext_controls.startSpecialAction();
my_IO.setSpeed(0);
my_IO.motorControlOff();
my_IO.startDancingAction();
break;
}
}