Complete JAW and LIFT removal

In next step, remove those actuator also from eb-boards.

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 308092dc293a01b2b329516b6d60c3be15852f34..fa5f5f02c5fd28dfc4a8991bc9479990f76d02d7 100644 (file)
--- 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 402393e63382fc4e595a05ba038681f195984103..9b63116c0a688d24a0500a0fe7e9bdaa902f3c03 100644 (file)
--- 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 ced9012b7d263b6a1e53beeed5c7f6441de997cf..70cd06b627ac81250096040454bf359ef1330099 100644 (file)
--- 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 5b16490f8d52dfdedfc31d4e7214ae5cf25d03f8..938a57de221a1f5198b36ec6365d162e3f1352a8 100644 (file)
--- 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 6ac3986c6edb45e96c6d248b05313296a3f2f5f8..b38d8805dfa60eb65ac9d01603537b293e900b60 100644 (file)
--- a/src/display-qt/displayqt.ui
+++ b/src/display-qt/displayqt.ui
@@ -72,26 +72,6 @@
       </property>
      </widget>
     </item>
-    <item>
-     <widget class="QLabel" name="comp_JAW">
-      <property name="font">
-       <font>
-        <pointsize>12</pointsize>
-        <weight>75</weight>
-        <bold>true</bold>
-       </font>
-      </property>
-      <property name="styleSheet">
-       <string notr="true">background-color: rgb(255, 0, 0);</string>
-      </property>
-      <property name="text">
-       <string>JAW</string>
-      </property>
-      <property name="alignment">
-       <set>Qt::AlignCenter</set>
-      </property>
-     </widget>
-    </item>
     <item>
      <widget class="QLabel" name="comp_PWR">
       <property name="font">

diff --git a/src/display-qt/ortesignals.cpp b/src/display-qt/ortesignals.cpp
index 42a8a27dad364e6274fdf30315bdf71c915815d7..2d8c8981f45ef9daad32ce24667394fb9dd7fcbe 100644 (file)
--- 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 33b96cfa4520fc7a608bad5f60f7a5f6f93b7531..1cfde16edd0e1df496dc0a5353d234f3c79d9207 100644 (file)
--- 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 84c5f0b5ac2b932d8ae9343c5b832043b7c52551..dc3b08ca7f119ad30474a3b2a4185b38379d325f 100644 (file)
--- 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 01df0456ec90c98692cd2decbe5dc22edec50238..6c2c2d751939800346256efb0b610cd6cb577bea 100644 (file)
--- 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 7b38bd51156e5093ffa027d3cf37e84394250f7b..1feb9c8860b15dc01a2212b7df535835c4cdf73f 100644 (file)
--- 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 b61cf90548985cb86f59874565091431fcf60b88..6c25e3774bfea4a3a7825436ddace5f063205a98 100644 (file)
--- a/src/robofsm/actuators.h
+++ b/src/robofsm/actuators.h
@@ -33,24 +33,6 @@ of the robot.
 
 #include <stdint.h>
 
-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 b417f8d67435f5b7ff464ddb255e277ebd971bf8..32d3e6e71649dda4d7d4e9f3ef4465d84bac7ea0 100644 (file)
--- 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 e1b304ae772b16c810f2bd859e60e670de1e6654..d68cf551aed98262f852ad520db6bc1fad0ac2ea 100644 (file)
--- 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 5fa6cc8ea4f6f6d2b719d488006ccc2a47444f86..a3ffabe62dda1bf3e78c302f871f605ed5b37697 100644 (file)
--- 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 57e667d72ef76f672d6867eb28e76fbe5dab8057..b4442c6a6463de57aa344a4b18c04499ebec1c2a 100644 (file)
--- 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 38beaf9cfe2fa898cd17b77fda054bcb2c312724..058d364d9ff3ce85e90e450c26c6a6fc42eca7d7 100644 (file)
--- 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 c2c273c98f73a72e9cef168e8f86ce983be88d03..4df97fedd82e3cce87acd89d7b582f5d2fcbf4de 100644 (file)
--- 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 3e68a8ac3784ce4eb4d30650c4a17944d1459a7e..7605acc0a6ddd9002f973cdafcb5a9842fa9083a 100644 (file)
--- 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 8133653ed335af52d4be7d10d4eb11d49b2a43ac..d3c03303e6612be0a8e67d45b98c7f2d23a56cc0 100644 (file)
--- 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 552809031848ca0ee9855d73556bb3a665c4c838..574e5aa47902f06581b7179405bebc394df54344 100644 (file)
--- 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