robofsm: Competition 2012

Simple competition state machine. Go to the nearest totem and go back on the boat.

diff --git a/src/robofsm/competition2012.cc b/src/robofsm/competition2012.cc
new file mode 100644 (file)
index 0000000..a48f94b
--- /dev/null
+++ b/src/robofsm/competition2012.cc
@@ -0,0 +1,342 @@
+/*
+ * competition.cc       12/02/28
+ *
+ * Robot's control program intended for homologation (approval phase) on Eurobot 2012.
+ *
+ * Copyright: (c) 2012 CTU Flamingos
+ *            CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#define FSM_MAIN
+#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 <trgen.h>
+#include "match-timing.h"
+#include <ul_log.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);
+/* movement states - buillon */
+FSM_STATE_DECL(aproach_buillon);
+FSM_STATE_DECL(place_buillon);
+FSM_STATE_DECL(leave_buillon);
+/* Pushing the bottle */
+FSM_STATE_DECL(push_bottle);
+FSM_STATE_DECL(leave_bottle);
+FSM_STATE_DECL(goto_totem);
+FSM_STATE_DECL(approach_totem);
+FSM_STATE_DECL(leave_totem);
+FSM_STATE_DECL(go_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.62,
+                               PLAYGROUND_HEIGHT_M - (ROBOT_WIDTH_M/2.0));
+                       robot_trajectory_add_final_point_trans(
+                               0.72,
+                               1.14,
+                               TURN_CW(DEG2RAD(180)));
+                       break;
+               case EV_MOTION_DONE:
+               case EV_TIMER:
+                       FSM_TRANSITION(place_buillon);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(place_buillon)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow);
+                       robot_trajectory_add_final_point_trans(
+                               ROBOT_AXIS_TO_FRONT_M + ROBOT_JAWS_LENGHT_M,
+                               1.14,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+               case EV_TIMER:
+                       FSM_TRANSITION(leave_buillon);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_buillon)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               0.64,
+                               1.14,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+               case EV_TIMER:
+                       FSM_TRANSITION(push_bottle);
+                       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.9);
+                       robot_trajectory_add_final_point_trans(
+                               0.64,
+                               ROBOT_AXIS_TO_FRONT_M + 0.05,
+                               ARRIVE_FROM(DEG2RAD(270), 0.10));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(leave_bottle);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_bottle)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               0.64,
+                               0.4,
+                               TURN_CCW(DEG2RAD(180)));
+                       break;
+               case EV_MOTION_DONE:
+               case EV_TIMER:
+                       FSM_TRANSITION(goto_totem);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(goto_totem)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               0.4,
+                               TURN_CCW(DEG2RAD(90)));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(approach_totem);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(approach_totem)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               0.875 - ROBOT_AXIS_TO_FRONT_M - 0.05,
+                               ARRIVE_FROM(DEG2RAD(90), 0.10));
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(leave_totem);
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(leave_totem)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new_backward(&tcSlow); // new trajectory
+                       robot_trajectory_add_final_point_trans(
+                               1.1,
+                               0.4,
+                               NO_TURN());
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(go_home);
+               default:
+                       break;
+       }
+}
+
+
+FSM_STATE(go_home)
+{
+       switch(FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(&tcSlow); // new trajectory
+                       robot_trajectory_add_point_trans(
+                               0.64,
+                               0.6);
+                       robot_trajectory_add_point_trans(
+                               0.64,
+                               1.0);
+                       robot_trajectory_add_final_point_trans(
+                               0.35,
+                               1.0,
+                               ARRIVE_FROM(DEG2RAD(180),0.1));
+                       break;
+               case EV_MOTION_DONE:
+               default:
+                       break;
+       }
+}
+
+// totem 1100 x 1000 polovička totemu 125
+// druhá láhev na 1883
+
+/************************************************************************
+ * MAIN FUNCTION
+ ************************************************************************/
+
+int main()
+{
+       int rv;
+
+       rv = robot_init();
+       if (rv) error(1, errno, "robot_init() returned %d\n", rv);
+
+       robot.obstacle_avoidance_enabled = true;
+
+       robot.fsm.main.debug_states = 1;
+       robot.fsm.motion.debug_states = 1;
+       //robot.fsm.act.debug_states = 1;
+
+       robot.fsm.main.state = &fsm_state_main_init;
+       //robot.fsm.main.transition_callback = trans_callback;
+       //robot.fsm.motion.transition_callback = move_trans_callback;
+
+        rv = robot_start();
+       if (rv) error(1, errno, "robot_start() returned %d\n", rv);
+
+       robot_destroy();
+
+       return 0;
+} 
\ No newline at end of file