src/eb_lift_11/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_dir(ENGINE_DIR_FW);
  36                 engine_A_pwm(20);
  37
  38                 if(fsm->flags & CAN_LIFT_SWITCH_DOWN){
  39                   stop();
  40                   fsm->act_pos=0;
  41                   fsm->current_state = wait_for_cmd;
  42                   fsm->flags |= CAN_LIFT_HOMED;
  43                   fsm->trigger_can_send=1;
  44                   fsm->can_req_homing=0;
  45 //                   fsm->can_req_position = 0x54E2;
  46                 }
  47                 else if (timer_msec >= (time_start + 8000)) {
  48                   stop();
  49                   fsm->current_state = wait_for_cmd;
  50                   fsm->flags |= CAN_LIFT_TIMEOUT;
  51                   fsm->flags &= ~CAN_LIFT_HOMED;
  52                   fsm->trigger_can_send=1;
  53                   fsm->can_req_homing=0;
  54                 }
  55                 break;
  56         case EVENT_EXIT:
  57                 break;
  58         }
  59 }
  60
  61 void fsm_lift_init(struct fsm *fsm, enum event event)
  62 {
  63         switch (event) {
  64         case EVENT_ENTRY:
  65                 DBG_ENTRY();
  66                 fsm->flags |= CAN_LIFT_INITIALIZING;
  67                 break;
  68         case EVENT_DO:
  69                 fsm->flags &= ~CAN_LIFT_INITIALIZING;
  70                 fsm->current_state = wait_for_cmd;
  71                 break;
  72         case EVENT_EXIT:
  73                 break;
  74         }
  75 }
  76
  77 static void stop()
  78 {
  79         engine_A_pwm(0);
  80         engine_A_en(ENGINE_EN_OFF);
  81 }
  82
  83
  84 #define DEAD_ZONE       100
  85 static bool do_control(struct fsm *fsm, int P)
  86 {
  87         bool finished;
  88         int e = fsm->req_pos - fsm->act_pos;
  89         int action = (P*e) / 20;                // ORIGINAL: int action = P*e;
  90
  91         engine_A_dir(action > 0);
  92         action = action > 100 ? 100 : action;
  93
  94 //#define BANG_BANG
  95 #ifdef BANG_BANG
  96         action = action > 0 ? action : -action;
  97         action = action > 40 ? 100 : 0;
  98 #else
  99         action = action > 0 ? action : -action;
 100 #endif
 101
 102         engine_A_pwm(action);
 103         engine_A_en(ENGINE_EN_ON);
 104
 105         if (fsm->req_target > fsm->start_pos)
 106                 finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
 107         else
 108                 finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
 109
 110         return finished;
 111 }
 112
 113 static void wait_for_cmd(struct fsm *fsm, enum event event)
 114 {
 115         static int last_can_req_pos = 0;
 116         switch (event) {
 117         case EVENT_ENTRY:
 118                 DBG_ENTRY();
 119                 stop();
 120                 break;
 121         case EVENT_DO:
 122                 //homing if push home button
 123                 if (fsm->flags & CAN_LIFT_SWITCH_HOME){
 124                   fsm->current_state=homing;
 125                   break;
 126                 }
 127                 //homing if match start, but did not homing before start
 128                 if ((fsm->can_req_homing) && !(fsm->flags & CAN_LIFT_HOMED)){
 129                   fsm->current_state=homing;
 130                   break;
 131                 }
 132                 do_control(fsm, 2);
 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                 break;
 141         case EVENT_EXIT:
 142                 break;
 143         }
 144 }
 145
 146 #define XMIN(a,b) ((a) < (b) ? (a) : (b))
 147 #define XMAX(a,b) ((a) > (b) ? (a) : (b))
 148 static void move(struct fsm *fsm, enum event event)
 149 {
 150         static int counter;
 151         static bool lift_stopped_on_end = false;
 152         bool finished;
 153         switch (event) {
 154         case EVENT_ENTRY:
 155                 counter = 0;
 156                 DBG_ENTRY();
 157                 fsm->time_start = timer_msec;
 158                 fsm->start_pos = fsm->act_pos;
 159                 if(fsm->req_spd == 0)
 160                         fsm->req_pos = fsm->req_target;
 161                 else
 162                         fsm->req_pos = fsm->start_pos;
 163                 break;
 164         case EVENT_DO:
 165                 if ((fsm->flags & CAN_LIFT_SWITCH_UP) && (fsm->can_response != fsm->req_target)){
 166                        if(!lift_stopped_on_end){
 167                           fsm->can_response = fsm->req_target;
 168                           fsm->current_state = wait_for_cmd;
 169                           fsm->req_pos = fsm->act_pos;
 170                           lift_stopped_on_end=true;
 171                        }
 172                        else{
 173                          lift_stopped_on_end=false;
 174                       }
 175                 }
 176                 if ((fsm->flags & CAN_LIFT_SWITCH_DOWN) && (fsm->can_response != fsm->req_target)) {
 177                        if(!lift_stopped_on_end){
 178                           fsm->can_response = fsm->req_target;
 179                           fsm->current_state = wait_for_cmd;
 180                           fsm->act_pos=0;
 181                           fsm->req_pos = fsm->act_pos;
 182                           lift_stopped_on_end=true;
 183                        }
 184                        else{
 185                          lift_stopped_on_end=false;
 186                       }
 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                 if(fsm->req_spd != 0 && counter++ >= 10)
 194                 {
 195                         counter = 0;
 196                         if(fsm->req_target > fsm->start_pos) {
 197                                   fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
 198                         } else {
 199                                   fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
 200                         }
 201                 }
 202                 if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 3000)) {
 203                         fsm->flags |= CAN_LIFT_TIMEOUT;
 204                         fsm->can_response = fsm->req_target;
 205                         fsm->current_state = wait_for_cmd;
 206                         fsm->req_pos = fsm->act_pos;
 207                 }
 208                 finished = do_control(fsm, fsm->req_spd ? 2 : 2);
 209                 if (finished && fsm->req_pos == fsm->req_target) {
 210                         fsm->can_response = fsm->req_target;
 211                         fsm->current_state = wait_for_cmd;
 212                 }
 213                 break;
 214         case EVENT_EXIT:
 215                 stop();
 216                 fsm->trigger_can_send = true;;
 217                 break;
 218         }
 219 }