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 <stdio.h>
|
||||
#include "include/io.h"
|
||||
|
||||
#define NORMAL_SPEED 2000.0f
|
||||
#define TURNING_SPEED 1300.0f
|
||||
#define ON_STAGE_SPEED 1100.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,
|
||||
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 = 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
#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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue