2 * robot_orte.c 08/04/21
6 * Copyright: (c) 2008 CTU Dragons
7 * CTU FEE - Department of Control Engineering
12 #include <roboorte_robottype.h>
16 #include <movehelper.h>
19 #include <laser-nav.h>
20 #include "map_handling.h"
23 #include <can_msg_def.h>
26 #define DBG(format, ...) printf(format, ##__VA_ARGS__)
28 #define DBG(format, ...)
32 extern sem_t measurement_received;
34 /* ----------------------------------------------------------------------
35 * PUBLISHER CALLBACKS - GENERIC
36 * ---------------------------------------------------------------------- */
38 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
39 void *sendCallBackParam)
41 struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
44 *instance = robot.ref_pos;
45 ROBOT_UNLOCK(ref_pos);
48 void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
49 void *sendCallBackParam)
51 struct est_pos_type *instance = (struct est_pos_type *)vinstance;
54 *instance = robot.est_pos;
55 ROBOT_UNLOCK(est_pos);
58 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
59 void *sendCallBackParam)
63 /* ----------------------------------------------------------------------
64 * SUBSCRIBER CALLBACKS - GENERIC
65 * ---------------------------------------------------------------------- */
67 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
68 void *recvCallBackParam)
70 struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
72 switch (info->status) {
74 /* locking should not be needed, but... */
75 ROBOT_LOCK(motion_irc);
76 robot.motion_irc = *instance;
77 robot.motion_irc_received = 1;
78 ROBOT_UNLOCK(motion_irc);
80 /* FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo); */
81 robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
84 robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
85 DBG("ORTE deadline occurred - motion_irc receive\n");
90 void rcv_corr_distances_cb(const ORTERecvInfo *info, void *vinstance,
91 void *recvCallBackParam)
93 struct corr_distances_type *instance =
94 (struct corr_distances_type *)vinstance;
96 switch (info->status) {
98 /* locking should not be needed, but... */
99 ROBOT_LOCK(corr_distances);
100 robot.corr_distances = *instance;
101 ROBOT_UNLOCK(corr_distances);
103 /* wake up motion-control/thread_trajectory_follower */
104 sem_post(&measurement_received);
106 /*FIXME: is following OK? (pecam1) */
107 robot.hw_status[STATUS_UZV] = HW_STATUS_OK;
110 robot.hw_status[STATUS_UZV] = HW_STATUS_FAILED;
116 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
117 void *recvCallBackParam)
119 switch (info->status) {
123 DBG("ORTE deadline occurred - motion_speed receive\n");
128 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
129 void *recvCallBackParam)
131 switch (info->status) {
135 DBG("ORTE deadline occurred - motion_status receive\n");
140 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
141 void *recvCallBackParam)
143 switch (info->status) {
145 robot.hw_status[STATUS_PWR]=HW_STATUS_OK;
148 robot.hw_status[STATUS_PWR]=HW_STATUS_FAILED;
149 DBG("ORTE deadline occurred - pwr_voltage receive\n");
154 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
155 void *recvCallBackParam)
157 switch (info->status) {
161 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
166 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
167 void *recvCallBackParam)
169 struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
170 static struct robot_cmd_type last_instance;
172 switch (info->status) {
174 /* Stupid way of controlling the robot by
175 * pluggin in and out start connector. */
176 switch (robot.state) {
178 if (instance->start) {
179 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
181 robot.hw_status[STATUS_STA] = HW_STATUS_OK;
183 if (!instance->start) {
184 robot.state = START_PLUGGED_IN;
185 printf("START_PLUGGED_IN\n");
189 case START_PLUGGED_IN:
190 robot.hw_status[STATUS_STA] = HW_STATUS_OK;
191 /* Competition starts when plugged out */
192 if (instance->start) {
193 FSM_SIGNAL(MAIN, EV_START, NULL);
194 robot.state = COMPETITION_STARTED;
199 case COMPETITION_STARTED: {
200 /* Subsequent plug-in stops the robot */
202 if (!instance->start) {
203 robot.hw_status[STATUS_STA] = HW_STATUS_WARNING;
210 last_instance = *instance;
213 robot.hw_status[STATUS_STA] = HW_STATUS_FAILED;
214 DBG("ORTE deadline occurred - robot_cmd receive\n");
219 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
220 void *recvCallBackParam)
222 struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
224 switch (info->status) {
227 robot.hokuyo = *instance;
228 robot.hw_status[STATUS_HOK] = HW_STATUS_OK;
229 ROBOT_UNLOCK(hokuyo);
230 update_map_hokuyo(instance);
234 robot.hw_status[STATUS_HOK] = HW_STATUS_FAILED;
235 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
240 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
241 void *recvCallBackParam)
243 struct camera_result_type *instance = (struct camera_result_type *)vinstance;
245 switch (info->status) {
247 ROBOT_LOCK(camera_result);
248 robot.game_conf = instance->lot;
249 ROBOT_UNLOCK(camera_result);
250 robot.hw_status[STATUS_CAM] = HW_STATUS_OK;
254 robot.hw_status[STATUS_CAM] = HW_STATUS_FAILED;
255 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
259 /* ----------------------------------------------------------------------
260 * SUBSCRIBER CALLBACKS - EB2008
261 * ---------------------------------------------------------------------- */
263 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
264 void *recvCallBackParam)
266 static unsigned short old_lift_pos=0;
267 static unsigned short old_pusher_pos=0;
268 switch (info->status) {
270 // new data arrived and requested position equals actual position
271 if (robot.orte.actuator_status.lift_pos == robot.orte.lift.pos &&
272 robot.orte.actuator_status.lift_pos != old_lift_pos)
273 FSM_SIGNAL(ACT, EV_LIFT_IN_POS, NULL);
274 if (robot.orte.actuator_status.pusher_pos == robot.orte.pusher.pos &&
275 robot.orte.actuator_status.pusher_pos != old_pusher_pos)
276 FSM_SIGNAL(ACT, EV_PUSHER_IN_POS, NULL);
277 if (robot.orte.actuator_status.status & ~LIFT_ENDSW_MASK) {
278 robot.hw_status[STATUS_LFT]=HW_STATUS_WARNING;
280 robot.hw_status[STATUS_LFT]=HW_STATUS_OK;
282 old_lift_pos = robot.orte.actuator_status.lift_pos;
283 old_pusher_pos = robot.orte.actuator_status.pusher_pos;
286 robot.hw_status[STATUS_LFT]=HW_STATUS_FAILED;
287 DBG("ORTE deadline occurred - actuator_status receive\n");
292 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
293 void *recvCallBackParam)
295 switch (info->status) {
297 double old_distance_value = robot.puck_distance;
300 robot.puck_distance = robot.orte.puck_distance.distance;
301 ROBOT_UNLOCK(sharps);
303 if (PUCK_INSIDE(robot.orte.puck_distance.distance) && !PUCK_INSIDE(old_distance_value)) {
304 // signal puck is inside
305 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
307 if (PUCK_REACHABLE(robot.orte.puck_distance.distance) && !PUCK_REACHABLE(old_distance_value)) {
308 // signal puck is reachable
309 FSM_SIGNAL(MAIN, EV_PUCK_REACHABLE, NULL);
314 // FIXME F.J. (?) inform display in case of invalid data?
315 // FIXME F.J. (?) limited space on the display...
316 //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
317 DBG("ORTE deadline occurred - servos receive\n");
322 void rcv_puck_inside_cb(const ORTERecvInfo *info, void *vinstance,
323 void *recvCallBackParam)
325 switch (info->status) {
327 if (robot.use_back_switch) {
328 static bool previous_switch_status = 0;
329 if (robot.orte.puck_inside.status & 0x02) {
330 //FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
332 /* React on the first switch or when moving backward. */
333 if ((previous_switch_status & 0x02) == 0 ||
334 robot.moves_backward) {
335 FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
337 previous_switch_status = robot.orte.puck_inside.status;
341 static bool previous = false;
342 bool switched = ((robot.orte.puck_inside.status & 0x01) != 0);
344 if (switched && !previous) {
345 FSM_SIGNAL(ACT, EV_PUCK_INSIDE, NULL);
350 robot.hw_status[STATUS_CHE]=HW_STATUS_OK;
354 robot.hw_status[STATUS_CHE]=HW_STATUS_FAILED;
355 DBG("ORTE deadline occurred - servos receive\n");
362 static int cmp_double(const void *v1, const void *v2)
364 const double *d1 = v1, *const d2 = v2;
374 int robot_init_orte()
378 robot.orte.strength = 20;
380 rv = robottype_roboorte_init(&robot.orte);
384 /* creating publishers */
385 robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
386 robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
387 robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
388 //???robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
390 // I didn't know what was the difference between the callback function pointer
391 // being NULL and being set to pointer to empty send_dummy_cb function. The DIFFERENCE
393 // - NULL: message is published only when OrtePublicationSend called
394 // - pointer to empty function: message is published periodically
395 robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
396 robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
397 robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
398 robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
399 robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
400 robottype_publisher_fsm_main_create(&robot.orte, send_dummy_cb, &robot.orte);
401 robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
402 robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
403 robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
405 /* create generic subscribers */
406 robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
407 robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
408 robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
409 robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
410 robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
411 //robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
412 robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
413 robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
415 /* create subscribers */
416 robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
417 //robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
418 robottype_subscriber_puck_inside_create(&robot.orte, rcv_puck_inside_cb, &robot.orte);
419 robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);