src/eb_jaws_11/fsm_jaws.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_jaws_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_JAWS_INITIALIZING;
  29                 break;
  30         case EVENT_DO:
  31                 /* TODO: Homing */
  32                 fsm->flags &= ~CAN_JAWS_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         engine_B_pwm(0);
  46         engine_B_en(ENGINE_EN_OFF);
  47 }
  48
  49
  50 #define DEAD_ZONE       10
  51 static bool do_control(struct fsm *fsm, int P)
  52 {
  53         bool finished, finished_left, finished_right;
  54         int e_left = fsm->req_pos - fsm->act_pos_left;
  55         int e_right = fsm->req_pos - fsm->act_pos_right;
  56         int action_left = (P*e) / 10;           // ORIGINAL: int action = P*e;
  57         int action_right = (P*e) / 10;
  58
  59         engine_A_dir(action_left< 0);
  60         engine_B_dir(action_right < 0);
  61 //#define BANG_BANG
  62 #ifdef BANG_BANG
  63         action_left = action_left > 0 ? action_left : -action_left;
  64         action_left = action_left > 40 ? 100 : 0;
  65
  66         action_right = action_right > 0 ? action_right : -action_right;
  67         action_right = action_right > 40 ? 100 : 0;
  68 #else
  69         action_left = action_left > 0 ? action_left : -action_left;
  70         action_right= action_right > 0 ? action_right : -action_right;
  71 #endif
  72         engine_A_pwm(action_left);
  73         engine_A_en(ENGINE_EN_ON);
  74
  75         engine_B_pwm(action_right);
  76         engine_B_en(ENGINE_EN_ON);
  77
  78         if (fsm->req_target > fsm->start_pos){
  79                 finished_left = fsm->act_pos_left > fsm->req_target - DEAD_ZONE;
  80                 finished_right = fsm->act_pos_right > fsm->req_target - DEAD_ZONE;
  81         }
  82         else {
  83                 finished_left = fsm->act_pos_left < fsm->req_target + DEAD_ZONE;
  84                 finished_right = fsm->act_pos_right < fsm->req_target + DEAD_ZONE;
  85         }
  86
  87 //      if (fsm->req_target > fsm->start_pos)
  88 //              finished = fsm->act_pos > fsm->req_target - DEAD_ZONE;
  89 //      else
  90 //              finished = fsm->act_pos < fsm->req_target + DEAD_ZONE;
  91 //
  92 //      return finished;
  93
  94         finished=finished_left && finished_right;
  95
  96         return finished;
  97 }
  98
  99 static void wait_for_cmd(struct fsm *fsm, enum event event)
 100 {
 101         static int last_can_req_pos = 0;
 102         switch (event) {
 103         case EVENT_ENTRY:
 104                 DBG_ENTRY();
 105                 stop();
 106                 break;
 107         case EVENT_DO:
 108                 do_control(fsm, 2);
 109                 if (fsm->can_req_position != last_can_req_pos &&
 110                     fsm->can_req_position != fsm->req_target) {
 111                         last_can_req_pos = fsm->can_req_position;
 112                         fsm->req_target = fsm->can_req_position;
 113                         fsm->req_spd = fsm->can_req_spd;
 114                         fsm->current_state = move;
 115                 }
 116                 break;
 117         case EVENT_EXIT:
 118                 break;
 119         }
 120 }
 121
 122 #define XMIN(a,b) ((a) < (b) ? (a) : (b))
 123 #define XMAX(a,b) ((a) > (b) ? (a) : (b))
 124 static void move(struct fsm *fsm, enum event event)
 125 {
 126         static int counter;
 127         bool finished;
 128         switch (event) {
 129         case EVENT_ENTRY:
 130                 counter = 0;
 131                 DBG_ENTRY();
 132                 fsm->time_start = timer_msec;
 133                 fsm->start_pos = fsm->act_pos;
 134                 if(fsm->req_spd == 0)
 135                         fsm->req_pos = fsm->req_target;
 136                 else
 137                         fsm->req_pos = fsm->start_pos;
 138                 break;
 139         case EVENT_DO:
 140                 if (fsm->can_req_position != fsm->req_target) {
 141                         fsm->flags |= CAN_VIDLE_TIMEOUT;
 142                         fsm->current_state = wait_for_cmd;
 143                 }
 144                 if(fsm->req_spd != 0 && counter++ >= 10)
 145                 {
 146                         counter = 0;
 147                         if(fsm->req_target > fsm->start_pos) {
 148                                   fsm->req_pos = XMIN(fsm->req_pos + fsm->req_spd,fsm->req_target);
 149                         } else {
 150                                   fsm->req_pos = XMAX(fsm->req_pos - fsm->req_spd,fsm->req_target);
 151                         }
 152                 }
 153                 if (timer_msec - fsm->time_start > (fsm->req_spd == 0 ? 1000 : 3000)) {
 154                         fsm->flags |= CAN_VIDLE_TIMEOUT;
 155                         fsm->can_response = fsm->req_target;
 156                         fsm->current_state = wait_for_cmd;
 157                         fsm->req_pos = fsm->act_pos;
 158                 }
 159                 finished = do_control(fsm, fsm->req_spd ? 2 : 2);
 160                 if (finished && fsm->req_pos == fsm->req_target) {
 161                         fsm->can_response = fsm->req_target;
 162                         fsm->current_state = wait_for_cmd;
 163                 }
 164                 break;
 165         case EVENT_EXIT:
 166                 stop();
 167                 fsm->trigger_can_send = true;;
 168                 break;
 169         }
 170 }