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 "vhn.h"
  37
  38 #define CAN_SPEED       1000000         //< CAN bus speed
  39 #define CAN_ISR         0
  40
  41 #define ADC_ISR         1
  42
  43 #define TIMER_IRQ_PRIORITY      5
  44
  45
  46 struct fsm fsm_jaws;
  47
  48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  49 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  50 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  51
  52
  53
  54
  55 void init_motors(void){
  56
  57         init_engine_A();                        // initialization of PWM unit
  58         engine_A_en(ENGINE_EN_ON);              //enable motor A
  59         engine_A_dir(ENGINE_DIR_FW);            //set direction
  60         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  61
  62         init_engine_B();                        // initialization of PWM unit
  63         engine_B_en(ENGINE_EN_ON);              //enable motor B
  64         engine_B_dir(ENGINE_DIR_FW);            //set direction
  65         engine_B_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  66
  67         /*      vhn_init(); */
  68 }
  69
  70 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  71 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  72         /* set interrupt vector */
  73         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  74         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  75         /* enable interrupt */
  76         VICIntEnable = 1<<source;
  77 }
  78
  79 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  80 /** timer0 & ISR **/
  81
  82 void timer0_irq() __attribute__((interrupt));
  83 volatile uint32_t timer_msec = 0, timer_usec = 0;
  84
  85 void init_timer0(uint32_t prescale, uint32_t period) {
  86         T0TCR = 0x2; /* reset */
  87         T0PR = prescale - 1;
  88         T0MR0 = period;
  89         T0MCR = 0x3; /* match0: reset & irq */
  90         T0EMR = 0; /* no ext. match */
  91         T0CCR = 0x0; /* no capture */
  92         T0TCR = 0x1; /* go! */
  93 }
  94
  95 void timer0_irq() {
  96         static unsigned cnt1khz = 0;
  97
  98         /* reset timer irq */
  99         T0IR = -1;
 100
 101         /* increment timer_usec */
 102         timer_usec += 10;
 103         /* increment msec @1kHz */
 104         if (++cnt1khz == 100) {
 105                 cnt1khz = 0;
 106                 ++timer_msec;
 107         }
 108
 109         /* int acknowledge */
 110         VICVectAddr = 0;
 111 }
 112
 113
 114 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 115
 116 // #define START_PIN    15              // start pin
 117 // #define START_SEND_PRIOD_FAST        50      /* [miliseconds] */
 118 // #define START_SEND_PRIOD_SLOW        300     /* [miliseconds] */
 119 // #define START_SEND_FAST_COUNT        10      /* How many times to send start with small period (after a change) */
 120
 121
 122 // void start_button(void)
 123 // {
 124 //      can_msg_t msg;
 125 //      bool start_condition;
 126 //      static bool last_start_condition = 0;
 127 //
 128 //      static int count = 0;
 129 //      static uint32_t next_send = 0;
 130 //
 131 //
 132 //      start_condition = (IO0PIN & (1<<START_PIN)) == 0;
 133 //
 134 //      if (start_condition != last_start_condition) {
 135 //              last_start_condition = start_condition;
 136 //              count = 0;
 137 //              next_send = timer_msec; /* Send now */
 138 //      }
 139 //
 140 //      if (timer_msec >= next_send) {
 141 //              msg.id = CAN_ROBOT_CMD;
 142 //              msg.flags = 0;
 143 //              msg.dlc = 1;
 144 //              msg.data[0] = start_condition;
 145 //              /*while*/ (can_tx_msg(&msg));
 146 //
 147 //              if (count < START_SEND_FAST_COUNT) {
 148 //                      count++;
 149 //                      next_send = timer_msec + START_SEND_PRIOD_FAST;
 150 //              } else
 151 //                      next_send = timer_msec + START_SEND_PRIOD_SLOW;
 152 //      }
 153 //
 154 //
 155 // }
 156
 157
 158 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 159 void CAN_rx(can_msg_t *msg) {
 160         uint32_t spd;
 161         can_msg_t rx_msg;
 162         uint32_t req =0;
 163         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 164
 165
 166         deb_led_on(LEDB);
 167
 168         switch (rx_msg.id)
 169         {
 170                 case CAN_JAW_LEFT_CMD:
 171                         deb_led_on(LEDB);
 172                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 173                         spd = rx_msg.data[2];
 174
 175                         if (req >= 0x150 && req <= 0x3e0) {
 176                                 fsm_jaws.flags &= ~CAN_JAWS_OUT_OF_BOUNDS;
 177                                 fsm_jaws.can_req_position_left = req;// save new req position of lift
 178                                 fsm_jaws.can_req_spd_left = spd;// save new req spd of lift
 179                         } else
 180                                 fsm_jaws.flags |= CAN_JAWS_OUT_OF_BOUNDS;
 181                 break;
 182
 183                 case CAN_JAW_RIGHT_CMD:
 184                         deb_led_on(LEDB);
 185                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 186                         spd = rx_msg.data[2];
 187
 188                         if (req >= 0x150 && req <= 0x3e0) {
 189                                 fsm_jaws.flags &= ~CAN_JAWS_OUT_OF_BOUNDS;
 190                                 fsm_jaws.can_req_position_right = req;// save new req position of lift
 191                                 fsm_jaws.can_req_spd_right = spd;// save new req spd of lift
 192                         } else
 193                                 fsm_jaws.flags |= CAN_JAWS_OUT_OF_BOUNDS;
 194                 break;
 195
 196                 default:break;
 197         }
 198
 199         deb_led_off(LEDB);
 200 }
 201
 202
 203 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 204 void init_periphery(void){
 205
 206         can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 207         init_motors();
 208
 209         /* init timer0 */
 210         init_timer0(1, CPU_APB_HZ/100000);
 211         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 212
 213         init_uart();
 214         init_adc(ADC_ISR);
 215
 216
 217 }
 218 /*********************************************************/
 219 void can_send_status(void)
 220 {
 221         can_msg_t tx_msg;
 222         tx_msg.id = CAN_JAW_LEFT_STATUS;
 223         tx_msg.dlc = 5;
 224         tx_msg.flags = 0;
 225         tx_msg.data[0] = (fsm_jaws.act_pos_left  >> 8) & 0xFF;
 226         tx_msg.data[1] = fsm_jaws.act_pos_left & 0xFF;
 227         tx_msg.data[2] = (fsm_jaws.can_response  >> 8) & 0xFF;
 228         tx_msg.data[3] = fsm_jaws.can_response & 0xFF;
 229         tx_msg.data[4] = fsm_jaws.flags;
 230         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 231
 232         //can_msg_t tx_msg;
 233         tx_msg.id = CAN_JAW_RIGHT_STATUS;
 234         tx_msg.dlc = 5;
 235         tx_msg.flags = 0;
 236         tx_msg.data[0] = (fsm_jaws.act_pos_right  >> 8) & 0xFF;
 237         tx_msg.data[1] = fsm_jaws.act_pos_right & 0xFF;
 238         tx_msg.data[2] = (fsm_jaws.can_response  >> 8) & 0xFF;
 239         tx_msg.data[3] = fsm_jaws.can_response & 0xFF;
 240         tx_msg.data[4] = fsm_jaws.flags;
 241         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 242 }
 243
 244 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 245 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 246
 247 void dbg_print_time()
 248 {
 249         char str[10];
 250         unsigned t = timer_usec, i;
 251         memset(str, ' ', sizeof(str));
 252         str[9] = 0;
 253         str[8] = '\n';
 254         for (i=7; t > 0; i--) {
 255                 str[i] = t%10 + '0';
 256                 t /= 10;
 257         }
 258         send_rs_str(str);
 259 }
 260
 261 void fsm_jaws_init(struct fsm *fsm, enum event event);
 262
 263
 264 void blink_led()
 265 {
 266         static uint32_t led_time = 0;
 267
 268         if(timer_msec >= led_time + 500)
 269         {
 270                 led_time = timer_msec;
 271                 /*  static int up;
 272                     if (up == 0)
 273                     fsm_vidle.can_req_position = 0x380;
 274                     if (up == 6)
 275                     fsm_vidle.can_req_position = 0x1e0;
 276                     up = (up+1)%12;
 277                 */
 278                 deb_led_change(LEDG);
 279                 send_rs_str("LEFT_ACT_POS:");
 280                 send_rs_int(fsm_jaws.act_pos_left);
 281                 send_rs_str("\n");
 282                 send_rs_str("RIGHT_ACT_POS:");
 283                 send_rs_int(fsm_jaws.act_pos_right);
 284                 send_rs_str("\n");
 285         }
 286 }
 287
 288
 289 // #define BUMPER_PIN   17              // bumper pin  (SCK1/P0_17)
 290 // #define COLOR_PIN    3               // change color of dress pin  (SDA1/P0_3)
 291 //
 292 // #define BUMPER_LEFT 19       // left bumper MOSI1/P0_19
 293 // #define BUMPER_RIGHT 9               // right bumper RXD1/P0_9
 294
 295 #define BUMPER_REAR     17              // rear bumper in the lift  (SCK1/P0_17)
 296
 297 #define BUMPER_LEFT 19                  // left bumper MOSI1/P0_19
 298 #define BUMPER_RIGHT 9                  // right bumper RXD1/P0_9
 299
 300 #define BUMPER_LEFT_CORNER 16                   // left bumper in rear corner
 301 #define BUMPER_RIGHT_CORNER 13                  // right bumper in rear corner
 302
 303 void handle_bumper()
 304 {
 305         static uint32_t bumper_time = 0;
 306         char sw = 0;
 307
 308         if (timer_msec >= bumper_time + 100)
 309         {
 310                 can_msg_t tx_msg;
 311
 312                 bumper_time = timer_msec;
 313
 314
 315
 316                 if (IO0PIN & (1<<BUMPER_REAR))
 317                         sw &= ~CAN_SWITCH_BUMPER;
 318                 else
 319                         sw |= CAN_SWITCH_BUMPER;
 320
 321                 if (IO0PIN & (1<<BUMPER_LEFT_CORNER))
 322                         sw |= CAN_SWITCH_CORNER_LEFT;
 323                 else
 324                         sw &= ~CAN_SWITCH_CORNER_LEFT;
 325
 326                 if (IO0PIN & (1<<BUMPER_RIGHT_CORNER))
 327                         sw |= CAN_SWITCH_CORNER_RIGHT;
 328                 else
 329                         sw &= ~CAN_SWITCH_CORNER_RIGHT;
 330
 331                 if (IO0PIN & (1<<BUMPER_LEFT))
 332                         sw &= ~CAN_SWITCH_LEFT;
 333                 else
 334                         sw |= CAN_SWITCH_LEFT;
 335
 336                 if (IO0PIN & (1<<BUMPER_RIGHT))
 337                         sw &= ~CAN_SWITCH_RIGHT;
 338                 else
 339                         sw |= CAN_SWITCH_RIGHT;
 340
 341 //              if (sw & CAN_SWITCH_COLOR)
 342 //                      deb_led_off(LEDY);
 343 //              else
 344 //                      deb_led_on(LEDY);
 345
 346                 if (sw & (CAN_SWITCH_BUMPER|CAN_SWITCH_LEFT|CAN_SWITCH_RIGHT|BUMPER_RIGHT_CORNER|BUMPER_LEFT_CORNER))
 347                         deb_led_on(LEDR);
 348                 else
 349                         deb_led_off(LEDR);
 350
 351                 /* TODO: Put color to the second bit */
 352
 353                 tx_msg.id = CAN_ROBOT_SWITCHES;
 354                 tx_msg.dlc = 1;
 355                 tx_msg.flags = 0;
 356                 tx_msg.data[0] = sw;
 357
 358                 can_tx_msg(&tx_msg);
 359         }
 360 }
 361
 362 // void test_vhn()
 363 // {
 364 //      vhn_init();
 365 //
 366 //      char dir = ENGINE_DIR_BW;
 367 //      int spd = 0;
 368 //      while(1) {
 369 //              spd = (spd+1)%100;
 370 //              if (spd == 0) {
 371 //                      dir = !dir;
 372 //                      vhn_reset();
 373 //              }
 374 //              vhn_speed(spd, dir);
 375 //              send_rs_int(spd);
 376 //              uart_send_char(' ');
 377 //              send_rs_int(dir);
 378 //              send_rs_str(" Vidle started\n");
 379 // /*           char ch = uart_get_char(); */
 380 // /*           switch (ch) { */
 381 // /*           case '-': */
 382 // /*                   dir = !dir; */
 383 // /*                   break; */
 384 // /*           case '0': */
 385 // /*                   vhn_speed(0, dir); */
 386 // /*                   break; */
 387 // /*           case '1': */
 388 // /*                   vhn_speed(0x40, dir); */
 389 // /*                   break; */
 390 // /*           case '2': */
 391 // /*                   vhn_speed(0x80, dir); */
 392 // /*                   break; */
 393 // /*           case '3': */
 394 // /*                   vhn_speed(0xa0, dir); */
 395 // /*                   break; */
 396 // /*           default: */
 397 // /*                   ch='?'; */
 398 // /*           } */
 399 // /*           uart_send_char(ch); */
 400 // //           blink_led();
 401 //      }
 402 //
 403 // }
 404
 405 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 406 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 407 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 408 int main(void)
 409 {
 410         uint32_t main_time = timer_usec;
 411         uint32_t status_time = timer_usec;
 412
 413
 414         init_periphery();
 415
 416         /* TODO: Add comment FIXME: zkusit smazat, mam moct ze to melo neco spojeneho s chelae z eb09  */
 417         //SET_PIN(PINSEL1, 1, PINSEL_0);
 418         //SET_PIN(PINSEL1, 3, PINSEL_0);
 419
 420
 421
 422
 423 //      SET_PIN(PINSEL0, START_PIN, PINSEL_0);          // inicializace start pinu
 424         SET_PIN(PINSEL0, COLOR_PIN, PINSEL_0);
 425         SET_PIN(PINSEL1, 1, PINSEL_0);          // inicializace bumper pinu (FIXME SET_PIN je BLBA implemetace, musim ji nekdy opravit)
 426
 427         SET_PIN(PINSEL1, 3, PINSEL_0);
 428         SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
 429
 430
 431 //      IO0DIR &= ~((1<<START_PIN) | (1<<BUMPER_RIGHT) | (1 << COLOR_PIN));
 432         IO0DIR &= ~((1<<BUMPER_PIN) | (1<<BUMPER_LEFT));
 433
 434         //IO1DIR &= ~(3<<20);
 435
 436         send_rs_str("Jaws started\n");
 437         // The above send_rs_str is importat - we wait for the first AD conversion to be finished
 438         fsm_jaws.act_pos_left = adc_val[0];
 439         fsm_jaws.act_pos_right = adc_val[1];
 440
 441         init_fsm(&fsm_jaws, &fsm_jaws_init);
 442
 443 /*      test_vhn(); */
 444 /*      return; */
 445
 446         while(1){
 447                 if(timer_usec >= main_time + 1000)
 448                 {
 449                         main_time = timer_usec;
 450
 451                         //dbg_print_time();
 452
 453                         fsm_jaws.act_pos_left = adc_val[0];
 454                         fsm_jaws.act_pos_right = adc_val[1];
 455
 456
 457                         run_fsm(&fsm_jaws);
 458                 }
 459
 460                 if (timer_msec >= status_time + 100 || //repeat sending message every 100 ms
 461                     fsm_jaws
 462 .trigger_can_send) {   //or when something important happen
 463                         fsm_jaws
 464 .trigger_can_send = false;
 465                         status_time = timer_msec; //save new time, when message was sent
 466                         can_send_status();
 467
 468                 }
 469
 470 //              start_button();
 471                 handle_bumper();
 472
 473                 blink_led();
 474         }
 475 }
 476
 477 /** @} */