#include <orte.h>
#include <roboorte_robottype.h>
-#include <robodata.h>
+#include "robodata.h"
#include <robot.h>
#include <movehelper.h>
#include <math.h>
#include <string.h>
#include <can_msg_def.h>
#include <actuators.h>
+#include <ul_log.h>
+
+UL_LOG_CUST(ulogd_robot_orte); /* Log domain name = ulogd + name of the file */
#ifdef ORTE_DEBUG
- #define DBG(format, ...) printf(format, ##__VA_ARGS__)
+ #define DBG(format, ...) ul_logdeb(format, ##__VA_ARGS__)
#else
#define DBG(format, ...)
#endif
double dleft, dright, dtang, dphi;
static bool first_run = true;
/* spocitat prevodovy pomer */
- const double n = (double)(28.0 / 1.0);
+ const double n = (double)(29.0 / 1.0);
/* vzdalenost na pulz IRC */
- const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*500.0);
-
+ const double c = (M_PI*2*ROBOT_WHEEL_RADIUS_M) / (n * 4*512.0);
switch (info->status) {
case NEW_DATA:
if (first_run) {
break;
}
- dleft = ((robot.motion_irc.left - instance->left) >> 8) * c;
- dright = ((instance->right - robot.motion_irc.right) >> 8) * c;
+
+ dright = ((robot.motion_irc.left - instance->left) >> 8) * c;
+ dleft = ((instance->right - robot.motion_irc.right) >> 8) * c;
dtang = (dleft + dright) / 2.0;
dphi = (dright - dleft) / (2.0*ROBOT_ROTATION_RADIUS_M);
if (!instance->start_condition) {
robot.status[COMPONENT_START] = STATUS_WARNING;
robot.start_state = START_PLUGGED_IN;
- printf("START_PLUGGED_IN\n");
+ ul_logmsg("START_PLUGGED_IN\n");
}
break;
if (instance->start_condition) {
FSM_SIGNAL(MAIN, EV_START, NULL);
robot.start_state = COMPETITION_STARTED;
- printf("STARTED\n");
+ ul_logmsg("STARTED\n");
}
break;
* SUBSCRIBER CALLBACKS - EB2008
* ---------------------------------------------------------------------- */
-void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_status_type *instance = (struct vidle_status_type *)vinstance;
+ struct jaws_status_type *instance = (struct jaws_status_type *)vinstance;
+ static int last_response_left = 0;
+ static int last_response_right = 0;
switch (info->status) {
case NEW_DATA:
// new data arrived and requested position equals actual position
- if (instance->flags == 0)
- robot.status[COMPONENT_VIDLE]=STATUS_OK;
+ if (instance->flags.left == 0 &&
+ instance->flags.right == 0)
+ robot.status[COMPONENT_JAWS] = STATUS_OK;
else
- robot.status[COMPONENT_VIDLE]=STATUS_WARNING;
+ robot.status[COMPONENT_JAWS] = STATUS_WARNING;
- if (instance->response == act_vidle_get_last_reqest() ||
- instance->flags & CAN_VIDLE_TIMEOUT) {
- //FSM_SIGNAL(MAIN, EV_VIDLE_DONE, NULL);
- /* TODO: Uncomment the above and fix */
- }
+ if (instance->response.left != last_response_left &&
+ instance->response.right != last_response_right &&
+ instance->response.left == act_jaw_left_get_last_reqest() &&
+ instance->response.left == act_jaw_right_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_JAWS_DONE, NULL);
+
+ last_response_left = instance->response.left;
+ last_response_right = instance->response.right;
break;
case DEADLINE:
- robot.status[COMPONENT_VIDLE]=STATUS_FAILED;
+ robot.status[COMPONENT_JAWS] = STATUS_FAILED;
DBG("ORTE deadline occurred - actuator_status receive\n");
break;
}
}
+void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct lift_status_type *instance = (struct lift_status_type *)vinstance;
+ static int last_response = 0;
+ switch (info->status) {
+ case NEW_DATA:
+ // new data arrived and requested position equals actual position
+ if (instance->flags == 0)
+ robot.status[COMPONENT_LIFT] = STATUS_OK;
+ else
+ robot.status[COMPONENT_LIFT] = STATUS_WARNING;
+
+ if (instance->response != last_response &&
+ instance->response == act_lift_get_last_reqest())
+ FSM_SIGNAL(MAIN, EV_LIFT_DONE, NULL);
+ last_response = instance->response;
+ break;
+ case DEADLINE:
+ robot.status[COMPONENT_LIFT] = STATUS_FAILED;
+ DBG("ORTE deadline occurred - actuator_status receive\n");
+ break;
+ }
+}
+
void rcv_robot_switches_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
- static bool last_left, last_right;
+ static bool last_strategy;
switch (info->status) {
case NEW_DATA:
robot.team_color = instance->team_color;
- if (instance->bumper_pressed)
+
+ if (!last_strategy && instance->strategy && (robot.start_state == POWER_ON)) {
+ /* strategy switching */
+ FSM_SIGNAL(MAIN, EV_SWITCH_STRATEGY, NULL);
+ }
+ last_strategy = instance->strategy;
+ break;
+ case DEADLINE:
+ break;
+ }
+}
+
+void rcv_robot_bumpers_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct robot_bumpers_type *instance = (struct robot_bumpers_type *)vinstance;
+ static bool last_left, last_right;
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->bumper_right_across || instance->bumper_left_across)
FSM_SIGNAL(MOTION, EV_OBSTACLE_BEHIND, NULL);
- if ((!last_left && instance->bumper_left) ||
- (!last_right && instance->bumper_right))
+
+ if (instance->bumper_left || instance->bumper_right) {
FSM_SIGNAL(MOTION, EV_OBSTACLE_SIDE, NULL);
- last_right = instance->bumper_right;
- last_left = instance->bumper_left;
+ }
break;
case DEADLINE:
break;
robottype_publisher_fsm_act_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_fsm_motion_create(&robot.orte, send_dummy_cb, &robot.orte);
robottype_publisher_camera_control_create(&robot.orte, send_dummy_cb, &robot.orte);
- robottype_publisher_vidle_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_jaws_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
+ robottype_publisher_lift_cmd_create(&robot.orte, send_dummy_cb, &robot.orte);
/* create generic subscribers */
robottype_subscriber_odo_data_create(&robot.orte, rcv_odo_data_cb, &robot.orte);
//robottype_subscriber_pwr_ctrl_create(&robot.orte, rcv_pwr_ctrl_cb, &robot.orte);
robottype_subscriber_robot_cmd_create(&robot.orte, rcv_robot_cmd_cb, &robot.orte);
robottype_subscriber_hokuyo_scan_create(&robot.orte, rcv_hokuyo_scan_cb, &robot.orte);
- robottype_subscriber_vidle_status_create(&robot.orte, rcv_vidle_status_cb, &robot.orte);
+ robottype_subscriber_jaws_status_create(&robot.orte, rcv_jaws_status_cb, &robot.orte);
+ robottype_subscriber_lift_status_create(&robot.orte, rcv_lift_status_cb, &robot.orte);
robottype_subscriber_robot_switches_create(&robot.orte, rcv_robot_switches_cb, &robot.orte);
- robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
+ robottype_subscriber_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte);
+ robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte);
return rv;
}
projects / eurobot / public.git / blobdiff