+++ /dev/null
-#define FSM_MAIN
-#include "robodata.h"
-#include <robot.h>
-#include <fsm.h>
-#include <unistd.h>
-#include <math.h>
-#include <movehelper.h>
-#include <map.h>
-#include <sharp.h>
-#include <robomath.h>
-#include <string.h>
-#include <robodim.h>
-#include <error.h>
-#include "actuators.h"
-#include <sharp.h>
-#include <trgen.h>
-#include "match-timing.h"
-#include <stdbool.h>
-#include <ul_log.h>
-#include "common-states.h"
-
-UL_LOG_CUST(ulogd_strategy_homologation); /* Log domain name = ulogd + name of the file */
-
-
-FSM_STATE(homologation_wait_for_start)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- start_entry();
-//#ifdef COMPETITION
- ul_logmsg("waiting for start\n");
- FSM_TIMER(1000);
- break;
-//#endif
- case EV_START:
- start_go();
- robot_calibrate_odometry();
- FSM_TRANSITION(homologation_approach_buillon);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- start_timer();
- break;
- case EV_EXIT:
- start_exit();
- break;
- case EV_SWITCH_STRATEGY:
- FSM_TRANSITION(calibrate);
- break;
- case EV_RETURN:
- break;
- default:;
- }
-}
-
-FSM_STATE(homologation_approach_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- //robot.use_back_bumpers = false;
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.65,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
- robot_trajectory_add_point_trans(
- 0.65,
- 1.3);
- robot_trajectory_add_final_point_trans(
- 0.5,
- 1.1,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- case EV_TIMER:
- //robot.use_back_bumpers = true;
- FSM_TRANSITION(homologation_push_bottle);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(homologation_push_bottle)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.64,
- 0.7);
- robot_trajectory_add_final_point_trans(
- 0.64 + 0.08,
- ROBOT_AXIS_TO_FRONT_M + 0.08,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- break;
- default:
- break;
- }
-}
\ No newline at end of file