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
  21
  22 void fsm_lift_init(struct fsm *fsm, enum event event)
  23 {
  24         switch (event) {
  25         case EVENT_ENTRY:
  26                 DBG_ENTRY();
  27                 fsm->can_req_position = fsm->act_pos;
  28                 fsm->flags |= CAN_LIFT_INITIALIZING;
  29                 break;
  30         case EVENT_DO:
  31                 //homing, TODO add timeout
  32                 while(IO0PIN & (1<<END_SWITCH_UP)){
  33                   engine_A_pwm(50);
  34                 }
  35                 engine_A_pwm(0);
  36                 fsm->act_pos=0;
  37                 fsm->flags &= ~CAN_LIFT_INITIALIZING;
  38                 fsm->current_state = wait_for_cmd;
  39                 break;
  40         case EVENT_EXIT:
  41                 break;
  42         }
  43 }
  44
  45 static void stop()
  46 {
  47         engine_A_pwm(0);
  48         engine_A_en(ENGINE_EN_OFF);
  49 }
  50
  51
  52 #define DEAD_ZONE       10
  53 static bool do_control(struct fsm *fsm, int P)
  54 {
  55         bool finished;
  56         int e = fsm->req_pos - fsm->act_pos;
  57         int action = (P*e) / 10;                // ORIGINAL: int action = P*e;
  58
  59         engine_A_dir(action < 0);
  60 //#define BANG_BANG
  61 #ifdef BANG_BANG
  62         action = action > 0 ? action : -action;
  63         action = action > 40 ? 100 : 0;
  64 #else
  65         action = action > 0 ? action : -action;
  66 #endif
  67         if((IO0PIN & (1<<END_SWITCH_UP))&&(IO0PIN & (1<<END_SWITCH_DOWN))){
  68           engine_A_pwm(action);
  69           engine_A_en(ENGINE_EN_ON);
  70         }else {
  71           stop();
  72         }
  73
  74         if (fsm->req_target > fsm->start_pos)
  75                 finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
  76         else
  77                 finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
  78
  79         return finished;
  80 }
  81
  82 static void wait_for_cmd(struct fsm *fsm, enum event event)
  83 {
  84         static int last_can_req_pos = 0;
  85         switch (event) {
  86         case EVENT_ENTRY:
  87                 DBG_ENTRY();
  88                 stop();
  89                 break;
  90         case EVENT_DO:
  91                 do_control(fsm, 2);
  92                 if (fsm->can_req_position != last_can_req_pos &&
  93                     fsm->can_req_position != fsm->req_target) {
  94                         last_can_req_pos = fsm->can_req_position;
  95                         fsm->req_target = fsm->can_req_position;
  96                         fsm->req_spd = fsm->can_req_spd;
  97                         fsm->current_state = move;
  98                 }
  99                 break;
 100         case EVENT_EXIT:
 101                 break;
 102         }
 103 }
 104
 105 #define XMIN(a,b) ((a) < (b) ? (a) : (b))
 106 #define XMAX(a,b) ((a) > (b) ? (a) : (b))
 107 static void move(struct fsm *fsm, enum event event)
 108 {
 109         static int counter;
 110         bool finished;
 111         switch (event) {
 112         case EVENT_ENTRY:
 113                 counter = 0;
 114                 DBG_ENTRY();
 115                 fsm->time_start = timer_msec;
 116                 fsm->start_pos = fsm->act_pos;
 117                 if(fsm->req_spd == 0)
 118                         fsm->req_pos = fsm->req_target;
 119                 else
 120                         fsm->req_pos = fsm->start_pos;
 121                 break;
 122         case EVENT_DO:
 123                 if (fsm->can_req_position != fsm->req_target) {
 124                         fsm->flags |= CAN_LIFT_TIMEOUT;
 125                         fsm->current_state = wait_for_cmd;
 126                 }
 127                 if(fsm->req_spd != 0 && counter++ >= 10)
 128                 {
 129                         counter = 0;
 130                         if(fsm->req_target > fsm->start_pos) {
 131                                   fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
 132                         } else {
 133                                   fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
 134                         }
 135                 }
 136                 if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 3000)) {
 137                         fsm->flags |= CAN_LIFT_TIMEOUT;
 138                         fsm->can_response = fsm->req_target;
 139                         fsm->current_state = wait_for_cmd;
 140                         fsm->req_pos = fsm->act_pos;
 141                 }
 142                 finished = do_control(fsm, fsm->req_spd ? 2 : 2);
 143                 if (finished && fsm->req_pos == fsm->req_target) {
 144                         fsm->can_response = fsm->req_target;
 145                         fsm->current_state = wait_for_cmd;
 146                 }
 147                 break;
 148         case EVENT_EXIT:
 149                 stop();
 150                 fsm->trigger_can_send = true;;
 151                 break;
 152         }
 153 }