{
unsigned char data[4];
- data[0] = orte_data->motion_speed.right >> 8;
- data[1] = orte_data->motion_speed.right & 0xff;
- data[2] = orte_data->motion_speed.left >> 8;
- data[3] = orte_data->motion_speed.left & 0xff;
+ data[0] = orte_data->motion_speed.left >> 8;
+ data[1] = orte_data->motion_speed.left & 0xff;
+ data[2] = orte_data->motion_speed.right >> 8;
+ data[3] = orte_data->motion_speed.right & 0xff;
can_send(CAN_MOTION_CMD, 4, data);
return 0;
}
-int set_vidle_cmd(uint16_t req_pos)
+int set_jaws_cmd(struct robottype_orte_data *orte_data)
{
- unsigned char data[2];
+ unsigned char data[3];
- data[0] = req_pos >> 8;
- data[1] = req_pos & 0xff;
- can_send(CAN_VIDLE_CMD, 2, data);
+ data[0] = orte_data->jaws_cmd.req_pos.left >> 8;
+ data[1] = orte_data->jaws_cmd.req_pos.left & 0xff;
+ data[2] = orte_data->jaws_cmd.speed.left;
+ can_send(CAN_JAW_LEFT_CMD, 3, data);
+
+ data[0] = orte_data->jaws_cmd.req_pos.right >> 8;
+ data[1] = orte_data->jaws_cmd.req_pos.right & 0xff;
+ data[2] = orte_data->jaws_cmd.speed.right;
+ can_send(CAN_JAW_RIGHT_CMD, 3, data);
return 0;
}
-/**
- * Sends #CAN_HOKUYO_PITCH message.
- *
- * - data = orte_data->pusher.pos
- */
-int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
+int set_lift_cmd(struct robottype_orte_data *orte_data)
{
- unsigned char data = orte_data->hokuyo_pitch.angle;
+ unsigned char data[4];
- can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
- return 0;
-}
+ data[0] = orte_data->lift_cmd.req_pos >> 8;
+ data[1] = orte_data->lift_cmd.req_pos & 0xff;
+ data[2] = orte_data->lift_cmd.speed;
+ data[3] = orte_data->lift_cmd.homing;
+ can_send(CAN_LIFT_CMD, 4, data);
+ return 0;
+}
int can_send(canid_t id, unsigned char length, unsigned char *data)
{
switch(frame.can_id) {
/* voltage measurements from power board */
-
+
/* robot commands (start, ..) */
- case CAN_VIDLE_STATUS:
- orte->vidle_status.act_pos = (frame.data[0] << 8) | frame.data[1];
- orte->vidle_status.response = (frame.data[2] << 8) | frame.data[3];
- orte->vidle_status.flags = frame.data[4];
- ORTEPublicationSend(orte->publication_vidle_status);
+ case CAN_JAW_LEFT_STATUS:
+ orte->jaws_status.act_pos.left = (frame.data[0] << 8) | frame.data[1];
+ orte->jaws_status.response.left = (frame.data[2] << 8) | frame.data[3];
+ orte->jaws_status.flags.left = frame.data[4];
+ ORTEPublicationSend(orte->publication_jaws_status);
break;
+ case CAN_JAW_RIGHT_STATUS:
+ orte->jaws_status.act_pos.right = (frame.data[0] << 8) | frame.data[1];
+ orte->jaws_status.response.right = (frame.data[2] << 8) | frame.data[3];
+ orte->jaws_status.flags.right = frame.data[4];
+ ORTEPublicationSend(orte->publication_jaws_status);
+ break;
+ case CAN_LIFT_STATUS:
+ orte->lift_status.act_pos = (frame.data[0] << 8) | frame.data[1];
+ orte->lift_status.response = (frame.data[2] << 8) | frame.data[3];
+ orte->lift_status.flags = frame.data[4];
+ ORTEPublicationSend(orte->publication_lift_status);
+ break;
case CAN_ROBOT_CMD:
orte->robot_cmd.start_condition = frame.data[0];
ORTEPublicationSend(orte->publication_robot_cmd);
break;
case CAN_ROBOT_SWITCHES:
- orte->robot_switches.bumper_pressed = !!(frame.data[0] & CAN_SWITCH_BUMPER);
- orte->robot_switches.bumper_left = !!(frame.data[0] & CAN_SWITCH_LEFT);
- orte->robot_switches.bumper_right = !!(frame.data[0] & CAN_SWITCH_RIGHT);
orte->robot_switches.team_color = (frame.data[0] & CAN_SWITCH_COLOR) ? 1 : 0;
+ orte->robot_switches.strategy = !!(frame.data[0] & CAN_SWITCH_STRATEGY);
ORTEPublicationSend(orte->publication_robot_switches);
break;
+ case CAN_ROBOT_BUMPERS:
+ orte->robot_bumpers.bumper_left = (frame.data[0] & CAN_BUMPER_LEFT) ? 0 : 1;
+ orte->robot_bumpers.bumper_left_across = (frame.data[0] & CAN_BUMPER_LEFT_ACROSS) ? 0 : 1;
+ orte->robot_bumpers.bumper_right = (frame.data[0] & CAN_BUMPER_RIGHT) ? 0 : 1;
+ orte->robot_bumpers.bumper_right_across = (frame.data[0] & CAN_BUMPER_RIGHT_ACROSS) ? 0 : 1;
+ orte->robot_bumpers.bumper_rear = (frame.data[0] & CAN_BUMPER_REAR) ? 0 : 1;
+ ORTEPublicationSend(orte->publication_robot_bumpers);
+ break;
/* positioning by odometry */
case CAN_ODO_DATA:
/* motion status */
case CAN_MOTION_STATUS:
- orte->motion_status.err_left =
+ orte->motion_status.err_right =
(frame.data[0]<<8)|(frame.data[1]);
- orte->motion_status.err_right =
+ orte->motion_status.err_left =
(frame.data[2]<<8)|(frame.data[3]);
if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
ORTEPublicationSend(orte->publication_motion_status);
}
}
-void rcv_vidle_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+void rcv_jaws_cmd_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct vidle_cmd_type *vidle_cmd = (struct vidle_cmd_type *)vinstance;
+ struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- set_vidle_cmd(vidle_cmd->req_pos);
+ set_jaws_cmd(orte_data);
break;
case DEADLINE:
break;
}
}
-void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
- void *recvCallBackParam)
+void rcv_lift_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
{
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
- case NEW_DATA:
- set_hokuyo_pitch(orte_data);
- break;
- case DEADLINE:
-// printf("ORTE deadline occurred - hokuyo pitch receive\n");
- break;
- }
+ case NEW_DATA:
+ set_lift_cmd(orte_data);
+ break;
+ case DEADLINE:
+ break;
+}
}
-
int main(int argc, char *argv[])
{
robottype_publisher_motion_irc_create(&orte, NULL, NULL);
robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
robottype_publisher_robot_switches_create(&orte, NULL, NULL);
- robottype_publisher_vidle_status_create(&orte, NULL, NULL);
+ robottype_publisher_robot_bumpers_create(&orte, NULL, NULL);
+ robottype_publisher_jaws_status_create(&orte, NULL, NULL);
+ robottype_publisher_lift_status_create(&orte, NULL, NULL);
printf("Publishers OK\n");
/* creating subscribers */
robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
- robottype_subscriber_vidle_cmd_create(&orte, rcv_vidle_cmd_cb, &orte);
+ robottype_subscriber_jaws_cmd_create(&orte, rcv_jaws_cmd_cb, &orte);
+ robottype_subscriber_lift_cmd_create(&orte, rcv_lift_cmd_cb, &orte);
robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
-
+
printf("subscribers OK\n");
projects / eurobot / public.git / blobdiff