-/*
+ /*
* competition.cc 12/02/28
*
* Robot's control program intended for homologation (approval phase) on Eurobot 2012.
#include <trgen.h>
#include "match-timing.h"
#include <ul_log.h>
+#include "common-states.h"
UL_LOG_CUST(ulogd_homologation); /* Log domain name = ulogd + name of the file */
* Trajectory constraints used, are initialized in the init state
************************************************************************/
-struct TrajectoryConstraints tcFast, tcSlow, tcVerySlow;
-
-/************************************************************************
- * FSM STATES DECLARATION
- ************************************************************************/
-
-FSM_STATE_DECL(init);
-FSM_STATE_DECL(wait_for_start);
-/* from homologation */
-/* movement states - buillon */
-FSM_STATE_DECL(aproach_buillon);
-/* Pushing the bottle */
-FSM_STATE_DECL(push_bottle);
-/* New states */
-FSM_STATE_DECL(reach_central_buillon);
-FSM_STATE_DECL(leave_central_buillon);
-FSM_STATE_DECL(push_bottle_bw);
-FSM_STATE_DECL(return_home);
-
-FSM_STATE(init)
-{
- switch (FSM_EVENT) {
- case EV_ENTRY:
- tcSlow = trajectoryConstraintsDefault;
- tcSlow.maxv = 0.3;
- tcSlow.maxacc = 0.3;
- tcSlow.maxomega = 1;
- FSM_TRANSITION(wait_for_start);
- break;
- default:
- break;
- }
-}
-
-void set_initial_position()
-{
- // TODO define initial position
- robot_set_est_pos_trans(ROBOT_AXIS_TO_BACK_M,
- PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0),
- 0);
-}
-
-void actuators_home()
-{
- //act_jaws(CLOSE);
- // drive lift to home position
- //act_lift(0, 0, 1);
- // unset the homing request
- //act_lift(0, 0, 0);
-}
-
-FSM_STATE(wait_for_start)
-{
- pthread_t thid;
- #ifdef WAIT_FOR_START
- ul_logdeb("WAIT_FOR_START mode set\n");
- #else
- ul_logdeb("WAIT_FOR_START mode NOT set\n");
- #endif
- #ifdef COMPETITION
- ul_logdeb("COMPETITION mode set\n");
- #else
- ul_logdeb("COMPETITION mode NOT set\n");
- #endif
- switch (FSM_EVENT) {
- case EV_ENTRY:
- pthread_create(&thid, NULL, timing_thread, NULL);
-#ifdef WAIT_FOR_START
- FSM_TIMER(1000);
- break;
-#endif
- case EV_START:
- /* start competition timer */
- sem_post(&robot.start);
- actuators_home();
- set_initial_position();
- FSM_TRANSITION(aproach_buillon);
- break;
- case EV_TIMER:
- FSM_TIMER(1000);
- // We set initial position periodically in
- // order for it to be updated on the display
- // if the team color is changed during waiting
- // for start.
- set_initial_position();
- if (robot.start_state == START_PLUGGED_IN)
- actuators_home();
- break;
- case EV_RETURN:
- case EV_MOTION_ERROR:
- case EV_MOTION_DONE:
- //case EV_VIDLE_DONE:
- case EV_SWITCH_STRATEGY:
- DBG_PRINT_EVENT("unhandled event");
- break;
- case EV_EXIT:
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(aproach_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- 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:
- FSM_TRANSITION(reach_central_buillon);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(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.05,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(reach_central_buillon);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(reach_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.64,
- 0.7);
- robot_trajectory_add_point_trans(
- 1.0,
- 0.45);
- robot_trajectory_add_final_point_trans(
- 1.3,
- 0.58,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(leave_central_buillon);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(leave_central_buillon)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_final_point_trans(
- 1.0,
- 0.45,
- NO_TURN());
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(push_bottle_bw);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(push_bottle_bw)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new_backward(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.7,
- 0.3);
- robot_trajectory_add_final_point_trans(
- 0.64+0.08,
- ROBOT_AXIS_TO_BACK_M + 0.05,
- ARRIVE_FROM(DEG2RAD(270), 0.10));
- break;
- case EV_MOTION_DONE:
- FSM_TRANSITION(return_home);
- break;
- default:
- break;
- }
-}
-
-FSM_STATE(return_home)
-{
- switch(FSM_EVENT) {
- case EV_ENTRY:
- robot_trajectory_new(&tcSlow); // new trajectory
- robot_trajectory_add_point_trans(
- 0.64 + 0.08,
- 0.7);
- robot_trajectory_add_final_point_trans(
- 0.4,
- 1.0,
- ARRIVE_FROM(DEG2RAD(180), 0.10));
- break;
- case EV_MOTION_DONE:
- break;
- default:
- break;
- }
-}
/************************************************************************
* MAIN FUNCTION
************************************************************************/
robot.fsm.motion.debug_states = 1;
//robot.fsm.act.debug_states = 1;
- robot.fsm.main.state = &fsm_state_main_init;
+ robot.fsm.main.state = &fsm_state_main_get_central_buillon_first;
//robot.fsm.main.transition_callback = trans_callback;
//robot.fsm.motion.transition_callback = move_trans_callback;
+ tcVeryFast = trajectoryConstraintsDefault;
+ tcVeryFast.maxv = 1;
+ tcVeryFast.maxacc = 0.6;
+ tcVeryFast.maxomega = 2;
+ tcFast = trajectoryConstraintsDefault;
+ tcFast.maxv = 0.6;
+ tcFast.maxacc = 0.2;
+ tcFast.maxomega = 1;
+ tcFast.maxe = 0.02;
+ tcSlow = trajectoryConstraintsDefault;
+ tcSlow.maxv = 0.4;
+ tcSlow.maxacc = 0.2;
+ tcSlow.maxomega = 1;
+ tcSlow.maxe = 0.02;
+ tcVerySlow = trajectoryConstraintsDefault;
+ tcVerySlow.maxv = 0.1;
+ tcVerySlow.maxacc = 0.1;
+ tcVerySlow.maxomega = 0.2;
+ tcVerySlow.maxe = 0.02;
rv = robot_start();
if (rv) error(1, errno, "robot_start() returned %d\n", rv);
projects / eurobot / public.git / blobdiff