From: Petr Silhavik <silhavik.p@gmail.com>
Date: Sun, 17 Feb 2013 16:55:05 +0000 (+0100)
Subject: robofsm: MotionControl change functions to accept new interface
X-Git-Url: http://rtime.felk.cvut.cz/gitweb/eurobot/public.git/commitdiff_plain/c51fab7c9c678cce3644548ab055ef049ba9f255

robofsm: MotionControl change functions to accept new interface
---

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index cbe94bb1..e7854264 100644
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -34,6 +34,8 @@
 #include "robot_config.h"
 #include <robomath.h>
 #include <ul_log.h>
+#include "guard.hpp"
+#include "events.h"
 
 UL_LOG_CUST(ulogd_motion_control); /* Log domain name = ulogd + name of the file */
 
@@ -110,7 +112,7 @@ static void delete_actual_trajectory()
 	old = actual_trajectory;
 	actual_trajectory = NULL;
 	pthread_mutex_unlock(&actual_trajectory_lock);
-	robot_send_speed(0,0);
+	robot.move_helper.send_speed(0,0);
 	if (old) delete(old);
 }
 
@@ -123,16 +125,14 @@ static void notify_fsm(bool done, double error)
 	if (error > MAX_POS_ERROR_M) {
 		if (!lost_sent) {
 			lost_sent = true;
-			FSM_SIGNAL(MOTION, EV_TRAJECTORY_LOST, NULL);
+			robot.sched.queue_event(robot.MOTION, new evTrajectoryLost());
 		}
 	} else {
 		lost_sent = false;
 		if (done) {
-			if (error < CLOSE_TO_TARGET_M) {
-    				FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE_AND_CLOSE, NULL);
-			} else if (!done_sent) {
+			if (error < CLOSE_TO_TARGET_M || !done_sent) {
+				robot.sched.queue_event(robot.MOTION, new evTrajectoryDone());
 				done_sent = true;
-				FSM_SIGNAL(MOTION, EV_TRAJECTORY_DONE, NULL);
 			}
 		} else {
 			done_sent = false;
@@ -166,7 +166,7 @@ static void check_for_collision_in_future(Trajectory *traj, double current_time)
 			continue;
 		if (map->cells[ycell][xcell].detected_obstacle > 0) {
 			if (sem_trywait(&recalculation_not_running) == 0) {
-				FSM_SIGNAL(MOTION, EV_OBSTACLE, NULL);
+				robot.sched.queue_event(robot.MOTION, new evObstacle());
 				break;
 			}
 		}
@@ -221,13 +221,14 @@ static void do_control()
 		if (ref_pos.omega > actual_trajectory->constr.maxomega)
 			DBG("Omega constraint problem %lf, max %lf -------------------- \n", ref_pos.omega, actual_trajectory->constr.maxomega);
 
-		ROBOT_LOCK(ref_pos);
-		robot.ref_pos.x = ref_pos.x;
-		robot.ref_pos.y = ref_pos.y;
-		robot.ref_pos.phi = ref_pos.phi;
-		ROBOT_UNLOCK(ref_pos);
+		{
+			Guard g(robot.lock_ref_pos);
+			robot.ref_pos.x = ref_pos.x;
+			robot.ref_pos.y = ref_pos.y;
+			robot.ref_pos.phi = ref_pos.phi;
+		}
 
-		robot_get_est_pos(&est_pos.x, &est_pos.y, &est_pos.phi);
+		robot.get_est_pos(est_pos.x, est_pos.y, est_pos.phi);
 
 #ifdef MOTION_PRINT_REF
 		static double last_t;
@@ -250,7 +251,7 @@ static void do_control()
 
 
 	// Apply controller output
-	robot_send_speed(speedl, speedr);
+	robot.move_helper.send_speed(speedl, speedr);
 	pthread_mutex_unlock(&actual_trajectory_lock);
 }
 
@@ -429,7 +430,7 @@ void get_future_pos(double rel_time_sec, Pos &pos, double &switch_time)
 		// Robot doesn't move, so return current position
 		pthread_mutex_unlock(&actual_trajectory_lock);
 
-		robot_get_est_pos(&pos.x, &pos.y, &pos.phi);
+		robot.get_est_pos(pos.x, pos.y, pos.phi);
 		pos.v = 0;
 		pos.omega = 0;
 	}
diff --git a/src/robofsm/motion-control.h b/src/robofsm/motion-control.h
index 0f029e9c..ee6536b9 100644
--- a/src/robofsm/motion-control.h
+++ b/src/robofsm/motion-control.h
@@ -9,9 +9,6 @@
 #ifndef MOTION_CONTROL_H
 #define MOTION_CONTROL_H
 
-#if !defined(FSM_MOTION) && !defined(MOTION_CONTROL) && !defined(MOTION_CONTROL_INIT_ONLY)
-#error This file should only be included from fsmmove.cc
-#endif
 
 #ifdef __cplusplus
 extern "C" {