lift: Implemented P controller

diff --git a/src/eb_lift_09/def.h b/src/eb_lift_09/def.h
index 118ab39fee0ab9c9880d87cf0bc92d887521effe..b9fdb5a77aa65d0d30a1567c355fdc4215508935 100644 (file)
--- a/src/eb_lift_09/def.h
+++ b/src/eb_lift_09/def.h
@@ -82,8 +82,8 @@
                bool switch_front;                      // up or front end switch
                bool switch_back;                       // down or back end switch
                uint8_t init_flag;                      // init status flag
-               uint32_t p, i;                          ///< PI controller constants * 1000
-               int32_t sum;                            ///< PI controller integrator state
+               uint32_t p;                             ///< PI controller constants * 1000
+               uint32_t last_move_time;                ///< Last timer_msec when act_pos changed
        };
 
        extern volatile uint32_t timer_msec;

diff --git a/src/eb_lift_09/lift.c b/src/eb_lift_09/lift.c
index 992289804b6bceaa2b479e9a75216632a51c06d7..261c44facfcc3af60435e4dd5c52a8790f6d6e16 100644 (file)
--- a/src/eb_lift_09/lift.c
+++ b/src/eb_lift_09/lift.c
@@ -79,11 +79,15 @@ void move_lift(uint8_t direct,uint8_t velocity){    //how many pulzes
 void fsm_irc(struct fsm *fsm)
 {
        int32_t irc = *fsm->irc ;
-  
+
        if(fsm->motor_dir == ENGINE_DIR_FW)
                fsm->act_pos += irc - fsm->last_irc;
        else
                fsm->act_pos -= irc - fsm->last_irc;
+
+       if (irc != fsm->last_irc) 
+               fsm->last_move_time = timer_msec;
+
        
        fsm->last_irc = irc;
        
@@ -107,6 +111,7 @@ void fsm_init(struct fsm *fsm, events my_event)
                  fsm->motor_dir = ENGINE_DIR_FW;
                  fsm->lenght = 0;
                  fsm->ans_can = 0;
+                 fsm->p = 1000;
                  
                  if(fsm->type == FSM_LIFT)
                  {
@@ -132,12 +137,13 @@ void fsm_init(struct fsm *fsm, events my_event)
                  }
                  
                  fsm->last_irc =  *fsm->irc;
+                 can_flags |= fsm->init_flag;
                break;
                
                case EVENT_DO:
                  
                    if(fsm_homing_block == fsm->type)                   // block lift until release fsm_homing_block
-                       fsm->current_state = &fsm_homing_fw;    
+                       fsm->current_state = &fsm_homing_bw;    
                    break;
                    
                case EVENT_EXIT:
@@ -157,17 +163,16 @@ void fsm_homing_fw(struct fsm *fsm, events my_event){
 
     switch(my_event){
                case EVENT_ENTRY:
-                       can_flags |= fsm->init_flag;
                        PRINT_STR(fsm->type,"FSM_HOMING_FW: EVENT_ENTRY  \n");
                        fsm->motor_dir = ENGINE_DIR_FW;
                        fsm->set_engine(fsm->motor_dir, fsm->speed_home);
-                       fsm->timeout_glob = time_ms;
+                       fsm->timeout_glob = timer_msec;
                        
                break;  
                        
                case EVENT_DO:
                  
-                       if(fsm->switch_front) // || (fsm->timeout_glob + TIME_HOMING_WAIT < time_ms))
+                       if(fsm->switch_front) // || (fsm->timeout_glob + TIME_HOMING_WAIT < timer_msec))
                        {
                          fsm->set_engine(fsm->motor_dir, ENG_STOP);
                          fsm->act_pos = 0;                             // reset position
@@ -192,8 +197,8 @@ void fsm_homing_bw(struct fsm *fsm, events my_event){
                        PRINT_STR(fsm->type,"FSM_HOMING_BW: EVENT_ENTRY  \n");
                        fsm->motor_dir = ENGINE_DIR_BW;
                        fsm->set_engine(ENGINE_DIR_BW, fsm->speed_home);
-                       fsm->timeout_switch = time_ms;
-                       fsm->timeout_glob = time_ms;            // FIXME nevim jestli je potreba
+                       fsm->timeout_switch = timer_msec;
+                       fsm->timeout_glob = timer_msec;         // FIXME nevim jestli je potreba
                break;  
                        
                case EVENT_DO:
@@ -228,13 +233,13 @@ void fsm_homing_fw_up(struct fsm *fsm, events my_event){
                        PRINT_STR(fsm->type,"FSM_HOMING_FW_UP: EVENT_ENTRY  \n");
                        fsm->motor_dir = ENGINE_DIR_FW;
                        fsm->set_engine(fsm->motor_dir, fsm->speed_home);
-                       fsm->timeout_glob = time_ms;
+                       fsm->timeout_glob = timer_msec;
                        
                break;  
                        
                case EVENT_DO:
                  
-                       if(fsm->switch_front || (fsm->timeout_glob + 1000 < time_ms))
+                       if(fsm->switch_front || (fsm->timeout_glob + 1000 < timer_msec))
                        {
                          fsm->set_engine(fsm->motor_dir, ENG_STOP);                      
                          fsm->current_state = &fsm_homing_bw_zero;     // move to fsm_homing_bw
@@ -259,8 +264,8 @@ void fsm_homing_bw_zero(struct fsm *fsm, events my_event){
                        PRINT_STR(fsm->type,"FSM_HOMING_BW_ZERO: EVENT_ENTRY  \n");
                        fsm->motor_dir = ENGINE_DIR_BW;
                        fsm->set_engine(ENGINE_DIR_BW, fsm->speed_slow);
-                       fsm->timeout_switch = time_ms;
-                       fsm->timeout_glob = time_ms;            // FIXME nevim jestli je potreba
+                       fsm->timeout_switch = timer_msec;
+                       fsm->timeout_glob = timer_msec;         // FIXME nevim jestli je potreba
                break;  
                        
                case EVENT_DO:
@@ -300,7 +305,7 @@ void fsm_homing_fw_5(struct fsm *fsm, events my_event){
                        PRINT_STR(fsm->type,"FSM_HOMING_FW_5: EVENT_ENTRY  \n");
                        fsm->motor_dir = ENGINE_DIR_FW;
                        fsm->set_engine(fsm->motor_dir, fsm->speed_slow);
-                       fsm->timeout_glob = time_ms;
+                       fsm->timeout_glob = timer_msec;
                        
                break;  
                        
@@ -349,7 +354,9 @@ void fsm_stop(struct fsm *fsm, events my_event){
                        
                case EVENT_DO:
                  
-                   if(tbit(fsm->status,STAT_NEW_POS))
+                       //fsm->ans_can = fsm->act_pos; /* TODO remove this line */
+
+                 if(tbit(fsm->status,STAT_NEW_POS))
                    {
                      cbit(fsm->status,STAT_NEW_POS);
                      fsm->req_pos = fsm->can_req_position;
@@ -382,8 +389,7 @@ void fsm_move(struct fsm *fsm, events my_event){
                        fsm->motor_dir = ENGINE_DIR_FW;
                  }
                  
-                 fsm->timeout_switch = time_ms;
-                 fsm->timeout_glob = time_ms;          // FIXME nevim jestli je potreba
+                 fsm->last_move_time = timer_msec;
                  
                  PRINT_STR_VAL(fsm->type, "Act pos: ", (uint32_t)fsm->act_pos);
                  PRINT_STR_VAL(fsm->type, "fsm->req_pos: ", (uint32_t)fsm->req_pos);
@@ -400,13 +406,15 @@ void fsm_move(struct fsm *fsm, events my_event){
                  if ((fsm->motor_dir == ENGINE_DIR_BW && fsm->switch_back) ||
                      (fsm->motor_dir == ENGINE_DIR_FW && fsm->switch_front)) {
                        fsm->set_engine(fsm->motor_dir, ENG_STOP);
-                       fsm->current_state = &fsm_stop; // move to fsm_homing_bw
-               //      fsm->ans_can = fsm->act_pos;
+                       fsm->current_state = &fsm_wait; // move to fsm_homing_bw
+                       fsm->ans_can = fsm->req_pos;
                        can_send_status();
                        PRINT_STR(fsm->type,"fsm_move: EVENT_DO reached endswitch \n");
                        break;
                  }     
 
+                 fsm->req_pos = fsm->can_req_position;
+
                  /* Normal movement */
                  if (fsm->motor_dir == ENGINE_DIR_BW)
                          e = fsm->act_pos - fsm->req_pos;
@@ -415,19 +423,20 @@ void fsm_move(struct fsm *fsm, events my_event){
 
                  
                  if (e > 0) {
-                         u = e*fsm->p/1000 + fsm->sum*fsm->i/1000;
-                         fsm->sum += e;
-                         /* Go there*/
-                         if      (e < 20) u = 30;
-                         else if (e < 50) u = 40;
-                         else             u = 100;
+                         /* Go there (PI controller)*/
+                         u = e*fsm->p/1000;
+                         if (u>100) u = 100;
                          fsm->set_engine(fsm->motor_dir, u /*fsm->speed_normal*/);
                          fsm->ans_can = fsm->act_pos;
-                 } else {
+                 }
+                 
+                 if (e <= 0 || timer_msec - fsm->last_move_time > 100) {
                          /* We are there */
                          fsm->set_engine(fsm->motor_dir, ENG_STOP);
-                         fsm->sum = 0;
-                         fsm->current_state = &fsm_wait;       // move to fsm_homing_bw
+                         if (e <= 0)
+                                 fsm->current_state = &fsm_wait;
+                         else 
+                                 fsm->current_state = &fsm_stop;
                          fsm->ans_can = fsm->req_pos;
                          can_send_status();
                          PRINT_STR(fsm->type,"fsm_move: EVENT_DO reached target \n");
@@ -444,14 +453,14 @@ void fsm_move(struct fsm *fsm, events my_event){
 void fsm_wait(struct fsm *fsm, events my_event){
   switch(my_event){
                case EVENT_ENTRY:
-                 fsm->timeout_glob = time_ms;
+                 fsm->timeout_glob = timer_msec;
                  PRINT_STR(fsm->type,"FSM_WAIT: EVENT_ENTRY  \n");
                break;  
                        
                case EVENT_DO:
                  
-                  fsm->ans_can = fsm->act_pos;
-                 if((fsm->timeout_glob + TIME_WAIT) >+ time_ms)
+                       //fsm->ans_can = fsm->act_pos; /* TODO remove this line */
+                 if((fsm->timeout_glob + TIME_WAIT) >+ timer_msec)
                  {
                    PRINT_STR(fsm->type,"FSM_WAIT: EVENT_DO wait time done \n");
                    fsm->current_state = &fsm_stop;     // move to fsm_homing_bw