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