2 * robot_orte.c 08/04/21
6 * Copyright: (c) 2008 CTU Dragons
7 * CTU FEE - Department of Control Engineering
12 #include <roboorte_generic.h>
13 #include <roboorte_eb2008.h>
15 #include <robot_eb2008.h>
16 #include <movehelper_eb2008.h>
19 #include <laser-nav.h>
20 #include "map_handling.h"
23 /* ----------------------------------------------------------------------
24 * PUBLISHER CALLBACKS - GENERIC
25 * ---------------------------------------------------------------------- */
27 void send_ref_pos_cb(const ORTESendInfo *info, void *vinstance,
28 void *sendCallBackParam)
30 struct ref_pos_type *instance = (struct ref_pos_type *)vinstance;
33 *instance = robot.ref_pos;
34 ROBOT_UNLOCK(ref_pos);
37 void send_est_pos_cb(const ORTESendInfo *info, void *vinstance,
38 void *sendCallBackParam)
40 struct est_pos_type *instance = (struct est_pos_type *)vinstance;
43 *instance = robot.est_pos;
44 ROBOT_UNLOCK(est_pos);
47 void send_dummy_cb(const ORTESendInfo *info, void *vinstance,
48 void *sendCallBackParam)
52 /* ----------------------------------------------------------------------
53 * SUBSCRIBER CALLBACKS - GENERIC
54 * ---------------------------------------------------------------------- */
56 void rcv_motion_irc_cb(const ORTERecvInfo *info, void *vinstance,
57 void *recvCallBackParam)
59 struct motion_irc_type *instance = (struct motion_irc_type *)vinstance;
60 static struct motion_irc_type prevInstance;
61 static int firstRun = 1;
62 /* spocitat prevodovy pomer */
63 double n = (double)(28.0 / 1.0);
64 /* vzdalenost na pulz - 4.442 je empiricka konstanta :) */
65 double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
72 switch (info->status) {
75 prevInstance = *instance;
80 aktk0 = ((prevInstance.left - instance->left) >> 8) * c;
81 aktk1 = ((instance->right - prevInstance.right) >> 8) * c;
82 prevInstance = *instance;
84 deltaU = (aktk0 + aktk1) / 2;
85 deltAlfa = (aktk1 - aktk0) / (2.0*ROBOT_ROTATION_RADIUS_M);
87 struct mcl_robot_odo *odo = malloc(sizeof(struct mcl_robot_odo));
88 memset(odo, 0, sizeof(*odo));
91 odo->dangle = deltAlfa;
92 FSM_SIGNAL(LOC, EV_ODO_RECEIVED, odo);
93 robot.hw_status[STATUS_MOTION] = HW_STATUS_OK;
96 robot.hw_status[STATUS_MOTION] = HW_STATUS_FAILED;
97 DBG("ORTE deadline occurred - motion_irc receive\n");
102 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
103 void *recvCallBackParam)
105 switch (info->status) {
109 DBG("ORTE deadline occurred - motion_speed receive\n");
114 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
115 void *recvCallBackParam)
117 switch (info->status) {
121 DBG("ORTE deadline occurred - motion_status receive\n");
126 void rcv_joy_data_cb(const ORTERecvInfo *info, void *vinstance,
127 void *recvCallBackParam)
129 struct joy_data_type *instance = (struct joy_data_type *)vinstance;
131 switch (info->status) {
133 ROBOT_LOCK(joy_data);
134 robot.joy_data = *instance;
135 ROBOT_UNLOCK(joy_data);
138 DBG("ORTE deadline occurred - joy_data receive\n");
143 void rcv_pwr_voltage_cb(const ORTERecvInfo *info, void *vinstance,
144 void *recvCallBackParam)
146 switch (info->status) {
148 robot.hw_status[STATUS_POWER]=HW_STATUS_OK;
151 robot.hw_status[STATUS_POWER]=HW_STATUS_FAILED;
152 DBG("ORTE deadline occurred - pwr_voltage receive\n");
157 void rcv_pwr_ctrl_cb(const ORTERecvInfo *info, void *vinstance,
158 void *recvCallBackParam)
160 switch (info->status) {
164 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
169 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
170 void *recvCallBackParam)
172 struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
173 static unsigned char start_received = 0;
175 switch (info->status) {
177 if (instance->start && !start_received) {
178 FSM_SIGNAL(MAIN, EV_START, NULL);
183 DBG("ORTE deadline occurred - pwr_ctrl receive\n");
188 /* ----------------------------------------------------------------------
189 * SUBSCRIBER CALLBACKS - EB2008
190 * ---------------------------------------------------------------------- */
192 void rcv_servos_cb(const ORTERecvInfo *info, void *vinstance,
193 void *recvCallBackParam)
195 switch (info->status) {
199 DBG("ORTE deadline occurred - servos receive\n");
204 void rcv_drives_cb(const ORTERecvInfo *info, void *vinstance,
205 void *recvCallBackParam)
207 struct drives_type *instance = (struct drives_type *)vinstance;
209 switch (info->status) {
212 robot.drives = *instance;
213 ROBOT_UNLOCK(drives);
216 DBG("ORTE deadline occurred - drives receive\n");
221 void rcv_laser_data_cb(const ORTERecvInfo *info, void *vinstance,
222 void *recvCallBackParam)
224 struct laser_data_type *instance = (struct laser_data_type *)vinstance;
226 switch (info->status) {
228 struct mcl_laser_measurement *meas_angles;
229 meas_angles = malloc(sizeof(*meas_angles));
230 memset(meas_angles, 0, sizeof(*meas_angles));
232 meas_angles->count = 1;
233 meas_angles->val[0] =
234 (double)TIME2ANGLE(instance->period,
236 // DBG("a=%f\n", RAD2DEG(robot.meas_angles.val[0]));
239 FSM_SIGNAL_TIMED(LOC, EV_LASER_RECEIVED, meas_angles, 20/*ms*/, &sent);
241 DBG("%s: MCL is busy - not updating\n", __FUNCTION__);
244 robot.hw_status[STATUS_LASER] = HW_STATUS_OK;
248 robot.hw_status[STATUS_LASER] = HW_STATUS_FAILED;
249 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
254 void rcv_cmu_cb(const ORTERecvInfo *info, void *vinstance,
255 void *recvCallBackParam)
257 struct cmu_type *instance = (struct cmu_type *)vinstance;
258 static enum ball_color last_color = NO_BALL;
259 static unsigned char first = 1;
261 switch (info->status) {
264 robot.cmu = *instance;
267 last_color = robot.cmu.color;
270 if (robot.cmu.color != NO_BALL) {
271 if (last_color != robot.cmu.color) {
272 last_color = robot.cmu.color;
273 FSM_SIGNAL(MAIN, EV_BALL_INSIDE, NULL);
276 robot.hw_status[STATUS_CMU] = HW_STATUS_OK;
280 robot.hw_status[STATUS_CMU] = HW_STATUS_FAILED;
281 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
286 void rcv_bumper_cb(const ORTERecvInfo *info, void *vinstance,
287 void *recvCallBackParam)
289 struct bumper_type *instance = (struct bumper_type *)vinstance;
291 switch (info->status) {
294 robot.bumper = *instance;
295 ROBOT_UNLOCK(bumper);
299 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
304 void rcv_sharps_cb(const ORTERecvInfo *info, void *vinstance,
305 void *recvCallBackParam)
307 struct sharps_type *instance = (struct sharps_type *)vinstance;
308 //struct sharps_type *for_mcl;
310 switch (info->status) {
313 robot.sharps = *instance;
314 ROBOT_UNLOCK(sharps);
316 update_map(instance);
320 DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
325 void robot_init_orte()
327 robot.orte.strength = 20;
328 robot.gorte.strength = 20;
330 eb2008_roboorte_init(&robot.orte);
331 generic_roboorte_init(&robot.gorte);
333 /* creating publishers */
334 generic_publisher_motion_irc_create(&robot.gorte, NULL, &robot.gorte);
335 generic_publisher_motion_speed_create(&robot.gorte, NULL, &robot.gorte);
336 generic_publisher_motion_status_create(&robot.gorte, NULL, &robot.gorte);
337 generic_publisher_ref_pos_create(&robot.gorte, send_ref_pos_cb, &robot.gorte);
338 generic_publisher_est_pos_create(&robot.gorte, send_est_pos_cb, &robot.gorte);
339 generic_publisher_joy_data_create(&robot.gorte, NULL, &robot.gorte);
340 generic_publisher_pwr_voltage_create(&robot.gorte, NULL, &robot.gorte);
341 generic_publisher_pwr_ctrl_create(&robot.gorte, NULL, &robot.gorte);
343 eb2008_publisher_servos_create(&robot.orte, send_dummy_cb, &robot.orte);
344 eb2008_publisher_drives_create(&robot.orte, send_dummy_cb, &robot.orte);
346 /* create generic subscribers */
347 generic_subscriber_motion_irc_create(&robot.gorte, rcv_motion_irc_cb, &robot.gorte);
348 generic_subscriber_motion_speed_create(&robot.gorte, rcv_motion_speed_cb, &robot.gorte);
349 generic_subscriber_motion_status_create(&robot.gorte, rcv_motion_status_cb, &robot.gorte);
350 generic_subscriber_joy_data_create(&robot.gorte, rcv_joy_data_cb, &robot.gorte);
351 generic_subscriber_pwr_voltage_create(&robot.gorte, rcv_pwr_voltage_cb, &robot.gorte);
352 generic_subscriber_pwr_ctrl_create(&robot.gorte, rcv_pwr_ctrl_cb, &robot.gorte);
353 generic_subscriber_robot_cmd_create(&robot.gorte, rcv_robot_cmd_cb, &robot.gorte);
355 /* create eb2008 subscribers */
356 eb2008_subscriber_servos_create(&robot.orte, rcv_servos_cb, &robot.orte);
357 eb2008_subscriber_drives_create(&robot.orte, rcv_drives_cb, &robot.orte);
358 eb2008_subscriber_laser_data_create(&robot.orte, rcv_laser_data_cb, &robot.orte);
359 eb2008_subscriber_cmu_create(&robot.orte, rcv_cmu_cb, &robot.orte);
360 eb2008_subscriber_bumper_create(&robot.orte, rcv_bumper_cb, &robot.orte);
361 eb2008_subscriber_sharps_create(&robot.orte, rcv_sharps_cb, &robot.orte);