robofsm: mcl-laser test program.

diff --git a/src/robofsm/test/Makefile.omk b/src/robofsm/test/Makefile.omk
index 8b55dfd22c077654e704cdcea1d020311450d1a3..8be45497b9ca00503bec11c47883f7c94b0f968b 100644 (file)
--- a/src/robofsm/test/Makefile.omk
+++ b/src/robofsm/test/Makefile.omk
@@ -15,6 +15,9 @@ goto_SOURCES = goto.cc
 #test_PROGRAMS += display
 #display_SOURCES = display.cc
 
+test_PROGRAMS += mcl-laser
+mcl-laser_SOURCES = mcl-laser.cc
+
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot_eb2008 mcl laser-nav robomath uoled oledlib sercom roboorte_generic roboorte_eb2008 \
                robottype robottype_eb2008 pthread \

diff --git a/src/robofsm/test/mcl-laser.cc b/src/robofsm/test/mcl-laser.cc
new file mode 100644 (file)
index 0000000..a745664
--- /dev/null
+++ b/src/robofsm/test/mcl-laser.cc
@@ -0,0 +1,118 @@
+/*
+ * mcl-laser.cc                        08/04/27
+ *
+ * Movement test: move in a rectangle using laser and MCL for localization.
+ *
+ * Copyright: (c) 2008 CTU Dragons
+ *            CTU FEE - Department of Control Engineering
+ * License: GNU GPL v.2
+ */
+
+#define FSM_MAIN
+#include <movehelper_eb2008.h>
+#include <robot_eb2008.h>
+#include <trgen.h>
+#include <robomath.h>
+
+FSM_STATE_DECL(rectangle);
+FSM_STATE_DECL(turn);
+FSM_STATE_DECL(wait);
+
+FSM_STATE(main_init) {
+       /* Where we are at the begining? */
+       robot_set_act_pos_trans(1, 0.5, DEG2RAD(0));
+       FSM_TRANSITION(rectangle);
+}
+
+
+void follow_rectangle()
+{
+       static bool backward = false;
+       struct TrajectoryConstraints tc = trajectoryConstraintsDefault;
+       //tc.maxv *= 1.5;
+
+       /* Allocate new trajectory */
+       if (!backward)
+               robot_trajectory_new(&tc);
+       else
+               robot_trajectory_new_backward(&tc);
+       backward = !backward;
+
+       /* Add random point to the trajectory. */
+       robot_trajectory_add_point_trans(2, 0.5);
+       robot_trajectory_add_point_trans(2, 1.5);
+       robot_trajectory_add_point_trans(1, 1.5);
+       robot_trajectory_add_final_point_trans(1, 0.5, TURN(DEG2RAD(90)));
+}
+
+FSM_STATE(rectangle)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       follow_rectangle();
+                       break;
+               case EV_TRAJECTORY_ERROR:
+                       FSM_TRANSITION(turn);
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(turn);
+                       break;
+               default:
+                       break;
+       }
+}
+
+FSM_STATE(turn)
+{
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_trajectory_new(NULL);
+                       robot_trajectory_add_final_point_trans(1, 0.5, 
+                                               TURN_CW(DEG2RAD(-90)));
+                       break;
+               case EV_TRAJECTORY_ERROR:
+                       FSM_TRANSITION(wait);
+                       break;
+               case EV_MOTION_DONE:
+                       FSM_TRANSITION(wait);
+                       break;
+               default:
+                       break;
+       }
+}
+
+
+FSM_STATE(wait)
+{
+       FSM_TIMER(1000);
+       switch (FSM_EVENT) {
+               case EV_TIMER:
+                       FSM_TRANSITION(rectangle);
+                       break;
+               default:
+                       break;
+       }
+}
+
+int main()
+{
+       robot.use_loc = 1;
+
+       /* robot initialization */
+       robot_init();
+
+       FSM(MAIN)->debug_states = 1;
+       /*FSM(MOVE)->debug_states = 1;*/
+
+       robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
+       robot.fsm[FSM_ID_MOTION].state = &fsm_state_move_init;
+
+        /* start threads and wait */
+        robot_start();
+       robot_wait();
+
+       /* clean up */
+       robot_destroy();
+
+       return 0;
+}

diff --git a/src/robofsm/test/rectangle.cc b/src/robofsm/test/rectangle.cc
index b9e23609c906729d1a08c4cd8a59e9e39deaa7da..c9d1ceafea308104610e7e5f9ebc156a7c63b073 100644 (file)
--- a/src/robofsm/test/rectangle.cc
+++ b/src/robofsm/test/rectangle.cc
@@ -20,7 +20,7 @@ FSM_STATE_DECL(wait);
 
 FSM_STATE(main_init) {
        /* Where we are at the begining? */
-       robot_set_act_pos_trans(1, 0.5, DEG2RAD(30));
+       robot_set_act_pos_trans(1, 0.5, DEG2RAD(0));
        FSM_TRANSITION(rectangle);
 }