146 lines
3.1 KiB
C++
146 lines
3.1 KiB
C++
#include <stdexcept>
|
|
#include <CodeCell.h>
|
|
|
|
#include "IO.hpp"
|
|
|
|
#define UNIT_TESTS
|
|
|
|
/*
|
|
Etapes :
|
|
- 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
|
|
- 2eme etape : avncer j'usqua la fin de la montée / condition : si fin de montée detecté : etape suivante
|
|
- 3eme etape : tourner de 90* / condition : si action terminé : etape suivante
|
|
- 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
|
|
- 5eme etape : faire tourner actionneurs pour figurine
|
|
*/
|
|
|
|
/*
|
|
Modules necessaires :
|
|
- angle // Asservissement et control du robot
|
|
- choc // Controle du robot (fin !)
|
|
(?)- accelerometre // Asservissement
|
|
*/
|
|
|
|
// Movement states
|
|
enum class State {
|
|
WaitingForTirette,
|
|
WaitingTimer,
|
|
ForwardToRamp,
|
|
ForwardToScene,
|
|
Turn90Blue, // If blue team
|
|
Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
|
|
ForwardToSceneEdge,
|
|
Dancing
|
|
};
|
|
|
|
// CodeCell implementation
|
|
CodeCell code_cell;
|
|
IO my_IO {};
|
|
|
|
void setup()
|
|
{
|
|
// Initialise serial
|
|
Serial.begin(115200);
|
|
|
|
// Initialise the CodeCell for angle and tap detectIOn
|
|
code_cell.Init(MOTION_GYRO + MOTION_TAP_DETECTOR + MOTION_ACCELEROMETER);
|
|
|
|
Serial.println("CACA");
|
|
|
|
// Initialise
|
|
my_IO.init();
|
|
}
|
|
|
|
#ifdef UNIT_TESTS
|
|
|
|
void loop()
|
|
{
|
|
if(code_cell.Run())
|
|
{
|
|
my_IO.update();
|
|
|
|
my_IO.isTirettePulled();
|
|
}
|
|
}
|
|
|
|
#else
|
|
|
|
void loop()
|
|
{
|
|
static State actual_state {State::ForwardToRamp};
|
|
|
|
// Runs every 100ms - Put your code here
|
|
if(code_cell.Run())
|
|
{
|
|
switch(actual_state)
|
|
{
|
|
case State::WaitingForTirette:
|
|
if(IO::ext_controls.isTirettePulled())
|
|
{
|
|
actual_state = State::WaitingTimer;
|
|
}
|
|
break;
|
|
|
|
case State::WaitingTimer:
|
|
static unsigned long initial_time = millis();
|
|
|
|
if(millis() - initial_time >= 90000)
|
|
{
|
|
actual_state = State::ForwardToRamp;
|
|
}
|
|
break;
|
|
|
|
case State::ForwardToRamp:
|
|
IO::motors.forward();
|
|
|
|
if(IO::gyro.getAngle() > 9)
|
|
{
|
|
actual_state = State::ForwardToScene;
|
|
}
|
|
break;
|
|
|
|
case State::ForwardToScene:
|
|
IO::motors.forward();
|
|
|
|
if(IO::gyro.getAngle() < 1)
|
|
{
|
|
actual_state = IO::ext_controls.isSelectedColorBlue() ? State::Turn90Blue : State::Turn90Yellow;
|
|
}
|
|
break;
|
|
|
|
case State::Turn90Blue:
|
|
IO::motors.turnLeft();
|
|
|
|
if(IO::gyro.getAngle() > 90)
|
|
{
|
|
actual_state = State::ForwardToSceneEdge;
|
|
}
|
|
break;
|
|
|
|
case State::Turn90Yellow:
|
|
IO::motors.turnRight();
|
|
|
|
if(IO::gyro.getAngle() > 90)
|
|
{
|
|
actual_state = State::ForwardToSceneEdge;
|
|
}
|
|
break;
|
|
|
|
case State::ForwardToSceneEdge:
|
|
IO::motors.forward();
|
|
|
|
if(IO::gyro.tapDetected())
|
|
{
|
|
actual_state = State::Dancing;
|
|
}
|
|
break;
|
|
|
|
case State::Dancing:
|
|
IO::ext_controls.startSpecialAction();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif
|