From 05835c36c8bee395769f163f44e5727f69bda63c Mon Sep 17 00:00:00 2001 From: Michal Vokac Date: Thu, 25 Sep 2014 21:21:56 +0200 Subject: [PATCH] =?utf8?q?Complete=20JAW=20and=20LIFT=C2=A0removal?= MIME-Version: 1.0 Content-Type: text/plain; charset=utf8 Content-Transfer-Encoding: 8bit In next step, remove those actuator also from eb-boards. --- src/cand/cand.cc | 80 --------------------------------- src/display-qt/display_orte.cpp | 26 ----------- src/display-qt/display_orte.h | 2 - src/display-qt/displayqt.cpp | 8 ---- src/display-qt/displayqt.ui | 20 --------- src/display-qt/ortesignals.cpp | 1 - src/display-qt/promene.h | 18 ++++---- src/joyd/joyd.cc | 16 ------- src/robodim/robodim.h | 4 -- src/robofsm/actuators.c | 57 +---------------------- src/robofsm/actuators.h | 24 ---------- src/robofsm/common-states.cc | 2 +- src/robofsm/robot_orte.c | 60 ------------------------- src/robofsm/sick-day.cc | 1 - src/robofsm/sub-states.cc | 17 +------ src/robomon/RobomonAtlantis.cpp | 8 ---- src/robomon/Robot.cpp | 34 -------------- src/robomon/Robot.h | 2 - src/types/robottype.idl | 17 ------- src/types/robottype.ortegen | 4 -- 20 files changed, 11 insertions(+), 390 deletions(-) diff --git a/src/cand/cand.cc b/src/cand/cand.cc index 308092dc..fa5f5f02 100644 --- a/src/cand/cand.cc +++ b/src/cand/cand.cc @@ -105,36 +105,6 @@ int set_motor_speed(struct robottype_orte_data *orte_data) return 0; } -int set_jaws_cmd(struct robottype_orte_data *orte_data) -{ - unsigned char data[3]; - - 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; -} - -int set_lift_cmd(struct robottype_orte_data *orte_data) -{ - unsigned char data[4]; - - 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) { struct can_frame frame; @@ -170,24 +140,6 @@ void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame) orte->cl_sensor_status.pattern_match = frame.data[0]; ORTEPublicationSend(orte->publication_cl_sensor_status); break; - 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); @@ -347,34 +299,6 @@ void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance, } } -void rcv_jaws_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_jaws_cmd(orte_data); - break; - case DEADLINE: - break; - } -} - -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_lift_cmd(orte_data); - break; - case DEADLINE: - break; - } -} - void rcv_cl_sensor_cmd_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { @@ -424,16 +348,12 @@ int main(int argc, char *argv[]) robottype_publisher_robot_cmd_create(&orte, NULL, NULL); robottype_publisher_robot_switches_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); robottype_publisher_cl_sensor_status_create(&orte, NULL, NULL); printf("Publishers OK\n"); /* creating subscribers */ robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_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); robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte); robottype_subscriber_cl_sensor_cmd_create(&orte, rcv_cl_sensor_cmd_cb, &orte); printf("subscribers OK\n"); diff --git a/src/display-qt/display_orte.cpp b/src/display-qt/display_orte.cpp index 402393e6..9b63116c 100644 --- a/src/display-qt/display_orte.cpp +++ b/src/display-qt/display_orte.cpp @@ -195,32 +195,6 @@ void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvC last_status = status; } -void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) -{ - OrteSignals *inst=(OrteSignals *)recvCallBackParam; - struct jaws_status_type *m = (struct jaws_status_type *)vinstance; - UDE_hw_status_t status = UDE_HW_STATUS_FAILED; - static UDE_hw_status_t last_status; - static struct timespec last_sent; - switch (info->status) { - case NEW_DATA: - if (m->flags.left == 0 && m->flags.right == 0) - status = UDE_HW_STATUS_OK; - else - status = UDE_HW_STATUS_WARNING; - break; - case DEADLINE: - status = UDE_HW_STATUS_FAILED; - break; - } - if (status != last_status || - miliseconds_elapsed_since(&last_sent, 1000)) { - inst->status_con(JAW, status); - clock_gettime(CLOCK_MONOTONIC, &last_sent); - } - last_status = status; -} - void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam) { diff --git a/src/display-qt/display_orte.h b/src/display-qt/display_orte.h index ced9012b..70cd06b6 100644 --- a/src/display-qt/display_orte.h +++ b/src/display-qt/display_orte.h @@ -20,8 +20,6 @@ void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance, void *recv void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam); -void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam); - void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam); void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam); diff --git a/src/display-qt/displayqt.cpp b/src/display-qt/displayqt.cpp index 5b16490f..938a57de 100644 --- a/src/display-qt/displayqt.cpp +++ b/src/display-qt/displayqt.cpp @@ -180,14 +180,6 @@ void DisplayQT::display_status(UDE_component_t c, UDE_hw_status_t s) else ui->comp_ODO->setStyleSheet(YELLOW); break; - case JAW: - if(s==UDE_HW_STATUS_OK) - ui->comp_JAW->setStyleSheet(GREEN); - else if(s==UDE_HW_STATUS_FAILED) - ui->comp_JAW->setStyleSheet(RED); - else - ui->comp_JAW->setStyleSheet(YELLOW); - break; case PWR: if(s==UDE_HW_STATUS_OK) ui->comp_PWR->setStyleSheet(GREEN); diff --git a/src/display-qt/displayqt.ui b/src/display-qt/displayqt.ui index 6ac3986c..b38d8805 100644 --- a/src/display-qt/displayqt.ui +++ b/src/display-qt/displayqt.ui @@ -72,26 +72,6 @@ - - - - - 12 - 75 - true - - - - background-color: rgb(255, 0, 0); - - - JAW - - - Qt::AlignCenter - - - diff --git a/src/display-qt/ortesignals.cpp b/src/display-qt/ortesignals.cpp index 42a8a27d..2d8c8981 100644 --- a/src/display-qt/ortesignals.cpp +++ b/src/display-qt/ortesignals.cpp @@ -15,7 +15,6 @@ void OrteSignals::createOrte() //subscribers robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, this); robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, this); - robottype_subscriber_jaws_status_create(&orte, rcv_jaws_status_cb, this); robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, this); robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, this); robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, this); diff --git a/src/display-qt/promene.h b/src/display-qt/promene.h index 33b96cfa..1cfde16e 100644 --- a/src/display-qt/promene.h +++ b/src/display-qt/promene.h @@ -5,7 +5,6 @@ * 0. (not used) * 1. MOT - motor control unit * 2. ODO - odometry - * 3. JAW - jaws * 4. PWR - power sources * 5. HOK - Hokuyo * 6. SIC - Sick @@ -14,15 +13,14 @@ */ typedef enum { UKN = 0, - MOT = 1, - ODO = 2, - JAW = 3, - PWR = 4, - HOK = 5, - SIC = 6, - APP = 7, - STA = 8, - TIM = 9 + MOT, + ODO, + PWR, + HOK, + SIC, + APP, + STA, + TIM } UDE_component_t; /** diff --git a/src/joyd/joyd.cc b/src/joyd/joyd.cc index 84c5f0b5..dc3b08ca 100644 --- a/src/joyd/joyd.cc +++ b/src/joyd/joyd.cc @@ -63,18 +63,14 @@ static void button_act(char state, int id) case BT1: if(state) { //act ON - act_jaws(CATCH); - printf("jaws CATCH\n"); } else { ;//act OFF } break; case BT2: if(state) { - act_lift(0, 0, 1); //act ON } else { - act_lift(0, 0, 0); ;//act OFF } break; @@ -158,15 +154,10 @@ static void process_axis(int value, int id) case AXIS_S1: switch (value) { case 32767: - act_jaws(CLOSE); - printf("jaws CLOSE\n"); break; case -32767: - act_jaws(OPEN); - printf("jaws OPEN\n"); break; case 0: - printf("jaws stop\n"); break; default: printf("error!\n"); @@ -176,15 +167,10 @@ static void process_axis(int value, int id) case AXIS_S2: switch (value) { case 32767: - act_lift(DOWN, 0, 0); - printf("lift DOWN\n"); break; case -32767: - act_lift(UP, 0, 0); - printf("lift UP\n"); break; case 0: - printf("lift stop\n"); break; default: printf("error!\n"); @@ -291,8 +277,6 @@ int main(int argc, char *argv[]) /* creating publishers */ robottype_publisher_motion_speed_create(&orte, send_dummy_cb, &orte); - robottype_publisher_lift_cmd_create(&orte, send_dummy_cb, &orte); - robottype_publisher_jaws_cmd_create(&orte, send_dummy_cb, &orte); if(open_joystick(joy_name, &num_of_axis, &num_of_buttons) == -1) { printf("Joystick not found, exiting...\n"); diff --git a/src/robodim/robodim.h b/src/robodim/robodim.h index 01df0456..6c2c2d75 100644 --- a/src/robodim/robodim.h +++ b/src/robodim/robodim.h @@ -66,10 +66,6 @@ #define ROBOT_DIAGONAL_RADIUS_MM sqrt((ROBOT_WIDTH_MM*ROBOT_WIDTH_MM/4) + (ROBOT_AXIS_TO_FRONT_MM*ROBOT_AXIS_TO_FRONT_MM)) #define ROBOT_DIAGONAL_RADIUS_M (ROBOT_DIAGONAL_RADIUS_MM/1000.0) -#define ROBOT_JAWS_LENGHT_MM 130.0 /* JA */ -#define ROBOT_JAWS_LENGHT_M (ROBOT_JAWS_LENGHT_MM/1000.0) -#define ROBOT_LENGHT_WITH_PANWS_MM 170.0 /* PW */ -#define ROBOT_LENGHT_WITH_PAWNS_M (ROBOT_LENGHT_WITH_PANWS_MM/1000.0) #define ROBOT_AXIS_TO_FIGURE_CENTER_MM 300.0 #define ROBOT_AXIS_TO_FIGURE_CENTER_M (ROBOT_AXIS_TO_FIGURE_CENTER_MM/1000.0) diff --git a/src/robofsm/actuators.c b/src/robofsm/actuators.c index 7b38bd51..1feb9c88 100644 --- a/src/robofsm/actuators.c +++ b/src/robofsm/actuators.c @@ -23,15 +23,9 @@ static struct robottype_orte_data *orte = NULL; -static uint16_t jaw_left_last_request; -static uint16_t jaw_right_last_request; -static uint16_t lift_last_request; - void act_init(struct robottype_orte_data *ortedata) { orte = ortedata; - act_jaws(OPEN); - act_lift(0, 0, 1); } void act_camera_recognize(char* target_color) @@ -60,58 +54,9 @@ void act_camera_off(void) ORTEPublicationSend(orte->publication_camera_control); } -void act_lift(uint16_t req_pos, char speed, char homing) -{ - - orte->lift_cmd.req_pos = req_pos; - orte->lift_cmd.speed = speed; - orte->lift_cmd.homing = homing; - /* Remember the request so that we change check for matching - * response in rcv_vidle_status_cb() */ - lift_last_request = req_pos; - ORTEPublicationSend(orte->publication_lift_cmd); -} - -void act_jaws(jaws_cmds cmd) -{ - switch (cmd) { - case OPEN: - orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN; - usleep(300000); - orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN; - break; - case CLOSE: - orte->jaws_cmd.req_pos.left = JAW_LEFT_CLOSE; - usleep(300000); - orte->jaws_cmd.req_pos.right = JAW_RIGHT_CLOSE; - break; - case CATCH: - orte->jaws_cmd.req_pos.left = JAW_LEFT_CATCH; - orte->jaws_cmd.req_pos.right = JAW_RIGHT_CATCH; - break; - default: - orte->jaws_cmd.req_pos.left = JAW_LEFT_OPEN; - orte->jaws_cmd.req_pos.right = JAW_RIGHT_OPEN; - } -} void act_omron_set_color(char target_color) { orte->cl_sensor_cmd.bank_nbr = target_color; ORTEPublicationSend(orte->publication_cl_sensor_cmd); -} - -uint16_t act_jaw_left_get_last_reqest(void) -{ - return jaw_left_last_request; -} - -uint16_t act_jaw_right_get_last_reqest(void) -{ - return jaw_right_last_request; -} - -uint16_t act_lift_get_last_reqest(void) -{ - return lift_last_request; -} +} \ No newline at end of file diff --git a/src/robofsm/actuators.h b/src/robofsm/actuators.h index b61cf905..6c25e377 100644 --- a/src/robofsm/actuators.h +++ b/src/robofsm/actuators.h @@ -33,24 +33,6 @@ of the robot. #include -typedef enum { - OPEN, - CLOSE, - CATCH -} jaws_cmds; - -#define JAW_LEFT_OPEN 0xd0 -#define JAW_RIGHT_OPEN 0x60 - -#define JAW_LEFT_CATCH 0x50 -#define JAW_RIGHT_CATCH 0xd0 - -#define JAW_LEFT_CLOSE 0x50 -#define JAW_RIGHT_CLOSE 0xd0 - -#define UP 0x140 -#define DOWN 0x0 - #ifdef __cplusplus extern "C" { #endif @@ -61,14 +43,8 @@ void act_camera_on(void); void act_camera_off(void); void act_camera_recognize(char* target_color); -void act_lift(uint16_t req_pos, char speed, char homing); -void act_jaws(jaws_cmds cmd); void act_omron_set_color(char target_color); -uint16_t act_jaw_left_get_last_reqest(void); -uint16_t act_jaw_right_get_last_reqest(void); -uint16_t act_lift_get_last_reqest(void); - #ifdef __cplusplus } #endif diff --git a/src/robofsm/common-states.cc b/src/robofsm/common-states.cc index b417f8d6..32d3e6e7 100644 --- a/src/robofsm/common-states.cc +++ b/src/robofsm/common-states.cc @@ -56,7 +56,7 @@ void set_initial_position() void actuators_home() { - act_jaws(OPEN); + ; } /* Check if the new point is within the playground scene */ diff --git a/src/robofsm/robot_orte.c b/src/robofsm/robot_orte.c index e1b304ae..d68cf551 100644 --- a/src/robofsm/robot_orte.c +++ b/src/robofsm/robot_orte.c @@ -368,61 +368,6 @@ void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, * SUBSCRIBER CALLBACKS - EB2008 * ---------------------------------------------------------------------- */ -void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, - void *recvCallBackParam) -{ - 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.left == 0 && - instance->flags.right == 0) - robot.status[COMPONENT_JAWS] = STATUS_OK; - else - robot.status[COMPONENT_JAWS] = STATUS_WARNING; - - 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_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) @@ -522,9 +467,6 @@ int robot_init_orte() 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_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_motion_irc_create(&robot.orte, rcv_motion_irc_cb, &robot.orte); @@ -534,8 +476,6 @@ int robot_init_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_sick_scan_create(&robot.orte, rcv_sick_scan_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_robot_bumpers_create(&robot.orte, rcv_robot_bumpers_cb, &robot.orte); robottype_subscriber_camera_result_create(&robot.orte, rcv_camera_result_cb, &robot.orte); diff --git a/src/robofsm/sick-day.cc b/src/robofsm/sick-day.cc index 5fa6cc8e..a3ffabe6 100644 --- a/src/robofsm/sick-day.cc +++ b/src/robofsm/sick-day.cc @@ -115,7 +115,6 @@ FSM_STATE(wait_for_start) case EV_RETURN: case EV_MOTION_ERROR: case EV_MOTION_DONE: - case EV_JAWS_DONE: DBG_PRINT_EVENT("unhandled event"); break; case EV_EXIT: diff --git a/src/robofsm/sub-states.cc b/src/robofsm/sub-states.cc index 57e667d7..b4442c6a 100644 --- a/src/robofsm/sub-states.cc +++ b/src/robofsm/sub-states.cc @@ -200,14 +200,11 @@ FSM_STATE(get_target_load) switch(FSM_EVENT) { case EV_ENTRY: - act_jaws(CATCH); - FSM_TIMER(1000); break; case EV_TIMER: if (++delay > 3) { // target was not confirmed DBG_PRINT_EVENT("OMRON: Target NOT valid!"); - act_jaws(OPEN); robot.target_loaded = false; robot.target_valid = false; FSM_TRANSITION(get_target_unload); @@ -231,24 +228,14 @@ FSM_STATE(get_target_load) /* unload target if not valid = no color match using OMRON */ FSM_STATE(get_target_unload) { - static int direction = 0; - switch(FSM_EVENT) { case EV_ENTRY: - direction = 0; - act_jaws(OPEN); break; case EV_TIMER: - act_jaws(CATCH); robot.target_loaded = true; FSM_TRANSITION(get_target_back); break; - case EV_JAWS_DONE: - if (direction == 0) { - direction = 1; - FSM_TIMER(2000); - } - break; + case EV_EXIT: break; } @@ -262,8 +249,6 @@ FSM_STATE(get_target_back) break; case EV_TIMER: break; - case EV_JAWS_DONE: - break; case EV_MOTION_DONE: case EV_MOTION_ERROR: SUBFSM_RET(NULL); diff --git a/src/robomon/RobomonAtlantis.cpp b/src/robomon/RobomonAtlantis.cpp index 38beaf9c..058d364d 100644 --- a/src/robomon/RobomonAtlantis.cpp +++ b/src/robomon/RobomonAtlantis.cpp @@ -591,12 +591,6 @@ bool RobomonAtlantis::event(QEvent *event) case QEVENT(QEV_SICK_SCAN): hokuyoScan->newScan(&orte.sick_scan); break; - case QEVENT(QEV_JAWS_CMD): - robotEstPosBest->setJaws(orte.jaws_cmd.req_pos.left); - robotRefPos->setJaws(orte.jaws_cmd.req_pos.left); - robotEstPosIndepOdo->setJaws(orte.jaws_cmd.req_pos.left); - robotEstPosOdo->setJaws(orte.jaws_cmd.req_pos.left); - break; case QEVENT(QEV_REFERENCE_POSITION): emit actualPositionReceivedSignal(); break; @@ -805,8 +799,6 @@ void RobomonAtlantis::createOrte() generic_rcv_cb, new OrteCallbackInfo(this, QEV_ESTIMATED_POSITION_BEST)); robottype_subscriber_sick_scan_create(&orte, generic_rcv_cb, new OrteCallbackInfo(this, QEV_SICK_SCAN)); - robottype_subscriber_jaws_cmd_create(&orte, - generic_rcv_cb, new OrteCallbackInfo(this, QEV_JAWS_CMD)); robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, this); robottype_subscriber_fsm_motion_create(&orte, diff --git a/src/robomon/Robot.cpp b/src/robomon/Robot.cpp index c2c273c9..4df97fed 100644 --- a/src/robomon/Robot.cpp +++ b/src/robomon/Robot.cpp @@ -25,7 +25,6 @@ Robot::Robot(const QString &text, const QPen &pen, const QBrush &brush) : this->pen = pen; this->brush = brush; setVisible(false); - setJaws(JAW_LEFT_CLOSE); moveRobot(ROBOT_START_X_M, ROBOT_START_Y_M, 0); } @@ -61,33 +60,8 @@ void Robot::paint(QPainter *painter, QLineF(xa, yb, xa+xd, yc)}; painter->drawLines(arrow, 3); - QLineF jawsLines[2]; /* field witch jaws lines */ const int offset = 40; /* offset of point of rotation from robot sides [mm] */ - const int hold = 40; /* offset from open position [mm] */ - const int close = 110; /* offset from open positon [mm] */ - const double y_hold = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(hold, 2)); - const double y_close = sqrt(pow(ROBOT_JAWS_LENGHT_MM, 2) - pow(close, 2)); - switch (jawsPosition) { - case JAW_LEFT_OPEN: - jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM); - jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM); - break; - - case JAW_LEFT_CATCH: - jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + hold, ROBOT_HEIGHT_MM + ROBOT_JAWS_LENGHT_MM); - jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - hold, ROBOT_HEIGHT_MM + y_hold); - break; - - case JAW_LEFT_CLOSE: - jawsLines[0] = QLineF(offset, ROBOT_HEIGHT_MM, offset + close, ROBOT_HEIGHT_MM + y_close); - jawsLines[1] = QLineF(ROBOT_WIDTH_MM - offset , ROBOT_HEIGHT_MM, ROBOT_WIDTH_MM - offset - close, ROBOT_HEIGHT_MM + y_close); - break; - } - - QPen penThick(Qt::black, 20, Qt::SolidLine); /* new pen with 20px thickness for jaws drawing */ - painter->setPen(penThick); - painter->drawLines(jawsLines, 2); /* draw jaws */ painter->setPen(pen); /* set pen to previous slim pen */ painter->setFont(QFont("Arial", 40, 0, 0)); @@ -112,11 +86,3 @@ void Robot::mySetVisible(bool show) { setVisible(show); } - -void Robot::setJaws(int value) -{ - QRectF r = boundingRect(); - //r.setBottom(0); - jawsPosition = value; - update(r); -} diff --git a/src/robomon/Robot.h b/src/robomon/Robot.h index 3e68a8ac..7605acc0 100644 --- a/src/robomon/Robot.h +++ b/src/robomon/Robot.h @@ -29,9 +29,7 @@ public: void moveRobot(double x, double y, double angle); public slots: void mySetVisible(bool show); - void setJaws(int value); private: - int jawsPosition; QString text; QPen pen; QBrush brush; diff --git a/src/types/robottype.idl b/src/types/robottype.idl index 8133653e..d3c03303 100644 --- a/src/types/robottype.idl +++ b/src/types/robottype.idl @@ -103,17 +103,6 @@ struct pos_octet { }; // Actuators -struct jaws_cmd { - pos_short req_pos; - pos_octet speed; -}; - -struct lift_cmd { - unsigned short req_pos; - octet speed; - octet homing; -}; - struct cl_sensor_cmd { octet bank_nbr; }; @@ -124,12 +113,6 @@ struct cl_sensor_status { /** Status sent from actuators */ -struct jaws_status { - pos_short act_pos; - pos_short response; - pos_octet flags; -}; - struct lift_status { unsigned short act_pos; unsigned short response; // Equals to req_pos when the move is done diff --git a/src/types/robottype.ortegen b/src/types/robottype.ortegen index 55280903..574e5aa4 100644 --- a/src/types/robottype.ortegen +++ b/src/types/robottype.ortegen @@ -2,10 +2,6 @@ type=cl_sensor_status topic=cl_sensor_status deadline=0.5 type=cl_sensor_cmd topic=cl_sensor_cmd -type=jaws_status topic=jaws_status deadline=0.5 -type=jaws_cmd topic=jaws_cmd -type=lift_status topic=lift_status deadline=0.5 -type=lift_cmd topic=lift_cmd type=match_time topic=match_time deadline=0.5 type=can_msg topic=can_msg type=robot_pos topic=est_pos_odo -- 2.39.2