src/eb_lift/fsm_lift.c

   1 #include <lpc21xx.h>
   2 #include <deb_led.h>
   3 #include <system_def.h>
   4 #include <string.h>
   5 #include <can_msg_def.h>
   6 #include "uar.h"
   7 #include "fsm.h"
   8 #include <engine.h>
   9 #include <stdbool.h>
  10 #include "def.h"
  11 #include <adc.h>
  12
  13 #define DBG_ENTRY() do {                        \
  14                 send_rs_str(__func__);          \
  15                 send_rs_str(": entry\n");       \
  16         } while(0);
  17
  18 static void wait_for_cmd(struct fsm *fsm, enum event event);
  19 static void move(struct fsm *fsm, enum event event);
  20 static void homing(struct fsm *fsm, enum event event);
  21 static void stop(void);
  22
  23
  24 static void homing(struct fsm *fsm, enum event event)
  25 {
  26         static uint32_t time_start = 0;
  27
  28         switch (event) {
  29         case EVENT_ENTRY:
  30                 DBG_ENTRY();
  31                 time_start = timer_msec;
  32                 break;
  33         case EVENT_DO:
  34                 //homing with 8s timeout
  35                 engine_A_en(ENGINE_EN_ON);
  36                 engine_A_dir(ENGINE_DIR_FW);
  37                 engine_A_pwm(50);
  38
  39                 if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
  40                         engine_A_pwm(0);
  41                         fsm->act_pos = 0;
  42                         fsm->current_state = wait_for_cmd;
  43                         fsm->flags |= CAN_LIFT_HOMED;
  44                         fsm->trigger_can_send = 1;
  45                         fsm->can_req_homing = 0;
  46                         fsm->can_req_position = 0;
  47                         //                fsm->can_req_position = 0x54E2;
  48                 } else if (timer_msec >= (time_start + 4000)) {
  49                         stop();
  50                         fsm->current_state = wait_for_cmd;
  51                         fsm->flags |= CAN_LIFT_TIMEOUT;
  52                         fsm->flags &= ~CAN_LIFT_HOMED;
  53                         fsm->trigger_can_send = 1;
  54                         fsm->can_req_homing = 0;
  55                 }
  56                 break;
  57         case EVENT_EXIT:
  58                 break;
  59         }
  60 }
  61
  62 void fsm_lift_init(struct fsm *fsm, enum event event)
  63 {
  64         switch (event) {
  65         case EVENT_ENTRY:
  66                 DBG_ENTRY();
  67                 fsm->flags |= CAN_LIFT_INITIALIZING;
  68                 break;
  69         case EVENT_DO:
  70                 fsm->flags &= ~CAN_LIFT_INITIALIZING;
  71                 fsm->current_state = wait_for_cmd;
  72                 break;
  73         case EVENT_EXIT:
  74                 break;
  75         }
  76 }
  77
  78 static void stop()
  79 {
  80         engine_A_pwm(0);
  81         engine_A_en(ENGINE_EN_OFF);
  82 }
  83
  84
  85 #define DEAD_ZONE       10
  86 static bool do_control(struct fsm *fsm, int P)
  87 {
  88         bool finished;
  89         int e = fsm->req_pos - fsm->act_pos;
  90         int action = (P*e) / 5;         // ORIGINAL: int action = P*e;
  91
  92         engine_A_dir(action > 0);
  93         action = action > 100 ? 100 : action;
  94
  95 //#define BANG_BANG
  96 #ifdef BANG_BANG
  97         action = action > 0 ? action : -action;
  98         action = action > 40 ? 100 : 0;
  99 #else
 100         action = action > 0 ? action : -action;
 101 #endif
 102
 103         engine_A_pwm(action);
 104         engine_A_en(ENGINE_EN_ON);
 105
 106         if (fsm->req_target > fsm->start_pos)
 107                 finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
 108         else
 109                 finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
 110
 111         return finished;
 112 }
 113
 114 static void wait_for_cmd(struct fsm *fsm, enum event event)
 115 {
 116         static int last_can_req_pos = 0;
 117         switch (event) {
 118         case EVENT_ENTRY:
 119                 DBG_ENTRY();
 120                 stop();
 121                 break;
 122         case EVENT_DO:
 123                 //homing if push home button or homing can msg. received
 124                 if ((fsm->flags & CAN_LIFT_SWITCH_HOME) || (fsm->can_req_homing)) {
 125                         fsm->can_req_homing = 0;
 126                         fsm->current_state = homing;
 127                         break;
 128                 }
 129                 /* if lift is not homed, do not allow movement and regulation */
 130                 if (fsm->flags & CAN_LIFT_HOMED) {
 131                         do_control(fsm, 2);
 132
 133                         if (fsm->can_req_position != last_can_req_pos &&
 134                                 fsm->can_req_position != fsm->req_target) {
 135                                 last_can_req_pos = fsm->can_req_position;
 136                                 fsm->req_target = fsm->can_req_position;
 137                                 fsm->req_spd = fsm->can_req_spd;
 138                                 fsm->current_state = move;
 139                         }
 140                 }
 141                 break;
 142         case EVENT_EXIT:
 143                 break;
 144         }
 145 }
 146
 147 #define XMIN(a,b) ((a) < (b) ? (a) : (b))
 148 #define XMAX(a,b) ((a) > (b) ? (a) : (b))
 149 static void move(struct fsm *fsm, enum event event)
 150 {
 151         static int counter;
 152         static bool lift_stopped_on_end = false;
 153         bool finished;
 154         switch (event) {
 155         case EVENT_ENTRY:
 156                 counter = 0;
 157                 DBG_ENTRY();
 158                 fsm->time_start = timer_msec;
 159                 fsm->start_pos = fsm->act_pos;
 160
 161                 /* check if movement starts on end switch */
 162                 if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) || (fsm->flags & CAN_LIFT_SWITCH_UP))
 163                         lift_stopped_on_end = true;
 164                 else
 165                         lift_stopped_on_end = false;
 166
 167                 if(fsm->req_spd == 0)
 168                         fsm->req_pos = fsm->req_target;
 169                 else
 170                         fsm->req_pos = fsm->start_pos;
 171                 break;
 172         case EVENT_DO:
 173                 /* if movement starts on end switch, ignore this, else stop movement on act position */
 174                 if ((fsm->flags & CAN_LIFT_SWITCH_UP) && !lift_stopped_on_end){
 175                         fsm->can_response = fsm->req_target;
 176                         fsm->current_state = wait_for_cmd;
 177                         fsm->req_pos = fsm->act_pos;
 178                         lift_stopped_on_end = true;
 179                 }
 180                 /* if movement starts on end switch, ignore this, else stop movement on act position */
 181                 if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && !lift_stopped_on_end) {
 182                         fsm->can_response = fsm->req_target;
 183                         fsm->current_state = wait_for_cmd;
 184                         fsm->act_pos = 0;
 185                         fsm->req_pos = fsm->act_pos;
 186                         lift_stopped_on_end = true;
 187                 }
 188
 189                 if (fsm->can_req_position != fsm->req_target) {
 190                         fsm->flags |= CAN_LIFT_TIMEOUT;
 191                         fsm->current_state = wait_for_cmd;
 192                 }
 193
 194                 if(fsm->req_spd != 0 && counter++ >= 10)
 195                 {
 196                         counter = 0;
 197                         if(fsm->req_target > fsm->start_pos) {
 198                                   fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
 199                         } else {
 200                                   fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
 201                         }
 202                 }
 203
 204                 if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 2000 : 4000)) {
 205                         fsm->flags |= CAN_LIFT_TIMEOUT;
 206                         fsm->can_response = fsm->req_target;
 207                         fsm->current_state = wait_for_cmd;
 208                         fsm->req_pos = fsm->act_pos;
 209                 }
 210
 211                 finished = do_control(fsm, fsm->req_spd ? 2 : 2);
 212
 213                 if (finished && fsm->req_pos == fsm->req_target) {
 214                         fsm->can_response = fsm->req_target;
 215                         fsm->current_state = wait_for_cmd;
 216                 }
 217                 break;
 218         case EVENT_EXIT:
 219                 stop();
 220                 fsm->trigger_can_send = true;;
 221                 break;
 222         }
 223 }