vidle: now the fork is moving continuously and it is possible to change speed of...

diff --git a/src/eb_vidle/fsm.h b/src/eb_vidle/fsm.h
index 8e5cbdf1b46664c98b43ca01bcde045b285354fc..56e59de0c32abde57dbf603d4be92748a5912468 100644 (file)
--- a/src/eb_vidle/fsm.h
+++ b/src/eb_vidle/fsm.h
@@ -20,6 +20,9 @@ struct fsm {
        state_fcn last_state;                   // last state
        int32_t act_pos;                        // actual position
        int32_t req_pos;                        // requested position
+       int32_t req_spd;
+       int32_t req_target;
+       volatile int32_t can_req_spd;
        volatile uint32_t can_req_position;     // next requested position
        int32_t start_pos;
        uint32_t can_response;                  // when the move is done, the value here equals to the req_pos

diff --git a/src/eb_vidle/fsm_vidle.c b/src/eb_vidle/fsm_vidle.c
index cd3b093353675f792619f3d87d7bb7b0fe9d8c38..01c2430d95ba78b6e4e1f6901a7142390de6f544 100644 (file)
--- a/src/eb_vidle/fsm_vidle.c
+++ b/src/eb_vidle/fsm_vidle.c
@@ -45,11 +45,10 @@ static void stop()
 
 
 #define DEAD_ZONE      10
-static bool do_control(struct fsm *fsm)
+static bool do_control(struct fsm *fsm, int P)
 {
        bool finished;
        int e = fsm->req_pos - fsm->act_pos;
-       int P = 2;
        int action = (P*e) / 10;                // ORIGINAL: int action = P*e;
 
 
@@ -82,11 +81,12 @@ static void wait_for_cmd(struct fsm *fsm, enum event event)
                stop();
                break;
        case EVENT_DO:
-               do_control(fsm);
+               do_control(fsm, 2);
                if (fsm->can_req_position != last_can_req_pos &&
-                   fsm->can_req_position != fsm->req_pos) {
+                   fsm->can_req_position != fsm->req_target) {
                        last_can_req_pos = fsm->can_req_position;
-                       fsm->req_pos = fsm->can_req_position;
+                       fsm->req_target = fsm->can_req_position;
+                       fsm->req_spd = fsm->can_req_spd;
                        fsm->current_state = move;
                }
                break;
@@ -95,24 +95,40 @@ static void wait_for_cmd(struct fsm *fsm, enum event event)
        }
 }
 
+#define XMIN(a,b) ((a) < (b) ? (a) : (b))
+#define XMAX(a,b) ((a) > (b) ? (a) : (b))
 static void move(struct fsm *fsm, enum event event)
 {
+       static int counter;
        bool finished;
        switch (event) {
        case EVENT_ENTRY:
+               counter = 0;
                DBG_ENTRY();
                fsm->time_start = timer_msec;
                fsm->start_pos = fsm->act_pos;
+               if(fsm->req_spd == 0)
+                       fsm->req_pos = fsm->req_target;
+               else
+                       fsm->req_pos = fsm->start_pos;
                break;
        case EVENT_DO:
-               if (timer_msec - fsm->time_start > 1000) {
+               if(fsm->req_spd != 0 && counter++ >= 10)
+               {
+                       counter = 0;
+                       if(fsm->req_target > fsm->start_pos)
+                                 fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
+                       else
+                                 fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
+               }
+               if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 2000)) {
                        fsm->flags = CAN_VIDLE_TIMEOUT;
                        fsm->current_state = wait_for_cmd;
                        fsm->req_pos = fsm->act_pos;
                }
                        
-               finished = do_control(fsm);
-               if (finished) {
+               finished = do_control(fsm, fsm->req_spd ? 5 : 2);
+               if (finished && fsm->req_pos == fsm->req_target) {
                        fsm->can_response = fsm->req_pos;
                        fsm->current_state = wait_for_cmd;
                }
@@ -122,4 +138,4 @@ static void move(struct fsm *fsm, enum event event)
                fsm->trigger_can_send = true;;
                break;
        }
-}
+}
\ No newline at end of file

diff --git a/src/eb_vidle/main.c b/src/eb_vidle/main.c
index 0acc709865b30dd0decea1009b050f89c2d71adc..009117f7ff95c1f0257140db62e72da9a85c5a92 100644 (file)
--- a/src/eb_vidle/main.c
+++ b/src/eb_vidle/main.c
@@ -151,6 +151,7 @@ void start_button(void)
 
 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 void CAN_rx(can_msg_t *msg) {
+       uint32_t spd;
        can_msg_t rx_msg;
        uint32_t req =0;
        memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
@@ -163,10 +164,12 @@ void CAN_rx(can_msg_t *msg) {
                case CAN_VIDLE_CMD:
                        deb_led_on(LEDB);
                        req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
+                       spd = rx_msg.data[2];
                        
                        if (req >= 0x150 && req <= 0x3e0) {
                                fsm_vidle.flags &= ~CAN_VIDLE_OUT_OF_BOUNDS;
                                fsm_vidle.can_req_position = req;// save new req position of lift
+                               fsm_vidle.can_req_spd = spd;// save new req spd of lift
                        } else
                                fsm_vidle.flags |= CAN_VIDLE_OUT_OF_BOUNDS;
                break;