From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Fri, 2 May 2008 21:08:25 +0000 (+0200)
Subject: Added sharpcalib - a program for calibration of sharp sensors
X-Git-Tag: eb2008~31^2^2~1
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/22ffecc726080775122831503af5c3260a17dee6

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 8bdf969a..1c19a186 100644
--- 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 750309b5..8bd71072 100644
--- 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 0a6573ec..cd6a136f 100644
--- 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
index 00000000..7dca94b3
--- /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;
+}