Added a timeout for PAMI to be far away from bordure
This commit is contained in:
parent
48e22faf62
commit
88da5981b4
300
Code/src/main.c
300
Code/src/main.c
|
|
@ -1,145 +1,155 @@
|
||||||
#include <pico/stdlib.h>
|
#include <pico/stdlib.h>
|
||||||
#include <stdio.h>
|
#include <stdio.h>
|
||||||
#include "include/io.h"
|
#include "include/io.h"
|
||||||
|
|
||||||
#define NORMAL_SPEED 2000.0f
|
#define NORMAL_SPEED 2000.0f
|
||||||
#define TURNING_SPEED 1300.0f
|
#define TURNING_SPEED 1300.0f
|
||||||
#define ON_STAGE_SPEED 1100.0f
|
#define ON_STAGE_SPEED 1500.0f
|
||||||
|
|
||||||
#define RAMPE_ANGLE -7.82907651006f
|
#define RAMPE_ANGLE -7.82907651006f
|
||||||
#define RAMPE_ANGLE_OFFSET 1.0f
|
#define RAMPE_ANGLE_OFFSET 1.0f
|
||||||
#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
|
#define RAMPE_START_ANGLE (RAMPE_ANGLE / 2 - RAMPE_ANGLE_OFFSET)
|
||||||
#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
|
#define RAMPE_END_ANGLE (RAMPE_ANGLE / 2 + RAMPE_ANGLE_OFFSET)
|
||||||
|
|
||||||
// Unit tests activation
|
// Unit tests activation
|
||||||
//#define UNIT_TESTS
|
//#define UNIT_TESTS
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Etapes :
|
Etapes :
|
||||||
- 1ere etape : avancer jusqu'a la montée / condition : si montée detecté : etape suivante
|
- 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
|
- 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
|
- 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
|
- 4eme etape : avancer jusqu'au bors du plateau / condition : si choc de fin detecté : etape suivante
|
||||||
- 5eme etape : faire tourner actionneurs pour figurine
|
- 5eme etape : faire tourner actionneurs pour figurine
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Modules necessaires :
|
Modules necessaires :
|
||||||
- angle // Asservissement et control du robot
|
- angle // Asservissement et control du robot
|
||||||
- choc // Controle du robot (fin !)
|
- choc // Controle du robot (fin !)
|
||||||
(?)- accelerometre // Asservissement
|
(?)- accelerometre // Asservissement
|
||||||
*/
|
*/
|
||||||
|
|
||||||
// Movement states
|
// Movement states
|
||||||
typedef enum state_t {
|
typedef enum state_t {
|
||||||
WaitingForTirette,
|
WaitingForTirette,
|
||||||
WaitingTimer,
|
WaitingTimer,
|
||||||
ForwardToRamp,
|
ForwardToRamp,
|
||||||
ForwardToScene,
|
ForwardToScene,
|
||||||
Turn90Blue, // If blue team
|
SceneTimeout,
|
||||||
Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
|
Turn90Blue, // If blue team
|
||||||
ForwardToSceneEdge,
|
Turn90Yellow, // If yellow team => this is the only action where you have to do something different depending on your team
|
||||||
Dancing
|
ForwardToSceneEdge,
|
||||||
} state_t;
|
Dancing
|
||||||
|
} state_t;
|
||||||
io_t io;
|
|
||||||
|
io_t io;
|
||||||
void main(void)
|
|
||||||
{
|
void main(void)
|
||||||
stdio_init_all();
|
{
|
||||||
|
stdio_init_all();
|
||||||
// Initialise IO
|
|
||||||
init_io();
|
// Initialise IO
|
||||||
|
init_io();
|
||||||
motor_control_activated(true);
|
|
||||||
|
motor_control_activated(true);
|
||||||
io.target_dir = 0.0f;
|
|
||||||
io.target_speed = 0.0f;
|
io.target_dir = 0.0f;
|
||||||
//my_IO.startDancingAction(15);
|
io.target_speed = 0.0f;
|
||||||
|
//my_IO.startDancingAction(15);
|
||||||
#ifdef UNIT_TESTS
|
|
||||||
Serial.println("UNIT_TESTS");
|
#ifdef UNIT_TESTS
|
||||||
#endif
|
Serial.println("UNIT_TESTS");
|
||||||
|
#endif
|
||||||
printf("Rampe Angle : %f\n", RAMPE_ANGLE);
|
|
||||||
printf("Rampe Angle Offset : %f\n", RAMPE_ANGLE_OFFSET);
|
printf("Rampe Angle : %f\n", RAMPE_ANGLE);
|
||||||
printf("Rampe Start Angle : %f\n", RAMPE_START_ANGLE);
|
printf("Rampe Angle Offset : %f\n", RAMPE_ANGLE_OFFSET);
|
||||||
printf("Rampe End Angle : %f\n", RAMPE_END_ANGLE);
|
printf("Rampe Start Angle : %f\n", RAMPE_START_ANGLE);
|
||||||
|
printf("Rampe End Angle : %f\n", RAMPE_END_ANGLE);
|
||||||
state_t actual_state = WaitingForTirette;
|
|
||||||
|
state_t actual_state = WaitingForTirette;
|
||||||
while(true)
|
|
||||||
{
|
while(true)
|
||||||
update_io();
|
{
|
||||||
|
update_io();
|
||||||
switch(actual_state)
|
|
||||||
{
|
switch(actual_state)
|
||||||
case WaitingForTirette:
|
{
|
||||||
if(io.is_tirette_pulled)
|
case WaitingForTirette:
|
||||||
{
|
if(io.is_tirette_pulled)
|
||||||
actual_state = WaitingTimer;
|
{
|
||||||
}
|
actual_state = WaitingTimer;
|
||||||
break;
|
}
|
||||||
|
break;
|
||||||
case WaitingTimer:
|
|
||||||
unsigned long initial_time = io.time_ms;
|
case WaitingTimer:
|
||||||
|
unsigned long initial_time = io.time_ms;
|
||||||
if(io.time_ms - initial_time >= 87000)
|
|
||||||
{
|
if(io.time_ms - initial_time >= 87000)
|
||||||
actual_state = ForwardToRamp;
|
{
|
||||||
}
|
actual_state = ForwardToRamp;
|
||||||
break;
|
}
|
||||||
|
break;
|
||||||
case ForwardToRamp:
|
|
||||||
motor_control_activated(true);
|
case ForwardToRamp:
|
||||||
io.target_speed = NORMAL_SPEED;
|
motor_control_activated(true);
|
||||||
|
io.target_speed = NORMAL_SPEED;
|
||||||
if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
|
|
||||||
{
|
if(io.gyro_data.y_angle < RAMPE_START_ANGLE)
|
||||||
actual_state = ForwardToScene;
|
{
|
||||||
}
|
actual_state = SceneTimeout;
|
||||||
break;
|
}
|
||||||
|
break;
|
||||||
case ForwardToScene:
|
|
||||||
if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
|
case SceneTimeout:
|
||||||
{
|
unsigned long initial_time = io.time_ms;
|
||||||
io.target_speed = TURNING_SPEED;
|
|
||||||
actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
|
if(io.time_ms - initial_time >= 500)
|
||||||
}
|
{
|
||||||
break;
|
actual_state = ForwardToScene;
|
||||||
|
}
|
||||||
case Turn90Blue:
|
break;
|
||||||
set_dir_with_angular_speed(90.0f);
|
|
||||||
|
case ForwardToScene:
|
||||||
if(io.gyro_data.z_angle >= 80.0f)
|
if(io.gyro_data.y_angle > RAMPE_END_ANGLE)
|
||||||
{
|
{
|
||||||
actual_state = ForwardToSceneEdge;
|
io.target_speed = TURNING_SPEED;
|
||||||
}
|
actual_state = io.is_color_blue ? Turn90Blue : Turn90Yellow;
|
||||||
break;
|
}
|
||||||
|
break;
|
||||||
case Turn90Yellow:
|
|
||||||
set_dir_with_angular_speed(-90.0f);
|
case Turn90Blue:
|
||||||
|
set_dir_with_angular_speed(90.0f);
|
||||||
if(io.gyro_data.z_angle <= -80.0f)
|
|
||||||
{
|
if(io.gyro_data.z_angle >= 80.0f)
|
||||||
actual_state = ForwardToSceneEdge;
|
{
|
||||||
}
|
actual_state = ForwardToSceneEdge;
|
||||||
break;
|
}
|
||||||
|
break;
|
||||||
case ForwardToSceneEdge:
|
|
||||||
io.target_speed = ON_STAGE_SPEED;
|
case Turn90Yellow:
|
||||||
|
set_dir_with_angular_speed(-90.0f);
|
||||||
if(io.gyro_data.y_angle > 3.0f)
|
|
||||||
{
|
if(io.gyro_data.z_angle <= -80.0f)
|
||||||
actual_state = Dancing;
|
{
|
||||||
}
|
actual_state = ForwardToSceneEdge;
|
||||||
break;
|
}
|
||||||
|
break;
|
||||||
case Dancing:
|
|
||||||
motor_control_activated(false);
|
case ForwardToSceneEdge:
|
||||||
io.target_speed = 0.0f;
|
io.target_speed = ON_STAGE_SPEED;
|
||||||
io.is_dancing = true;
|
|
||||||
break;
|
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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue