eb_lift_11: add homing flag to can lift cmd.

diff --git a/src/cand/cand.cc b/src/cand/cand.cc
index 847f5fb315f463af51bb38d027fe7d255fbffb35..41b08b47475820b2f8361a7a53a27f6c7558cb9e 100644 (file)
--- a/src/cand/cand.cc
+++ b/src/cand/cand.cc
@@ -131,14 +131,15 @@ int set_jaws_cmd(struct robottype_orte_data *orte_data)
        return 0;
 }
 
-int set_lift_cmd(uint16_t req_pos, unsigned char speed)
+int set_lift_cmd(struct robottype_orte_data *orte_data)
 {
-       unsigned char data[2];
+       unsigned char data[4];
 
-       data[0] = req_pos >> 8;
-       data[1] = req_pos & 0xff;
-       data[2] = speed;
-       can_send(CAN_LIFT_CMD, 3, data);
+       data[0] = orte_data->req_pos >> 8;
+       data[1] = orte_data->req_pos & 0xff;
+       data[2] = orte_data->speed;
+        data[3] = orte_data->homing;
+       can_send(CAN_LIFT_CMD, 4, data);
 
         return 0;
 }

diff --git a/src/eb_lift_11/fsm.h b/src/eb_lift_11/fsm.h
index d1788c84cabfd9dc5f62d8e4d25ce7ee21f2bfe5..eb9e0b712ea62f7bdf112a731b618dcab3996b20 100644 (file)
--- a/src/eb_lift_11/fsm.h
+++ b/src/eb_lift_11/fsm.h
@@ -29,6 +29,7 @@ struct fsm {
        uint8_t flags;   //< CAN flags bits (defined in can_msg_def.h)
        uint32_t time_start;    /* For timeout detection */
        bool trigger_can_send;
+        uint8_t can_req_homing;
 };
 
 

diff --git a/src/eb_lift_11/fsm_lift.c b/src/eb_lift_11/fsm_lift.c
index d0c01435c08abea6815e44fad15f4ece09ad5b09..0440924d07df318cb02cced90eaf1d00272282b7 100644 (file)
--- a/src/eb_lift_11/fsm_lift.c
+++ b/src/eb_lift_11/fsm_lift.c
@@ -39,6 +39,8 @@ static void homing(struct fsm *fsm, enum event event)
                   fsm->act_pos=0;
                   fsm->current_state = wait_for_cmd;  
                   fsm->flags |= CAN_LIFT_HOMED;
+                  fsm->trigger_can_send=1;
+                  fsm->can_req_homing=0;
 //                   fsm->can_req_position = 0x54E2;
                 }
                 else if (timer_msec >= (time_start + 3000)) {
@@ -47,8 +49,8 @@ static void homing(struct fsm *fsm, enum event event)
                   fsm->flags |= CAN_LIFT_TIMEOUT;
                   fsm->flags &= ~CAN_LIFT_HOMED;
                   fsm->trigger_can_send=1;
-                }
-                                
+                  fsm->can_req_homing=0;
+                }               
                 break;
         case EVENT_EXIT:
                 break;
@@ -120,7 +122,7 @@ static void wait_for_cmd(struct fsm *fsm, enum event event)
                   fsm->current_state=homing;
                 }
                 //homing if match start, but did not homing before start
-                if ((fsm->flags & CAN_LIFT_START) && !(fsm->flags & CAN_LIFT_HOMED)){
+                if ((fsm->can_req_homing) && !(fsm->flags & CAN_LIFT_HOMED)){
                   fsm->current_state=homing;
                 }
                do_control(fsm, 2);

diff --git a/src/eb_lift_11/main.c b/src/eb_lift_11/main.c
index 277c18d216bd784cc125c16d1a8f652710acb36e..9e5266610de265ffe5b8ae21abd963be7758da9e 100644 (file)
--- a/src/eb_lift_11/main.c
+++ b/src/eb_lift_11/main.c
@@ -261,6 +261,7 @@ void CAN_rx(can_msg_t *msg) {
                        deb_led_on(LEDB);
                        req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
                        spd = rx_msg.data[2];
+                        fsm_lift.can_req_homing=rx_msg.data[3];
                        // range 0 - A9C5
                        if (req >= LIFT_IRC_VAL_MIN && req <= LIFT_IRC_VAL_MAX) {
                                fsm_lift.flags &= ~CAN_LIFT_OUT_OF_BOUNDS;

diff --git a/src/types/robottype.idl b/src/types/robottype.idl
index 2795a50bdf069a80b882abd7dc1742f0a49e9789..f7e9045fb281c8ee6f34d1dd9a66f4bc9fb23420 100644 (file)
--- a/src/types/robottype.idl
+++ b/src/types/robottype.idl
@@ -98,6 +98,7 @@ struct jaws_cmd {
 struct lift_cmd {
        unsigned short req_pos;
        octet speed;
+        octet homing;
 };
 
 
@@ -111,7 +112,7 @@ struct jaws_status {
 struct lift_status {
        unsigned short act_pos;
        unsigned short response; // Equals to req_pos when the move is done
-       octet flags;             // Zero when everything OK, otherwise see CAN_VIDLE_FLAG_*
+       octet flags;             // Zero when everything OK, otherwise see CAN_LIFT_FLAG_*
 };
 
 // FIXME: What's this??? M.S.