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"
25 extern sem_t measurement_received;
27 /* ----------------------------------------------------------------------
28 * PUBLISHER CALLBACKS - GENERIC
29 * ---------------------------------------------------------------------- */
31 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
32 void *sendCallBackParam)
34 struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
37 *instance = robot.ref_pos;
38 ROBOT_UNLOCK(ref_pos);
41 void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
42 void *sendCallBackParam)
44 struct est_pos_type *instance = (struct est_pos_type *)vinstance;
47 *instance = robot.est_pos;
48 ROBOT_UNLOCK(est_pos);
51 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
52 void *sendCallBackParam)
56 /* ----------------------------------------------------------------------
57 * SUBSCRIBER CALLBACKS - GENERIC
58 * ---------------------------------------------------------------------- */
60 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
61 void *recvCallBackParam)
63 struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
65 switch (info->status) {
67 /* locking should not be needed, but... */
68 ROBOT_LOCK(motion_irc);
69 robot.motion_irc = *instance;
70 ROBOT_UNLOCK(motion_irc);
72 /* FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo); */
73 robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
76 robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
77 DBG("ORTE deadline occurred - motion_irc receive\n");
82 void rcv_corr_distances_cb(const ORTERecvInfo *info, void *vinstance,
83 void *recvCallBackParam)
85 struct corr_distances_type *instance =
86 (struct corr_distances_type *)vinstance;
88 switch (info->status) {
90 /* locking should not be needed, but... */
91 ROBOT_LOCK(corr_distances);
92 robot.corr_distances = *instance;
93 ROBOT_UNLOCK(corr_distances);
95 /* wake up motion-control/thread_trajectory_follower */
96 sem_post(&measurement_received);
98 /*FIXME: is following OK? (pecam1) */
99 robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
102 robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
103 DBG("ORTE deadline occurred - corr_distances receive\n");
108 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
109 void *recvCallBackParam)
111 switch (info->status) {
115 DBG("ORTE deadline occurred - motion_speed receive\n");
120 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
121 void *recvCallBackParam)
123 switch (info->status) {
127 DBG("ORTE deadline occurred - motion_status receive\n");
132 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
133 void *recvCallBackParam)
135 switch (info->status) {
137 robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
140 robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
141 DBG("ORTE deadline occurred - pwr_voltage receive\n");
146 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
147 void *recvCallBackParam)
149 switch (info->status) {
153 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
158 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
159 void *recvCallBackParam)
161 struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
162 static struct robot_cmd_type last_instance;
163 enum robot_state state;
165 switch (info->status) {
167 /* Stupid way of controlling the robot by
168 * pluggin in and out start connector. */
169 if (instance->start == last_instance.start)
175 printf("in state=%d \n", state);
179 /* On first plug-in laser is tarted */
182 FSM_SIGNAL(MAIN, EV_LASER_POWER, NULL);
183 state = LASER_STARTED;
187 /* Competition starts when plugged out */
188 if (!instance->start)
190 FSM_SIGNAL(MAIN, EV_START, NULL);
191 state = COMPETITION_STARTED;
194 case COMPETITION_STARTED:
195 /* Subsequent plug-in stops the robot */
196 if (!instance->start) {
201 last_instance = *instance;
202 printf("out state=%d \n", state);
208 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
213 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
214 void *recvCallBackParam)
216 struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
218 switch (info->status) {
221 robot.hokuyo = *instance;
222 ROBOT_UNLOCK(hokuyo);
223 update_map_hokuyo(instance);
227 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
231 /* ----------------------------------------------------------------------
232 * SUBSCRIBER CALLBACKS - EB2008
233 * ---------------------------------------------------------------------- */
235 void rcv_actuator_status_cb(const ORTERecvInfo *info, void *vinstance,
236 void *recvCallBackParam)
238 switch (info->status) {
240 if (robot.orte.actuator_status.status == 0) {
241 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_OK;
243 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_WARNING;
245 // robot.orte.actuator_status.lift_pos
246 // robot.orte.actuator_status.pusher_pos
247 //FSM_SIGNAL(ACT, STATUS_RCVD, NULL);
250 robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
251 DBG("ORTE deadline occurred - servos receive\n");
256 void rcv_puck_distance_cb(const ORTERecvInfo *info, void *vinstance,
257 void *recvCallBackParam)
259 switch (info->status) {
261 // FIXME (?) inform display in case of invalid data?
264 // FIXME (?) limited space on the display...
265 //robot.hw_status[STATUS_ACTUATORS]=HW_STATUS_FAILED;
266 DBG("ORTE deadline occurred - servos receive\n");
272 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
273 void *recvCallBackParam)
275 struct cmu_type *instance = (struct cmu_type *)vinstance;
276 static enum ball_color last_color = NO_BALL;
277 static unsigned char first = 1;
279 switch (info->status) {
282 robot.cmu = *instance;
285 last_color = robot.cmu.color;
288 if (robot.cmu.color != NO_BALL) {
289 if (last_color != robot.cmu.color) {
290 last_color = robot.cmu.color;
291 FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
294 robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
298 robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
299 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
307 static int cmp_double(const void *v1, const void *v2)
309 const double *d1 = v1, *const d2 = v2;
318 int robot_init_orte()
322 robot.orte.strength = 20;
324 rv = robottype_roboorte_init(&robot.orte);
328 /* creating publishers */
329 robottype_publisher_motion_irc_create(&robot.orte, NULL, &robot.orte);
330 robottype_publisher_motion_speed_create(&robot.orte, NULL, &robot.orte);
331 robottype_publisher_motion_status_create(&robot.orte, NULL, &robot.orte);
332 robottype_publisher_ref_pos_create(&robot.orte, send_ref_pos_cb, &robot.orte);
333 robottype_publisher_est_pos_create(&robot.orte, send_est_pos_cb, &robot.orte);
334 robottype_publisher_pwr_voltage_create(&robot.orte, NULL, &robot.orte);
335 robottype_publisher_pwr_ctrl_create(&robot.orte, NULL, &robot.orte);
337 robottype_publisher_lift_create(&robot.orte, send_dummy_cb, &robot.orte);
338 robottype_publisher_pusher_create(&robot.orte, send_dummy_cb, &robot.orte);
339 robottype_publisher_chelae_create(&robot.orte, send_dummy_cb, &robot.orte);
340 robottype_publisher_belts_create(&robot.orte, send_dummy_cb, &robot.orte);
341 /* not possible to have chelae&belts and picker publishers registeres together... */
342 // robottype_publisher_picker_create(&robot.orte, send_dummy_cb, &robot.orte);
343 robottype_publisher_holder_create(&robot.orte, send_dummy_cb, &robot.orte);
345 /* create generic subscribers */
346 robottype_subscriber_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte);
347 robottype_subscriber_motion_speed_create(&robot.orte, rcv_motion_speed_cb, &robot.orte);
348 robottype_subscriber_motion_status_create(&robot.orte, rcv_motion_status_cb, &robot.orte);
349 robottype_subscriber_corr_distances_create(&robot.orte, rcv_corr_distances_cb, &robot.orte);
350 robottype_subscriber_pwr_voltage_create(&robot.orte, rcv_pwr_voltage_cb, &robot.orte);
351 robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
352 robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
353 robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
355 /* create subscribers */
356 robottype_subscriber_actuator_status_create(&robot.orte, rcv_actuator_status_cb, &robot.orte);
357 robottype_subscriber_puck_distance_create(&robot.orte, rcv_puck_distance_cb, &robot.orte);
358 //robottype_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);