src/eb_test/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 #include <expansion.h>
  13 #define DBG_ENTRY() do {                        \
  14                 send_rs_str(__func__);          \
  15                 send_rs_str(": entry\n");       \
  16         } while(0);
  17
  18
  19
  20
  21
  22
  23 // definice stavu automatu
  24 static void wait_for_cmd(struct fsm *fsm, enum event event);
  25 static void move(struct fsm *fsm, enum event event);
  26
  27 // stavi init dela se jen po zapnuti
  28 void fsm_vidle_init(struct fsm *fsm, enum event event)
  29 {
  30         switch (event) {
  31         case EVENT_ENTRY:
  32                 DBG_ENTRY();
  33                 break;
  34         case EVENT_DO:
  35                 fsm->current_state =wait_for_cmd;
  36
  37
  38                 break;
  39         case EVENT_EXIT:
  40                 break;
  41         }
  42 }
  43
  44 // stav cekam na povel
  45
  46 static void wait_for_cmd(struct fsm *fsm, enum event event)
  47 {
  48         switch (event) {
  49         case EVENT_ENTRY:
  50                 DBG_ENTRY();
  51                 break;
  52         case EVENT_DO:
  53                 if (fsm->button1){
  54                         if (fsm->act_pos<300)
  55                         {
  56                                 engine_A_pwm(0);
  57                                 fsm->current_state = wait_for_cmd;
  58
  59
  60                         }
  61                         else
  62                         {
  63
  64                                 fsm->current_state = move;
  65
  66
  67                         }
  68                 }
  69                 else
  70                   {
  71                         if (fsm->act_pos>900)
  72                         {
  73                                 engine_A_pwm(0);
  74                                 fsm->current_state = wait_for_cmd;
  75
  76
  77                         }
  78                         else
  79                         {
  80
  81                                 fsm->current_state = move;
  82
  83
  84                         }
  85                 }
  86
  87
  88
  89
  90                 break;
  91         case EVENT_EXIT:
  92                 break;
  93         }
  94 }
  95
  96 #define treshold // treshold of reaction on delta
  97 static void move(struct fsm *fsm, enum event event)
  98 {
  99         switch (event) {
 100         case EVENT_ENTRY:
 101                 DBG_ENTRY();
 102                 break;
 103         case EVENT_DO:
 104                 if (fsm->button1)
 105                 {       engine_A_dir(ENGINE_DIR_BW);
 106                         if (fsm->act_pos>300)
 107                         {
 108                                 engine_A_pwm(20);
 109
 110
 111                         }
 112                         else
 113                         {
 114                                 fsm->current_state = wait_for_cmd;
 115                         }
 116
 117                 }
 118                 else
 119                   {     engine_A_dir(ENGINE_DIR_FW);
 120                         if (fsm->act_pos<900)
 121                         {
 122                                 engine_A_pwm(20);
 123
 124
 125
 126                         }
 127                         else
 128                         {
 129                                 fsm->current_state = wait_for_cmd;
 130                         }
 131
 132                 }
 133
 134
 135                 break;
 136         case EVENT_EXIT:
 137                 break;
 138         }
 139 }