src/eb_jaws_11/main.c

   1
   2 /**
   3  * @file main.c
   4  *
   5  *
   6  * @author Bc. Jiri Kubias, jiri.kubias@gmail.com
   7  * @author Michal Sojka <sojkam1@fel.cvut.cz>
   8  *
   9  * @addtogroup fork
  10  */
  11
  12
  13 /**
  14  * @defgroup fork Vidle (fork) application
  15  */
  16 /**
  17  * @ingroup fork
  18  * @{
  19  */
  20
  21
  22 #include <lpc21xx.h>                            /* LPC21xx definitions */
  23 #include <types.h>
  24 #include <deb_led.h>
  25 #include <system_def.h>
  26 #include <can_ids.h>
  27 #include <periph/can.h>
  28 #include <string.h>
  29 #include <deb_led.h>
  30 #include "engine.h"
  31 #include "uar.h"
  32 #include <can_msg_def.h>
  33 #include "fsm.h"
  34 #include "def.h"
  35 #include <adc.h>
  36 #include <servo.h>
  37 #include <expansion.h>
  38
  39 #define CAN_SPEED       1000000         //< CAN bus speed
  40 #define CAN_ISR         0
  41
  42 #define START_SEND_PRIOD_FAST   50      /* [miliseconds] */
  43 #define START_SEND_PRIOD_SLOW   300     /* [miliseconds] */
  44 #define START_SEND_FAST_COUNT   10      /* How many times to send start with small period (after a change) */
  45
  46 #define ADC_ISR         1
  47 #define CL_SENSOR_IN    16 //port for sending command to color sensor(via power switch)
  48
  49 #define JAW_LEFT        0
  50 #define JAW_RIGHT       1
  51
  52 #define TIMER_IRQ_PRIORITY      5
  53
  54 #define BUMPER_LEFT             GPIOPORT_8
  55 #define BUMPER_RIGHT            GPIOPORT_6
  56
  57 #define BUMPER_LEFT_ACROSS      GPIOPORT_3
  58 #define BUMPER_RIGHT_ACROSS     GPIOPORT_1
  59
  60 #define CL_SENSOR_OUT/*BUMPER_REAR_LEFT*/ GPIOPORT_7 //port for receiving data from color sensor used to be BUMPER_REAR_LEFT
  61 #define BUMPER_REAR             GPIOPORT_4
  62
  63 #define COLOR_PIN0              EXPPORT_4
  64 #define COLOR_PIN1              EXPPORT_5
  65 #define SWITCH_STRATEGY_PIN     EXPPORT_8
  66 #define START_PIN               EXPPORT_7
  67 #define SWITCH_HOME_PIN         EXPPORT_6
  68
  69
  70 struct fsm fsm_jaw_right;
  71 struct fsm fsm_jaw_left;
  72 int32_t cl_sensor_timer;
  73 int32_t cl_sensor_refresh;
  74 uint32_t option;
  75 uint32_t option_new;
  76
  77 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  78 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  79 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  80
  81 void init_motors(void){
  82
  83         init_engine_A();                        // initialization of PWM unit
  84         engine_A_en(ENGINE_EN_ON);              //enable motor A
  85         engine_A_dir(ENGINE_DIR_FW);            //set direction
  86         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  87
  88         init_engine_B();                        // initialization of PWM unit
  89         engine_B_en(ENGINE_EN_ON);              //enable motor A
  90         engine_B_dir(ENGINE_DIR_FW);            //set direction
  91         engine_B_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  92
  93 /*      vhn_init(); */
  94 }
  95
  96 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  97 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  98         /* set interrupt vector */
  99         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
 100         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
 101         /* enable interrupt */
 102         VICIntEnable = 1<<source;
 103 }
 104
 105 void start_button(void)
 106 {
 107         can_msg_t msg;
 108         bool start_condition;
 109         static bool last_start_condition = 0;
 110
 111         static int count = 0;
 112         static uint32_t next_send = 0;
 113
 114
 115         start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 116
 117         if (start_condition != last_start_condition) {
 118
 119
 120                 last_start_condition = start_condition;
 121                 count = 0;
 122                 next_send = timer_msec; /* Send now */
 123
 124         }
 125
 126         if (timer_msec >= next_send) {
 127                 msg.id = CAN_ROBOT_CMD;
 128                 msg.flags = 0;
 129                 msg.dlc = 1;
 130                 msg.data[0] = start_condition;
 131
 132 //              send_rs_str("start\n");
 133
 134                 /*while*/ (can_tx_msg(&msg));
 135
 136                 if (count < START_SEND_FAST_COUNT) {
 137                         count++;
 138                         next_send = timer_msec + START_SEND_PRIOD_FAST;
 139                 } else
 140                         next_send = timer_msec + START_SEND_PRIOD_SLOW;
 141         }
 142
 143
 144 }
 145
 146
 147 void robot_switches_handler()
 148 {
 149         static uint32_t color_time = 0;
 150         char sw = 0;
 151
 152         if (timer_msec >= color_time + 100)
 153         {
 154                 can_msg_t tx_msg;
 155
 156                 color_time = timer_msec;
 157
 158                 if (IO0PIN & (1<<COLOR_PIN0))
 159                         sw |= CAN_SWITCH_COLOR_0;
 160                 else
 161                         sw &= ~CAN_SWITCH_COLOR_0;
 162
 163                 if (IO0PIN & (1<<COLOR_PIN1))
 164                         sw |= CAN_SWITCH_COLOR_1;
 165                 else
 166                         sw &= ~CAN_SWITCH_COLOR_1;
 167
 168                 if (IO0PIN & (1<<SWITCH_STRATEGY_PIN))
 169                         sw &= ~CAN_SWITCH_STRATEGY;
 170                 else
 171                         sw |= CAN_SWITCH_STRATEGY;
 172
 173                 if (IO0PIN & (1<<SWITCH_HOME_PIN))
 174                         sw &= ~CAN_SWITCH_HOME;
 175                 else
 176                         sw |= CAN_SWITCH_HOME;
 177
 178                 // COLOR_0 = 1, COLOR_1 = 2, see team color list in robot.h
 179                 if (!(sw & CAN_SWITCH_COLOR_0) && !(sw & CAN_SWITCH_COLOR_1)){
 180                         deb_led_off(LEDY);
 181
 182                         send_rs_str("white\n");}
 183                 else
 184                         deb_led_on(LEDY);
 185
 186                 if ((sw & CAN_SWITCH_COLOR_0) && !(sw & CAN_SWITCH_COLOR_1)){
 187                         deb_led_off(LEDY);
 188
 189                         send_rs_str("green\n");}
 190                 else
 191                         deb_led_on(LEDY);
 192
 193                 if (!(sw & CAN_SWITCH_COLOR_0) && (sw & CAN_SWITCH_COLOR_1)){
 194                         deb_led_off(LEDY);
 195
 196                         send_rs_str("yellow\n");}
 197                 else
 198                         deb_led_on(LEDY);
 199
 200                 if (sw & CAN_SWITCH_STRATEGY){
 201                         deb_led_off(LEDY);
 202
 203                         send_rs_str("strategy\n");
 204                 }
 205                 else
 206                         deb_led_on(LEDY);
 207                 if (sw & CAN_SWITCH_HOME){
 208                         deb_led_off(LEDY);
 209
 210                         send_rs_str("home\n");
 211                 }
 212                 else
 213                         deb_led_on(LEDY);
 214
 215 //              send_rs_int(IO1PIN);
 216 //              send_rs_int(sw);
 217 //              send_rs_str("\n");
 218 //
 219                 /* TODO: Put color to the second bit */
 220
 221                 tx_msg.id = CAN_ROBOT_SWITCHES;
 222                 tx_msg.dlc = 1;
 223                 tx_msg.flags = 0;
 224                 tx_msg.data[0] = sw;
 225                 can_tx_msg(&tx_msg);
 226         }
 227 }
 228
 229
 230
 231 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 232 /** timer0 & ISR **/
 233
 234 void timer0_irq() __attribute__((interrupt));
 235 volatile uint32_t timer_msec = 0, timer_usec = 0;
 236
 237
 238 void init_timer0(uint32_t prescale, uint32_t period) {
 239         T0TCR = 0x2; /* reset */
 240         T0PR = prescale - 1;
 241         T0MR0 = period;
 242         T0MCR = 0x3; /* match0: reset & irq */
 243         T0EMR = 0; /* no ext. match */
 244         T0CCR = 0x0; /* no capture */
 245         T0TCR = 0x1; /* go! */
 246 }
 247
 248
 249 void timer0_irq() {
 250         static unsigned cnt1khz = 0;
 251
 252         /* reset timer irq */
 253         T0IR = -1;
 254
 255         /* increment timer_usec */
 256         timer_usec += 10;
 257         /* increment msec @1kHz */
 258         if (++cnt1khz == 100) {
 259                 cnt1khz = 0;
 260                 ++timer_msec;
 261         }
 262
 263         /* int acknowledge */
 264         VICVectAddr = 0;
 265 }
 266
 267
 268 void CAN_rx(can_msg_t *msg) {
 269         uint32_t spd;
 270         can_msg_t rx_msg;
 271         uint32_t req =0;
 272         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 273
 274
 275         deb_led_on(LEDB);
 276
 277         switch (rx_msg.id)
 278         {
 279                 //JAW_LEFT_OPEN ff      0xff
 280                 //JAW_RIGHT_OPEN        0x00
 281
 282                 //JAW_LEFT_CLOSE        0x80
 283                 //JAW_RIGHT_CLOSE       0x80
 284
 285                 //JAW_LEFT_CATCH        0xB8
 286                 //JAW_RIGHT_CATCH       0x38
 287
 288                 case CAN_JAW_LEFT_CMD:
 289
 290                         deb_led_on(LEDB);
 291                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 292                         spd = rx_msg.data[2];
 293
 294                         if (req >= 0x50 && req <= 0xD0) {
 295                                 set_servo(JAW_LEFT, (char)req);
 296                         }
 297
 298 //                      if (req >= fsm_jaw_left.min_pos && req <= fsm_jaw_left.max_pos) {
 299 //                              fsm_jaw_left.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
 300 //                              fsm_jaw_left.can_req_position = req;
 301 //                              fsm_jaw_left.can_req_spd = spd;
 302 //                      } else
 303 //                              fsm_jaw_left.flags |= CAN_JAW_OUT_OF_BOUNDS;
 304                 break;
 305
 306                 case CAN_JAW_RIGHT_CMD:
 307
 308                         deb_led_on(LEDB);
 309                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 310                         spd = rx_msg.data[2];
 311
 312                         if (req >= 0x60 && req <= 0xD0) {
 313                                 set_servo(JAW_RIGHT, (char)req);
 314                         }
 315
 316 //                      if (req >= fsm_jaw_right.min_pos && req <= fsm_jaw_right.max_pos) {
 317 //                              fsm_jaw_right.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
 318 //                              fsm_jaw_right.can_req_position = req;
 319 //                              fsm_jaw_right.can_req_spd = spd;
 320 //                      } else
 321 //                              fsm_jaw_right.flags |= CAN_JAW_OUT_OF_BOUNDS;
 322                 break;
 323                 case CAN_CL_SENSOR_CMD:
 324                         deb_led_on(LEDB);
 325                         uint32_t option_new = rx_msg.data[0];//option is from range 1-5
 326                         if(option == 0)
 327                         {
 328                                 option = option_new;
 329                                 option_new = 0;
 330                         }
 331                 break;
 332                 default:break;
 333         }
 334
 335         deb_led_off(LEDB);
 336 }
 337
 338
 339 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 340 void init_periphery(void){
 341
 342         can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 343         //init_motors();
 344
 345         /* init timer0 */
 346         init_timer0(1, CPU_APB_HZ/100000);
 347         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 348
 349         init_uart();
 350         //init_adc(ADC_ISR);
 351
 352         init_servo(7);  //7 is interrupt priority
 353         set_servo(JAW_LEFT, 0xB0);
 354         set_servo(JAW_RIGHT,0x70);
 355 }
 356
 357 /*********************************************************/
 358 void can_send_status(struct fsm *fsm){
 359         can_msg_t tx_msg;
 360         tx_msg.id = fsm->can_id;
 361         tx_msg.dlc = 5;
 362         tx_msg.flags = 0;
 363         tx_msg.data[0] = (fsm->act_pos  >> 8) & 0xFF;
 364         tx_msg.data[1] = fsm->act_pos & 0xFF;
 365         tx_msg.data[2] = (fsm->can_response  >> 8) & 0xFF;
 366         tx_msg.data[3] = fsm->can_response & 0xFF;
 367         tx_msg.data[4] = fsm->flags;
 368         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 369
 370 }
 371
 372 void cl_sensor_send_status(){
 373         can_msg_t tx_msg;
 374         tx_msg.id = CAN_CL_SENSOR_STATUS;
 375         tx_msg.dlc = 5;
 376         tx_msg.flags = 0;
 377         //tx_msg.data[0] = ((IO1PIN & 1<<CL_SENSOR_OUT) != 0) & 0xFF;// 1 - pattern match 0 - no match
 378         if((IO1PIN & 1<<CL_SENSOR_OUT) != 0) {
 379                 tx_msg.data[0] = CAN_CL_SENSOR_PATTERN_MATCH;
 380                 send_rs_str("trefa\n");
 381         }
 382         (can_tx_msg(&tx_msg));
 383 }
 384
 385 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 386 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 387
 388 void dbg_print_time()
 389 {
 390         char str[10];
 391         unsigned t = timer_usec, i;
 392         memset(str, ' ', sizeof(str));
 393         str[9] = 0;
 394         str[8] = '\n';
 395         for (i=7; t > 0; i--) {
 396                 str[i] = t%10 + '0';
 397                 t /= 10;
 398         }
 399         send_rs_str(str);
 400 }
 401
 402 void fsm_jaw_init(struct fsm *fsm, enum event event);
 403
 404 void blink_led()
 405 {
 406         static uint32_t led_time = 0;
 407
 408         if(timer_msec >= led_time + 500)
 409         {
 410                 led_time = timer_msec;
 411                 /*  static int up;
 412                     if (up == 0)
 413                     fsm_vidle.can_req_position = 0x380;
 414                     if (up == 6)
 415                     fsm_vidle.can_req_position = 0x1e0;
 416                     up = (up+1)%12;
 417                 */
 418                 deb_led_change(LEDG);
 419
 420                 send_rs_str("LEFT_JAW: ");
 421                 send_rs_int(fsm_jaw_left.act_pos);
 422                 send_rs_str("\tLEFT_FLAGS: ");
 423                 send_rs_int(fsm_jaw_left.flags);
 424
 425                 send_rs_str("\tRIGHT_JAW: ");
 426                 send_rs_int(fsm_jaw_right.act_pos);
 427                 send_rs_str("\tRIGHT_FLAGS: ");
 428                 send_rs_int(fsm_jaw_right.flags);
 429
 430                 send_rs_str("\n");
 431
 432         }
 433 }
 434
 435 void handle_bumper()
 436 {
 437         static uint32_t bumper_time = 0;
 438         char sw = 0;
 439
 440         if (timer_msec >= bumper_time + 100)
 441         {
 442                 can_msg_t tx_msg;
 443
 444                 bumper_time = timer_msec;
 445
 446
 447
 448                 /*if (IO1PIN & (1<<BUMPER_REAR_LEFT)){
 449                         sw &= ~CAN_BUMPER_REAR_LEFT;
 450
 451                         send_rs_str("rear_left\n");
 452
 453 //                      deb_led_on(LEDY);
 454                 }
 455                 else{
 456                         sw |= CAN_BUMPER_REAR_LEFT;
 457
 458 //                      deb_led_off(LEDY);
 459                 }*/
 460
 461                 if (IO1PIN & (1<<BUMPER_REAR)){
 462                         sw &= ~CAN_BUMPER_REAR_RIGHT;
 463                         sw &= ~CAN_BUMPER_REAR_LEFT;
 464                         send_rs_str("reart\n");
 465
 466 //                      deb_led_on(LEDY);
 467                 }
 468                 else{
 469                         sw |= CAN_BUMPER_REAR_RIGHT;
 470                         sw |= CAN_BUMPER_REAR_LEFT;
 471
 472 //                      deb_led_off(LEDY);
 473                 }
 474
 475                 if (IO1PIN & (1<<BUMPER_LEFT)){
 476
 477                         sw &= ~CAN_BUMPER_LEFT;
 478
 479                         send_rs_str("left\n");
 480
 481 //                      deb_led_on(LEDB);
 482                 }
 483                 else{
 484
 485                     sw |= CAN_BUMPER_LEFT;
 486
 487 //                  deb_led_off(LEDB);
 488                 }
 489                 if (IO1PIN & (1<<BUMPER_RIGHT)){
 490                         sw &= ~CAN_BUMPER_RIGHT;
 491                         send_rs_str("right\n");
 492 //                      deb_led_on(LEDG);
 493                 }
 494                 else{
 495                         sw |= CAN_BUMPER_RIGHT;
 496
 497 //                      deb_led_off(LEDG);
 498                 }
 499                 if (IO1PIN & (1<<BUMPER_LEFT_ACROSS)){
 500                         sw &= ~CAN_BUMPER_LEFT_ACROSS;
 501                         send_rs_str("left_across\n");
 502 //                      deb_led_on(LEDR);
 503                 }
 504                 else{
 505                         sw |= CAN_BUMPER_LEFT_ACROSS;
 506
 507 //                      deb_led_off(LEDR);
 508                 }
 509                 if (IO1PIN & (1<<BUMPER_RIGHT_ACROSS)){
 510                         sw &= ~CAN_BUMPER_RIGHT_ACROSS;
 511                         send_rs_str("right_across\n");
 512 //                      deb_le0d_on(LEDY);
 513                 }
 514                 else{
 515                         sw |= CAN_BUMPER_RIGHT_ACROSS;
 516
 517 //                      deb_led_off(LEDY);
 518                 }
 519
 520                 if (sw & (CAN_BUMPER_REAR_LEFT | CAN_BUMPER_REAR_RIGHT | CAN_BUMPER_LEFT | CAN_BUMPER_RIGHT | CAN_BUMPER_LEFT_ACROSS | CAN_BUMPER_RIGHT_ACROSS))
 521                         deb_led_on(LEDR);
 522                 else
 523                         deb_led_off(LEDR);
 524
 525 //              send_rs_int(IO1PIN);
 526 //              send_rs_int(sw);
 527 //              send_rs_str("\n");
 528                 tx_msg.id = CAN_ROBOT_BUMPERS;
 529                 tx_msg.dlc = 1;
 530                 tx_msg.flags = 0;
 531                 tx_msg.data[0] = sw;
 532                 can_tx_msg(&tx_msg);
 533         }
 534 }
 535
 536 void cl_sensor_init(){
 537         IO0DIR |= 1<<CL_SENSOR_IN;
 538         IO1DIR &= ~(1<<CL_SENSOR_OUT);
 539         IO0SET = (1<<CL_SENSOR_IN);
 540         option = 3;
 541         option_new = 0;
 542         cl_sensor_timer = 300 + timer_msec;
 543         cl_sensor_refresh = 1700 + timer_msec;
 544 }
 545
 546 void dummy_wait()
 547 {
 548         volatile unsigned int wait = 5900000;
 549         while(--wait);
 550 }
 551 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 552 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 553 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 554 int main(void)
 555 {
 556         uint32_t main_time = timer_usec;
 557         uint32_t status_time_left = timer_usec;
 558         uint32_t status_time_right = timer_usec;
 559         uint32_t status_time_sensor = timer_msec;
 560         //bool feedback;
 561
 562         init_periphery();
 563
 564         SET_PIN(PINSEL0, COLOR_PIN0, PINSEL_0);
 565         SET_PIN(PINSEL0, START_PIN, PINSEL_0);
 566         SET_PIN(PINSEL1, (SWITCH_STRATEGY_PIN-16), PINSEL_0);
 567         SET_PIN(PINSEL1, (COLOR_PIN1-16), PINSEL_0);
 568         SET_PIN(PINSEL1, (SWITCH_HOME_PIN - 16), PINSEL_0);
 569
 570         PINSEL2 &= ~(1 << 3) ;          //setting P1.25:16 as GPIO pins -> GPIO PORT on eb_board
 571         IO1DIR &= ~((1<<BUMPER_REAR) | /*(1<<BUMPER_REAR_LEFT) |*/ (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
 572         IO0DIR &= ~((1<<COLOR_PIN0) | (1<<COLOR_PIN1)| (1<<SWITCH_HOME_PIN) | (1<<START_PIN) | (1<<SWITCH_STRATEGY_PIN));
 573         cl_sensor_init();
 574
 575
 576
 577         send_rs_str("Jaws started\n");
 578         //uint32_t i = 5000000;
 579 //      uint32_t j;
 580 //      while(i--) for(j = 0; j < 10; j++);
 581         //close jaw, max_pos, open jaw min_pos
 582         //letf jaw potenciometer on ADC 0, header J32 on board
 583         //fsm_jaw_left.max_pos=0x320; //max 0x324
 584         //fsm_jaw_left.min_pos=0xB0; //min 0xC3
 585         //fsm_jaw_left.act_pos = adc_val[0];
 586
 587         //close jaw, max_pos, open jaw min_pos
 588         //letf jaw potenciometer on ADC 1, header J33 on board
 589         //fsm_jaw_right.max_pos=0x380; //max 0x382
 590         //fsm_jaw_right.min_pos=0x010; //min 0xF5
 591         //fsm_jaw_right.act_pos = adc_val[1];
 592
 593         //left jaw engine is engine A, conector MOTA on board
 594         //fsm_jaw_left.engine_en = &engine_A_en;
 595         //fsm_jaw_left.engine_dir = &engine_A_dir;
 596         //fsm_jaw_left.engine_pwm = &engine_A_pwm;
 597
 598         fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
 599
 600         //right jaw engine is engine B, conector MOTB on board
 601         //fsm_jaw_right.engine_en = &engine_B_en;
 602         //fsm_jaw_right.engine_dir = &engine_B_dir;
 603         //fsm_jaw_right.engine_pwm = &engine_B_pwm;
 604
 605         fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
 606
 607
 608 //      init_fsm(&fsm_jaw_right, &fsm_jaw_init);
 609 //      init_fsm(&fsm_jaw_left, &fsm_jaw_init);
 610
 611 /*      test_vhn(); */
 612 /*      return; */
 613         while(1){
 614                 //run fsms
 615                 if(timer_usec >= main_time + 1000){
 616                         main_time = timer_usec;
 617                         //dbg_print_time();
 618
 619                         //fsm_jaw_left.act_pos = adc_val[0];
 620 //                      run_fsm(&fsm_jaw_left);
 621
 622                         //fsm_jaw_right.act_pos = adc_val[1];
 623 //                      run_fsm(&fsm_jaw_right);
 624
 625                 }
 626                 //R/W color sensor
 627                 if(timer_msec >= status_time_sensor + 10){
 628                         status_time_sensor = timer_msec;
 629                         if(cl_sensor_timer < timer_msec){
 630                                 IO0SET = (1<<CL_SENSOR_IN);
 631
 632                         if(cl_sensor_refresh < timer_msec){
 633                               if(option != 0) {
 634                                 IO0CLR = (1<<CL_SENSOR_IN);
 635                                 cl_sensor_timer = 300*option + timer_msec;
 636                                 cl_sensor_refresh = 2000 - 300*option + timer_msec;
 637                                 option = option_new;
 638                                 option_new = 0;
 639                               }
 640                         }
 641                         }
 642                         cl_sensor_send_status();
 643                 //      feedback = (IO0PIN & 1<<CL_SENSOR_OUT) != 0;
 644                 //      if(feedback) send_rs_str("shoda barev\n");
 645                 //              else send_rs_str("jina barva\n");
 646                 }
 647                 //CAN send left jaw status
 648                 if (timer_msec >= status_time_left + 100 /*|| //repeat sending message every 100 ms
 649                     fsm_jaw_left.trigger_can_send*/) {   //or when something important happen
 650                         //fsm_jaw_left.trigger_can_send = false;
 651                         status_time_left = timer_msec; //save new time, when message was sent
 652                         can_send_status(&fsm_jaw_left);
 653
 654                 }
 655 //              CAN send left jaw status
 656                 if (timer_msec >= status_time_right + 100 /*|| //repeat sending message every 100 ms
 657                     fsm_jaw_right.trigger_can_send*/) {   //or when something important happen
 658                         //fsm_jaw_right.trigger_can_send = false;
 659                         status_time_right = timer_msec; //save new time, when message was sent
 660                         can_send_status(&fsm_jaw_right);
 661
 662
 663
 664                 }
 665                 start_button();
 666                 handle_bumper();
 667                 robot_switches_handler();
 668                 blink_led();
 669         }
 670 }
 671
 672 /** @} */