156 lines
4.1 KiB
C
156 lines
4.1 KiB
C
#include <pico/stdlib.h>
|
|
#include <stdio.h>
|
|
#include "include/io.h"
|
|
|
|
#define NORMAL_SPEED 2000.0f
|
|
#define TURNING_SPEED 1300.0f
|
|
#define ON_STAGE_SPEED 1500.0f
|
|
|
|
#define RAMPE_ANGLE -7.82907651006f
|
|
#define RAMPE_ANGLE_OFFSET 1.0f
|
|
#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
|
|
#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
|
|
|
|
// Unit tests activation
|
|
//#define UNIT_TESTS
|
|
|
|
/*
|
|
Etapes :
|
|
- 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
|
|
- 2eme etape : avancer 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
|
|
typedef enum state_t {
|
|
WaitingForTirette,
|
|
WaitingTimer,
|
|
ForwardToRamp,
|
|
ForwardToScene,
|
|
SceneTimeout,
|
|
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
|
|
} state_t;
|
|
|
|
io_t io;
|
|
|
|
void main(void)
|
|
{
|
|
stdio_init_all();
|
|
|
|
// Initialise IO
|
|
init_io();
|
|
|
|
motor_control_activated(true);
|
|
|
|
io.target_dir = 0.0f;
|
|
io.target_speed = 0.0f;
|
|
//my_IO.startDancingAction(15);
|
|
|
|
#ifdef UNIT_TESTS
|
|
Serial.println("UNIT_TESTS");
|
|
#endif
|
|
|
|
printf("Rampe Angle : %f\n", RAMPE_ANGLE);
|
|
printf("Rampe Angle Offset : %f\n", RAMPE_ANGLE_OFFSET);
|
|
printf("Rampe Start Angle : %f\n", RAMPE_START_ANGLE);
|
|
printf("Rampe End Angle : %f\n", RAMPE_END_ANGLE);
|
|
|
|
state_t actual_state = WaitingForTirette;
|
|
|
|
while(true)
|
|
{
|
|
update_io();
|
|
|
|
switch(actual_state)
|
|
{
|
|
case WaitingForTirette:
|
|
if(io.is_tirette_pulled)
|
|
{
|
|
actual_state = WaitingTimer;
|
|
}
|
|
break;
|
|
|
|
case WaitingTimer:
|
|
unsigned long initial_time = io.time_ms;
|
|
|
|
if(io.time_ms - initial_time >= 87000)
|
|
{
|
|
actual_state = ForwardToRamp;
|
|
}
|
|
break;
|
|
|
|
case ForwardToRamp:
|
|
motor_control_activated(true);
|
|
io.target_speed = NORMAL_SPEED;
|
|
|
|
if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
|
|
{
|
|
actual_state = SceneTimeout;
|
|
}
|
|
break;
|
|
|
|
case SceneTimeout:
|
|
unsigned long initial_time = io.time_ms;
|
|
|
|
if(io.time_ms - initial_time >= 500)
|
|
{
|
|
actual_state = ForwardToScene;
|
|
}
|
|
break;
|
|
|
|
case ForwardToScene:
|
|
if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
|
|
{
|
|
io.target_speed = TURNING_SPEED;
|
|
actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
|
|
}
|
|
break;
|
|
|
|
case Turn90Blue:
|
|
set_dir_with_angular_speed(90.0f);
|
|
|
|
if(io.gyro_data.z_angle >= 80.0f)
|
|
{
|
|
actual_state = ForwardToSceneEdge;
|
|
}
|
|
break;
|
|
|
|
case Turn90Yellow:
|
|
set_dir_with_angular_speed(-90.0f);
|
|
|
|
if(io.gyro_data.z_angle <= -80.0f)
|
|
{
|
|
actual_state = ForwardToSceneEdge;
|
|
}
|
|
break;
|
|
|
|
case ForwardToSceneEdge:
|
|
io.target_speed = ON_STAGE_SPEED;
|
|
|
|
if(io.gyro_data.y_angle > 3.0f)
|
|
{
|
|
actual_state = Dancing;
|
|
}
|
|
break;
|
|
|
|
case Dancing:
|
|
motor_control_activated(false);
|
|
io.target_speed = 0.0f;
|
|
io.is_dancing = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|