return 0;
}
-int set_pwr_alert(struct robottype_orte_data *orte_data)
-{
- unsigned char data=0;
- data = orte_data->pwr_alert.value;
-
- can_send(CAN_PWR_ALERT, 1, &data);
-
- return 0;
-}
-
int send_can_msg(struct robottype_orte_data *orte_data)
{
return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
{
unsigned char data[4];
- data[0] = orte_data->req_pos >> 8;
- data[1] = orte_data->req_pos & 0xff;
- data[2] = orte_data->speed;
- data[3] = orte_data->homing;
+ 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;
static int status_cnt = 0;
switch(frame.can_id) {
- /* voltage measurements from power board */
-
/* robot commands (start, ..) */
case CAN_JAW_LEFT_STATUS:
orte->jaws_status.act_pos.left = (frame.data[0] << 8) | frame.data[1];
break;
case CAN_ROBOT_SWITCHES:
orte->robot_switches.team_color = (frame.data[0] & CAN_SWITCH_COLOR) ? 1 : 0;
- orte->robot_switches.strategy = !!(frame.data[0] & CAN_SWITCH_STRATEGY);
+ orte->robot_switches.strategy = (frame.data[0] & CAN_SWITCH_STRATEGY ? 1 : 0);
ORTEPublicationSend(orte->publication_robot_switches);
break;
case CAN_ROBOT_BUMPERS:
- orte->robot_bumpers.bumper_left = !!(frame.data[0] & CAN_BUMPER_LEFT);
- orte->robot_bumpers.bumper_left_across = !!(frame.data[0] & CAN_BUMPER_LEFT_ACROSS);
- orte->robot_bumpers.bumper_right = !!(frame.data[0] & CAN_BUMPER_RIGHT);
- orte->robot_bumpers.bumper_right_across = !!(frame.data[0] & CAN_BUMPER_RIGHT_ACROSS);
- orte->robot_bumpers.bumper_rear = !!(frame.data[0] & CAN_BUMPER_REAR);
- ORTEPublicationSend(orte->publication_robot_switches);
+ 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_left = (frame.data[0] & CAN_BUMPER_REAR_LEFT) ? 0 : 1;
+ orte->robot_bumpers.bumper_rear_right = (frame.data[0] & CAN_BUMPER_REAR_RIGHT) ? 0 : 1;
+ ORTEPublicationSend(orte->publication_robot_bumpers);
break;
/* positioning by odometry */
case CAN_ODO_DATA:
- orte->odo_data.left =
+ orte->odo_data.right =
((frame.data[0]<<24)|
(frame.data[1]<<16)|
(frame.data[2]<<8));
- orte->odo_data.right =
+ orte->odo_data.left =
((frame.data[3]<<24)|
(frame.data[4]<<16)|
(frame.data[5]<<8));
/* positioning by odometry */
case CAN_MOTION_ODOMETRY_SIMPLE:
- orte->motion_irc.right =
+ orte->motion_irc.left =
((frame.data[0]<<24)|
(frame.data[1]<<16)|
(frame.data[2]<<8));
- orte->motion_irc.left =
+ orte->motion_irc.right =
((frame.data[3]<<24)|
(frame.data[4]<<16)|
(frame.data[5]<<8));
/* motion status */
case CAN_MOTION_STATUS:
- orte->motion_status.err_left =
+ orte->motion_status.err_left =
(frame.data[0]<<8)|(frame.data[1]);
- orte->motion_status.err_right =
+ orte->motion_status.err_right =
(frame.data[2]<<8)|(frame.data[3]);
if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
ORTEPublicationSend(orte->publication_motion_status);
status_cnt = 0;
}
break;
+ /* voltage measurements from power board */
case CAN_PWR_ADC1:
double volt33, voltBAT;
voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
ORTEPublicationSend(orte->publication_pwr_voltage);
break;
+
+ case CAN_PWR_ALERT:
+ orte->pwr_alert.alert_33 = (frame.data[0] & CAN_PWR_ALERT_33 ? 1 : 0);
+ orte->pwr_alert.alert_50 = (frame.data[0] & CAN_PWR_ALERT_50 ? 1 : 0);
+ orte->pwr_alert.alert_80 = (frame.data[0] & CAN_PWR_ALERT_80 ? 1 : 0);
+ orte->pwr_alert.bat_full = (frame.data[0] & CAN_PWR_BATT_FULL ? 1 : 0);
+ orte->pwr_alert.bat_mean = (frame.data[0] & CAN_PWR_BATT_MEAN ? 1 : 0);
+ orte->pwr_alert.bat_low = (frame.data[0] & CAN_PWR_BATT_LOW ? 1 : 0);
+ orte->pwr_alert.bat_critical = (frame.data[0] & CAN_PWR_BATT_CRITICAL ? 1 : 0);
+
+ ORTEPublicationSend(orte->publication_pwr_alert);
+ break;
+
default:
//FIXME: add logging here (Filip)
// printf("received CAN msg with unknown id: %x\n",frame.can_id);
break;
}
}
-void rcv_pwr_alert_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_pwr_alert(orte_data);
- break;
- case DEADLINE:
- //printf("ORTE deadline occurred - PWR_CTRL receive\n");
- break;
- }
-}
void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
switch (info->status) {
case NEW_DATA:
/* reversing motion direction, as it is different, than last year */
- orte_data->motion_speed.left *= -1;
- orte_data->motion_speed.right *=-1;
+ orte_data->motion_speed.left *= 1;
+ orte_data->motion_speed.right *= 1;
set_motor_speed(orte_data);
/*printf("motor cmd received\n");*/
break;
void rcv_lift_cmd_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct lift_cmd_type *lift_cmd = (struct lift_cmd_type *)vinstance;
+ struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- set_lift_cmd(lift_cmd->req_pos, lift_cmd->speed);
+ set_lift_cmd(orte_data);
break;
case DEADLINE:
break;
}
}
+struct robottype_orte_data orte;
+
int main(int argc, char *argv[])
{
int ret;
int size;
- struct robottype_orte_data orte;
struct can_frame frame;
if (cand_init() < 0) {
printf("cand: init OK\n");
}
- orte.strength = 1;
-
/* orte initialization */
if(robottype_roboorte_init(&orte)) {
printf("Roboorte initialization failed! Exiting...\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_jaws_cmd_create(&orte, rcv_jaws_cmd_cb, &orte);
robottype_subscriber_lift_cmd_create(&orte, rcv_lift_cmd_cb, &orte);
projects / eurobot / public.git / blobdiff