src/eb_lift_09/lift.c

   1 #include <lpc21xx.h>                            /* LPC21xx definitions */
   2 #include <deb_led.h>
   3 #include <system_def.h>
   4 #include <can_ids.h>
   5 #include <periph/can.h>
   6 #include <string.h>
   7 #include <deb_led.h>
   8 #include "servo.h"
   9 #include "engine.h"
  10 #include "lift.h"
  11 #include "uar.h"
  12 #include "def.h"
  13 #include "irc.h"
  14 #include "vhn.h"
  15 #include <can_msg_def.h>
  16
  17 #define ENG_STOP        0
  18 #define ENG_LIFT_HOME_SLOW_SPEED        15
  19 #define ENG_LIFT_HOME_SPEED     40
  20 #define ENG_LIFT_FULL_SPEED     100
  21 #define ENG_PUSHER_HOME_SPEED   70
  22 #define ENG_PUSHER_FULL_SPEED   100
  23 #define ENG_PUSHER_HOME_SLOW_SPEED      40
  24 #define TIME_WAIT               300
  25 #define TIME_HOMING_WAIT        7000
  26
  27
  28
  29 #define  PRINT_STR_VAL(fsm,str, val)    print_str_val(fsm, str, val)
  30 #define  PRINT_STR(fsm, str)    print_str(fsm, str)
  31
  32
  33 static uint8_t fsm_homing_block;
  34
  35
  36 void print_str(uint8_t type, uint8_t  text[])
  37 {
  38         if(type == FSM_LIFT)
  39           send_rs_str("FSM_LIFT    ");
  40         else
  41           send_rs_str("FSM_PUSHER  ");
  42
  43         send_rs_str(text);
  44         uart_send_char('\n');
  45 }
  46
  47
  48 void print_str_val(uint8_t type, uint8_t  text[], uint32_t val)
  49 {
  50         if(type == FSM_LIFT)
  51           send_rs_str("FSM_LIFT    ");
  52         else
  53           send_rs_str("FSM_PUSHER  ");
  54
  55         send_rs_str(text);
  56         uart_send_char(' ');
  57         send_rs_int(val);
  58         uart_send_char('\n');
  59 }
  60
  61 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  62 void move_pusher(unsigned char direct,unsigned char velocity){//how many pulzes
  63         engine_A_dir(direct);//set direction for lift
  64         engine_A_pwm(velocity); // set velocity, range 0~100~200
  65         engine_A_en(ENGINE_EN_ON);//enable motor of pusher
  66 }
  67
  68 void move_lift(uint8_t direct,uint8_t velocity){        //how many pulzes
  69 //      engine_B_dir(direct);                           //set direction for lift
  70 //      engine_B_pwm(velocity);                         // set velocity, range 0~100~200
  71 //      engine_B_en(ENGINE_EN_ON);                      //enable motor of pusher
  72
  73 //      vhn_speed(50, 0);
  74         vhn_speed(velocity , direct);
  75 //      PRINT_STR_VAL(FSM_LIFT,"VHN speed:", velocity);
  76 //      PRINT_STR_VAL(FSM_LIFT,"VHN direct:", direct);
  77 }
  78
  79
  80 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  81 void fsm_irc(struct fsm *fsm)
  82 {
  83         int32_t irc = *fsm->irc ;
  84
  85         if(fsm->motor_dir == ENGINE_DIR_FW)
  86                 fsm->act_pos += irc - fsm->last_irc;
  87         else
  88                 fsm->act_pos -= irc - fsm->last_irc;
  89
  90         if (irc != fsm->last_irc)
  91                 fsm->last_move_time = timer_msec;
  92
  93
  94         fsm->last_irc = irc;
  95
  96 }
  97
  98 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  99 void fsm_init(struct fsm *fsm, events my_event)
 100 {
 101
 102
 103   switch(my_event){
 104                 case EVENT_ENTRY:
 105                   PRINT_STR(fsm->type,"fsm_init: EVENT_ENTRY");
 106
 107                   fsm->act_pos = 0;
 108                   fsm->req_pos = 0;
 109                   fsm->can_req_position = 0;
 110                   fsm->timeout_glob = 0;        // ms
 111                   fsm->timeout_switch = 0;      // ms
 112                   fsm->motor_dir = ENGINE_DIR_FW;
 113                   fsm->lenght = 0;
 114                   fsm->ans_can = 0;
 115                   fsm->p = 1000;
 116
 117                   if(fsm->type == FSM_LIFT)
 118                   {
 119                         fsm->irc = &irc2_count;
 120                         fsm->set_engine = &move_lift;
 121                         fsm->sw_pin = LIFT_END_SWITCH; //LIFT_END_SWITCH;
 122                         fsm->speed_home = ENG_LIFT_HOME_SPEED;
 123                         fsm->speed_normal= ENG_LIFT_FULL_SPEED;
 124                         fsm->init_flag = LIFT_LIFT_INIT;
 125                         fsm->speed_slow = ENG_LIFT_HOME_SLOW_SPEED;
 126                         fsm_homing_block = FSM_LIFT;
 127
 128                   }
 129                   if(fsm->type == FSM_PUSHER)
 130                   {
 131                         fsm->irc = &irc1_count;
 132                         fsm->set_engine = &move_pusher;
 133                         fsm->sw_pin = PUSH_END_SWITCH;
 134                         fsm->speed_home = ENG_PUSHER_HOME_SPEED;
 135                         fsm->speed_normal= ENG_PUSHER_FULL_SPEED;
 136                         fsm->init_flag = LIFT_PUSHER_INIT;
 137                         fsm->speed_slow = ENG_PUSHER_HOME_SLOW_SPEED;
 138                   }
 139
 140                   fsm->last_irc =  *fsm->irc;
 141                   can_flags |= fsm->init_flag;
 142                 break;
 143
 144                 case EVENT_DO:
 145
 146                     if(fsm_homing_block == fsm->type)                   // block lift until release fsm_homing_block
 147                         fsm->current_state = &fsm_homing_bw;
 148                     break;
 149
 150                 case EVENT_EXIT:
 151
 152                 break;
 153                 default:break;
 154         }
 155 }
 156
 157
 158
 159
 160
 161 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 162 void fsm_homing_fw(struct fsm *fsm, events my_event){
 163
 164
 165     switch(my_event){
 166                 case EVENT_ENTRY:
 167                         PRINT_STR(fsm->type,"fsm_homing_fw: EVENT_ENTRY");
 168                         fsm->motor_dir = ENGINE_DIR_FW;
 169                         fsm->set_engine(fsm->motor_dir, fsm->speed_home);
 170                         fsm->timeout_glob = timer_msec;
 171
 172                 break;
 173
 174                 case EVENT_DO:
 175
 176                         if(fsm->switch_front) // || (fsm->timeout_glob + TIME_HOMING_WAIT < timer_msec))
 177                         {
 178                           fsm->set_engine(fsm->motor_dir, ENG_STOP);
 179                           fsm->act_pos = 0;                             // reset position
 180                           fsm->current_state = &fsm_homing_bw;  // move to fsm_homing_bw
 181                           PRINT_STR(fsm->type,"fsm_homing_fw: EVENT_DO reached endswitch");
 182                           PRINT_STR_VAL(fsm->type, "Measured IRC: ", *fsm->irc);
 183                         }
 184
 185                 break;
 186
 187                 case EVENT_EXIT:
 188                         break;
 189                         default:break;
 190         }
 191 }
 192
 193 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 194 void fsm_homing_bw(struct fsm *fsm, events my_event){
 195
 196     switch(my_event){
 197                 case EVENT_ENTRY:
 198                         PRINT_STR(fsm->type,"fsm_homing_bw: EVENT_ENTRY");
 199                         fsm->motor_dir = ENGINE_DIR_BW;
 200                         fsm->set_engine(ENGINE_DIR_BW, fsm->speed_home);
 201                         fsm->timeout_switch = timer_msec;
 202                         fsm->timeout_glob = timer_msec;         // FIXME nevim jestli je potreba
 203                 break;
 204
 205                 case EVENT_DO:
 206
 207                         if(fsm->switch_back)
 208                         {
 209                           fsm->set_engine(fsm->motor_dir, ENG_STOP);
 210                           fsm->lenght = -1 * fsm->act_pos;              // store measured lenght
 211                           fsm->act_pos = 0;                             // reset position
 212                           fsm->current_state = &fsm_homing_fw_up;               // move to fsm_stop
 213                           fsm->ans_can = 0;
 214                           PRINT_STR(fsm->type,"fsm_homing_bw: EVENT_DO reached endswitch");
 215
 216                         }
 217                 break;
 218
 219                 case EVENT_EXIT:
 220                         break;
 221                         default:break;
 222         }
 223 }
 224
 225
 226 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 227 void fsm_homing_fw_up(struct fsm *fsm, events my_event){
 228
 229
 230     switch(my_event){
 231                 case EVENT_ENTRY:
 232                         can_flags |= fsm->init_flag;
 233                         PRINT_STR(fsm->type,"fsm_homing_fw_up: EVENT_ENTRY");
 234                         fsm->motor_dir = ENGINE_DIR_FW;
 235                         fsm->set_engine(fsm->motor_dir, fsm->speed_home);
 236                         fsm->timeout_glob = timer_msec;
 237
 238                 break;
 239
 240                 case EVENT_DO:
 241
 242                         if(fsm->switch_front || (fsm->timeout_glob + 1000 < timer_msec))
 243                         {
 244                           fsm->set_engine(fsm->motor_dir, ENG_STOP);
 245                           fsm->current_state = &fsm_homing_bw_zero;     // move to fsm_homing_bw
 246
 247                           PRINT_STR(fsm->type,"fsm_homing_fw_up: EVENT_DO reached endswitch");
 248                         }
 249
 250                 break;
 251
 252                 case EVENT_EXIT:
 253                         break;
 254                         default:break;
 255         }
 256 }
 257
 258
 259 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 260 void fsm_homing_bw_zero(struct fsm *fsm, events my_event){
 261
 262     switch(my_event){
 263                 case EVENT_ENTRY:
 264                         PRINT_STR(fsm->type,"fsm_homing_bw_zero: EVENT_ENTRY");
 265                         fsm->motor_dir = ENGINE_DIR_BW;
 266                         fsm->set_engine(ENGINE_DIR_BW, fsm->speed_slow);
 267                         fsm->timeout_switch = timer_msec;
 268                         fsm->timeout_glob = timer_msec;         // FIXME nevim jestli je potreba
 269                 break;
 270
 271                 case EVENT_DO:
 272
 273                         if(fsm->switch_back)
 274                         {
 275                           fsm->set_engine(fsm->motor_dir, ENG_STOP);
 276                           fsm->act_pos = 0;
 277
 278                           if(fsm->type == FSM_LIFT)
 279                           {
 280                             fsm->current_state = &fsm_homing_fw_5;
 281                           }
 282                           else
 283                           {                                 /* PUSHER */
 284                             fsm->current_state = &fsm_stop;             // move to fsm_stop
 285                             PRINT_STR(fsm->type,"fsm_homing_bw_zero: EVENT_DO reached endswitch");
 286                           }
 287                         }
 288                 break;
 289
 290                 case EVENT_EXIT:
 291
 292                         break;
 293                         default:break;
 294         }
 295 }
 296
 297 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 298 void fsm_wait_for_pusher_init(struct fsm *fsm, events my_event);
 299 void fsm_homing_fw_5(struct fsm *fsm, events my_event){
 300
 301
 302     switch(my_event){
 303                 case EVENT_ENTRY:
 304
 305                         PRINT_STR(fsm->type,"fsm_homing_fw_5: EVENT_ENTRY");
 306                         fsm->motor_dir = ENGINE_DIR_FW;
 307                         fsm->set_engine(fsm->motor_dir, fsm->speed_slow);
 308                         fsm->timeout_glob = timer_msec;
 309
 310                 break;
 311
 312                 case EVENT_DO:
 313
 314                         if(fsm->switch_front || (fsm->act_pos >= (5*8)))
 315                         {
 316                           fsm->set_engine(fsm->motor_dir, ENG_STOP);
 317                           fsm->current_state = fsm_wait_for_pusher_init;
 318                           PRINT_STR_VAL(fsm->type,"fsm_homing_fw_5: EVENT_DO reached endswitch", *fsm->irc);
 319                           fsm_homing_block = FSM_PUSHER;                // release pusher
 320                         }
 321
 322                 break;
 323
 324                 case EVENT_EXIT:
 325                         break;
 326                         default:break;
 327         }
 328 }
 329
 330 void fsm_wait_for_pusher_init(struct fsm *fsm, events my_event){
 331         switch (my_event) {
 332                 case EVENT_ENTRY:
 333                         can_flags &= ~fsm->init_flag;
 334                         break;
 335                 case EVENT_DO:
 336                         if ((can_flags & LIFT_PUSHER_INIT) == 0) {
 337                                   fsm->current_state = &fsm_stop;       // move to fsm_homing_bw
 338                         }
 339                         break;
 340                 default:;
 341         }
 342 }
 343
 344
 345 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 346 void fsm_stop(struct fsm *fsm, events my_event){
 347         switch(my_event){
 348                 case EVENT_ENTRY:
 349                         can_flags &= ~fsm->init_flag;
 350                         can_send_status();
 351                         fsm->set_engine(fsm->motor_dir , ENG_STOP);
 352                         PRINT_STR(fsm->type,"fsm_stop: EVENT_ENTRY");
 353                         break;
 354
 355                 case EVENT_DO:
 356
 357 /*                      if(fsm->type == FSM_LIFT) { */
 358 /*                              if ((req >= LIFT_LIFT_PROTECT_PUSHER_TOP) && */
 359 /*                                 (fsm->act_pos >= LIFT_PUSHER_PROTECT_TOP )) */
 360 /*                                      can_flags |= LIFT_LIFT_ERR_POS; */
 361 /*                              else */
 362 /*                                      can_flags &= ~(LIFT_LIFT_ERR_POS); */
 363 /*                      } */
 364 /*                      if (fsm->type == FSM_PUSHER) { */
 365 /*                              if (fsm_pusher.act_pos < 0) { */
 366 /*                                      can_flags |= LIFT_PUSHER_ERR_POS; */
 367
 368 /*                              } */
 369 /*                              else  */
 370 /*                                      can_flags &= ~(LIFT_PUSHER_ERR_POS); */
 371 /*                      } */
 372
 373                         if(fsm->req_pos != fsm->can_req_position)
 374                         {
 375                                 fsm->req_pos = fsm->can_req_position;
 376                                 fsm->current_state = &fsm_move;         // move to fsm_move
 377                                 PRINT_STR_VAL(fsm->type, "fsm_stop: new position recieved", fsm->req_pos);
 378                                 PRINT_STR_VAL(fsm->type, "fsm_stop: act pos: ", fsm->act_pos);
 379                         }
 380
 381                         break;
 382                 case EVENT_EXIT:
 383                         break;
 384         }
 385 }
 386
 387
 388
 389 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 390 void fsm_move(struct fsm *fsm, events my_event){
 391         unsigned u;
 392         int e;
 393
 394         switch(my_event){
 395                 PRINT_STR(fsm->type,"fsm_move: EVENT_ENTRY");
 396                 case EVENT_ENTRY:
 397                   if (fsm->act_pos > fsm->req_pos) {
 398                         fsm->motor_dir = ENGINE_DIR_BW;         //set the direction of pusher moving
 399                   }
 400                   else {
 401                         fsm->motor_dir = ENGINE_DIR_FW;
 402                   }
 403
 404                   fsm->last_move_time = timer_msec;
 405                   fsm->enable_reg = 1;          // enable re
 406                 break;
 407
 408                 case EVENT_DO:
 409
 410                   /* Check end-switches */
 411
 412                   if((fsm->act_pos < fsm->req_pos) && (fsm->act_pos >= 0xF0))
 413                     set_servo(SERVO_DOOR, SERVO_DOOR_MAX);
 414
 415                   if ((fsm->motor_dir == ENGINE_DIR_BW && fsm->switch_back) ||
 416                       (fsm->motor_dir == ENGINE_DIR_FW && fsm->switch_front)) {
 417                         fsm->set_engine(fsm->motor_dir, ENG_STOP);
 418                         fsm->current_state = &fsm_wait; // move to fsm_homing_bw
 419                         fsm->ans_can = fsm->req_pos;
 420                         can_send_status();
 421                         PRINT_STR_VAL(fsm->type, "fsm_move: reached endswitch", fsm->req_pos);
 422                         // TODO FIXME TODO FIXME TODO FIXME nastavit act_pos =0?????
 423                         break;
 424                   }
 425
 426                   /* Normal movement */
 427                   if (fsm->motor_dir == ENGINE_DIR_BW)
 428                           e = fsm->act_pos - fsm->req_pos;
 429                   else
 430                           e = fsm->req_pos - fsm->act_pos;
 431
 432
 433                   if(fsm->req_pos == 0) // musi dojet na koncak
 434                   {
 435                     if(e <= 0 || timer_msec - fsm->last_move_time > 200)
 436                     {
 437                       fsm->set_engine(fsm->motor_dir, 35 /*fsm->speed_normal*/);
 438                       fsm->enable_reg = 0;
 439                       return;
 440                     }
 441                   }
 442
 443
 444                   if (e > 0 && fsm->enable_reg ) {
 445                           /* Go there (P controller)*/
 446                           u = e*fsm->p/1000;
 447                           if (u>100) u = 100;
 448                           fsm->set_engine(fsm->motor_dir, u /*fsm->speed_normal*/);
 449                           fsm->ans_can = fsm->act_pos;
 450                   }
 451
 452
 453
 454                   if (e <= 0 || timer_msec - fsm->last_move_time > 200) {
 455                           /* We are there */
 456                           fsm->set_engine(fsm->motor_dir, ENG_STOP);
 457                           if (e <= 0)
 458                                   fsm->current_state = &fsm_wait;
 459                           else
 460                                   fsm->current_state = &fsm_stop;
 461                           fsm->ans_can = fsm->req_pos;
 462                           can_send_status();
 463                           if (e <= 0)
 464                                   PRINT_STR(fsm->type,"fsm_move: reached target");
 465                           else
 466                                   PRINT_STR_VAL(fsm->type,"fsm_move: move timeout at pos", fsm->act_pos);
 467                   }
 468                   break;
 469                 case EVENT_EXIT:
 470                         fsm->set_engine(fsm->motor_dir, ENG_STOP);
 471                 break;
 472                 default:break;
 473         }
 474 }
 475
 476 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 477 void fsm_wait(struct fsm *fsm, events my_event){
 478         switch(my_event){
 479                 case EVENT_ENTRY:
 480                         fsm->timeout_glob = timer_msec;
 481                         PRINT_STR(fsm->type,"fsm_wait: EVENT_ENTRY");
 482                         break;
 483
 484                 case EVENT_DO:
 485
 486                         if((fsm->timeout_glob + TIME_WAIT) >+ timer_msec)
 487                         {
 488                                 fsm->current_state = &fsm_stop; // move to fsm_homing_bw
 489                         }
 490
 491                         break;
 492                 case EVENT_EXIT:
 493                         PRINT_STR(fsm->type,"fsm_wait: EXIT");
 494                         break;
 495                 default:break;
 496         }
 497
 498
 499 }