Added sharpcalib - a program for calibration of sharp sensors

diff --git a/src/robofsm/eb2008/robot_eb2008.h b/src/robofsm/eb2008/robot_eb2008.h
index 8bdf969a6100d4cc1391e5d21b727a5a07a294bc..1c19a1861bb18b56d3f26763bc1da04e554c1f1c 100644 (file)
--- a/src/robofsm/eb2008/robot_eb2008.h
+++ b/src/robofsm/eb2008/robot_eb2008.h
@@ -114,7 +114,6 @@ extern struct lock_log robot_lock_log;
 //#define __robot_lock_ lock   /* ROBOT_LOCK() */
 #define __robot_lock_ref_pos           lock_ref_pos
 #define __robot_lock_est_pos           lock_est_pos
-#define __robot_lock_des_pos           lock
 #define __robot_lock_joy_data          lock_joy_data
 #define __robot_lock_meas_angles       lock_meas_angles
 #define __robot_lock_drives            lock

diff --git a/src/robofsm/eb2008/robot_orte.c b/src/robofsm/eb2008/robot_orte.c
index 750309b5e980443038d2290d5794f174eea72cbe..8bd71072fd78a751b123439783a2d298aba512d1 100644 (file)
--- a/src/robofsm/eb2008/robot_orte.c
+++ b/src/robofsm/eb2008/robot_orte.c
@@ -309,6 +309,10 @@ void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
 
        switch (info->status) {
                case NEW_DATA: {
+                       ROBOT_LOCK(sharps);
+                       robot.sharps = *instance;
+                       ROBOT_UNLOCK(sharps);
+
                        update_map(instance);
                        break;
                }

diff --git a/src/robofsm/eb2008/test/Makefile.omk b/src/robofsm/eb2008/test/Makefile.omk
index 0a6573ecc1a5e2d3c17299a050e6b1f0486b4d9a..cd6a136f67bebec4bdaaf296c43dafa8397f6886 100644 (file)
--- a/src/robofsm/eb2008/test/Makefile.omk
+++ b/src/robofsm/eb2008/test/Makefile.omk
@@ -21,6 +21,9 @@ mcl-laser_SOURCES = mcl-laser.cc
 test_PROGRAMS += homologation
 homologation_SOURCES = homologation.cc
 
+test_PROGRAMS += sharpcalib
+sharpcalib_SOURCES = sharpcalib.cc
+
 # Libraries linked to all programs in this Makefile
 lib_LOADLIBES = robot_eb2008 mcl robodim_eb2008 laser-nav robomath uoled oledlib sercom roboorte_generic roboorte_eb2008 \
                robottype robottype_eb2008 pthread \

diff --git a/src/robofsm/eb2008/test/sharpcalib.cc b/src/robofsm/eb2008/test/sharpcalib.cc
new file mode 100644 (file)
index 0000000..7dca94b
--- /dev/null
+++ b/src/robofsm/eb2008/test/sharpcalib.cc
@@ -0,0 +1,45 @@
+
+#define FSM_MAIN
+#include <robot_eb2008.h>
+#include <movehelper_eb2008.h>
+#include <trgen.h>
+#include <robomath.h>
+
+FSM_STATE(init)
+{
+       static int cm = 100;
+       switch (FSM_EVENT) {
+               case EV_ENTRY:
+                       robot_set_est_pos_trans(1, 0.5, DEG2RAD(180));
+                       FSM_TIMER(1000);
+                       break;
+               case EV_TIMER:
+               case EV_MOTION_DONE:
+                       printf("%3d cm: sharp left: %5.3f right: %5.3f\n", cm, robot.sharps.front_left, robot.sharps.front_right);
+                       robot_move_by(0.01, NO_TURN(), NULL);
+                       cm--;
+                       if (cm == 0)
+                               robot_exit();
+                       break;
+               default:
+                       break;
+       }
+}
+
+
+int main()
+{
+       /* robot initialization */
+       robot_init();
+
+       robot.fsm[FSM_ID_MAIN].state = &fsm_state_main_init;
+
+        /* Start threads and wait */
+        robot_start();
+       robot_wait();
+
+       /* clean up */
+       robot_destroy();
+
+       return 0;
+}