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 ADC_ISR         1
  43
  44 #define JAW_LEFT        0
  45 #define JAW_RIGHT       1
  46
  47 #define TIMER_IRQ_PRIORITY      5
  48
  49 #define BUMPER_LEFT     EXPPORT_5
  50 #define BUMPER_RIGHT    EXPPORT_4
  51
  52 #define BUMPER_LEFT_ACROSS      EXPPORT_6
  53 #define BUMPER_RIGHT_ACROSS     EXPPORT_8
  54
  55 #define BUMPER_REAR_LEFT        EXPPORT_7
  56 #define BUMPER_REAR_RIGHT       EXPPORT_1
  57
  58 struct fsm fsm_jaw_right;
  59 struct fsm fsm_jaw_left;
  60
  61 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  62 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  63 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  64
  65 void init_motors(void){
  66
  67         init_engine_A();                        // initialization of PWM unit
  68         engine_A_en(ENGINE_EN_ON);              //enable motor A
  69         engine_A_dir(ENGINE_DIR_FW);            //set direction
  70         engine_A_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  71
  72         init_engine_B();                        // initialization of PWM unit
  73         engine_B_en(ENGINE_EN_ON);              //enable motor A
  74         engine_B_dir(ENGINE_DIR_FW);            //set direction
  75         engine_B_pwm(0);                        // STOP pwm is in percent, range 0~100~200
  76
  77 /*      vhn_init(); */
  78 }
  79
  80 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  81 void set_irq_handler(uint8_t source, uint8_t irq_vect, void (*handler)()) {
  82         /* set interrupt vector */
  83         ((uint32_t*)&VICVectAddr0)[irq_vect] = (uint32_t)handler;
  84         ((uint32_t*)&VICVectCntl0)[irq_vect] = 0x20 | source;
  85         /* enable interrupt */
  86         VICIntEnable = 1<<source;
  87 }
  88
  89 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  90 /** timer0 & ISR **/
  91
  92 void timer0_irq() __attribute__((interrupt));
  93 volatile uint32_t timer_msec = 0, timer_usec = 0;
  94
  95
  96 void init_timer0(uint32_t prescale, uint32_t period) {
  97         T0TCR = 0x2; /* reset */
  98         T0PR = prescale - 1;
  99         T0MR0 = period;
 100         T0MCR = 0x3; /* match0: reset & irq */
 101         T0EMR = 0; /* no ext. match */
 102         T0CCR = 0x0; /* no capture */
 103         T0TCR = 0x1; /* go! */
 104 }
 105
 106
 107 void timer0_irq() {
 108         static unsigned cnt1khz = 0;
 109
 110         /* reset timer irq */
 111         T0IR = -1;
 112
 113         /* increment timer_usec */
 114         timer_usec += 10;
 115         /* increment msec @1kHz */
 116         if (++cnt1khz == 100) {
 117                 cnt1khz = 0;
 118                 ++timer_msec;
 119         }
 120
 121         /* int acknowledge */
 122         VICVectAddr = 0;
 123 }
 124
 125
 126 void CAN_rx(can_msg_t *msg) {
 127         uint32_t spd;
 128         can_msg_t rx_msg;
 129         uint32_t req =0;
 130         memcpy(&rx_msg, msg, sizeof(can_msg_t));//make copy of message
 131
 132
 133         deb_led_on(LEDB);
 134
 135         switch (rx_msg.id)
 136         {
 137                 //JAW_LEFT_OPEN ff      0xff
 138                 //JAW_RIGHT_OPEN        0x00
 139
 140                 //JAW_LEFT_CLOSE        0x80
 141                 //JAW_RIGHT_CLOSE       0x80
 142
 143                 //JAW_LEFT_CATCH        0xB8
 144                 //JAW_RIGHT_CATCH       0x38
 145
 146                 case CAN_JAW_LEFT_CMD:
 147
 148                         deb_led_on(LEDB);
 149                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 150                         spd = rx_msg.data[2];
 151
 152                         if (req >= 0x50 && req <= 0xD0) {
 153                                 set_servo(JAW_LEFT, (char)req);
 154                         }
 155
 156 //                      if (req >= fsm_jaw_left.min_pos && req <= fsm_jaw_left.max_pos) {
 157 //                              fsm_jaw_left.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
 158 //                              fsm_jaw_left.can_req_position = req;
 159 //                              fsm_jaw_left.can_req_spd = spd;
 160 //                      } else
 161 //                              fsm_jaw_left.flags |= CAN_JAW_OUT_OF_BOUNDS;
 162                 break;
 163
 164                 case CAN_JAW_RIGHT_CMD:
 165
 166                         deb_led_on(LEDB);
 167                         req = ((rx_msg.data[0]<<8) | (rx_msg.data[1]));
 168                         spd = rx_msg.data[2];
 169
 170                         if (req >= 0x60 && req <= 0xD0) {
 171                                 set_servo(JAW_RIGHT, (char)req);
 172                         }
 173
 174 //                      if (req >= fsm_jaw_right.min_pos && req <= fsm_jaw_right.max_pos) {
 175 //                              fsm_jaw_right.flags &= ~CAN_JAW_OUT_OF_BOUNDS;
 176 //                              fsm_jaw_right.can_req_position = req;
 177 //                              fsm_jaw_right.can_req_spd = spd;
 178 //                      } else
 179 //                              fsm_jaw_right.flags |= CAN_JAW_OUT_OF_BOUNDS;
 180                 break;
 181
 182                 default:break;
 183         }
 184
 185         deb_led_off(LEDB);
 186 }
 187
 188
 189 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 190 void init_periphery(void){
 191
 192         can_init_baudrate(CAN_SPEED, CAN_ISR, CAN_rx);//initialization of CAN bus
 193         //init_motors();
 194
 195         /* init timer0 */
 196         init_timer0(1, CPU_APB_HZ/100000);
 197         set_irq_handler(4 /*timer0*/, TIMER_IRQ_PRIORITY, timer0_irq);
 198
 199         init_uart();
 200         init_adc(ADC_ISR);
 201
 202         init_servo(7);  //7 is interrupt priority
 203         set_servo(JAW_LEFT, 0xB0);
 204         set_servo(JAW_RIGHT,0x70);
 205 }
 206
 207 /*********************************************************/
 208 void can_send_status(struct fsm *fsm){
 209         can_msg_t tx_msg;
 210         tx_msg.id = fsm->can_id;
 211         tx_msg.dlc = 5;
 212         tx_msg.flags = 0;
 213         tx_msg.data[0] = (fsm->act_pos  >> 8) & 0xFF;
 214         tx_msg.data[1] = fsm->act_pos & 0xFF;
 215         tx_msg.data[2] = (fsm->can_response  >> 8) & 0xFF;
 216         tx_msg.data[3] = fsm->can_response & 0xFF;
 217         tx_msg.data[4] = fsm->flags;
 218         /*while*/(can_tx_msg(&tx_msg)); /* CAN erratum workaround */
 219
 220 }
 221
 222 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 223 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 224
 225 void dbg_print_time()
 226 {
 227         char str[10];
 228         unsigned t = timer_usec, i;
 229         memset(str, ' ', sizeof(str));
 230         str[9] = 0;
 231         str[8] = '\n';
 232         for (i=7; t > 0; i--) {
 233                 str[i] = t%10 + '0';
 234                 t /= 10;
 235         }
 236         send_rs_str(str);
 237 }
 238
 239 void fsm_jaw_init(struct fsm *fsm, enum event event);
 240
 241 void blink_led()
 242 {
 243         static uint32_t led_time = 0;
 244
 245         if(timer_msec >= led_time + 500)
 246         {
 247                 led_time = timer_msec;
 248                 /*  static int up;
 249                     if (up == 0)
 250                     fsm_vidle.can_req_position = 0x380;
 251                     if (up == 6)
 252                     fsm_vidle.can_req_position = 0x1e0;
 253                     up = (up+1)%12;
 254                 */
 255                 deb_led_change(LEDG);
 256
 257                 send_rs_str("LEFT_JAW: ");
 258                 send_rs_int(fsm_jaw_left.act_pos);
 259                 send_rs_str("\tLEFT_FLAGS: ");
 260                 send_rs_int(fsm_jaw_left.flags);
 261
 262                 send_rs_str("\tRIGHT_JAW: ");
 263                 send_rs_int(fsm_jaw_right.act_pos);
 264                 send_rs_str("\tRIGHT_FLAGS: ");
 265                 send_rs_int(fsm_jaw_right.flags);
 266
 267                 send_rs_str("\n");
 268
 269         }
 270 }
 271
 272 void handle_bumper()
 273 {
 274         static uint32_t bumper_time = 0;
 275         char sw = 0;
 276
 277         if (timer_msec >= bumper_time + 100)
 278         {
 279                 can_msg_t tx_msg;
 280
 281                 bumper_time = timer_msec;
 282
 283
 284
 285                 if (IO0PIN & (1<<BUMPER_REAR_LEFT)){
 286                         sw &= ~CAN_BUMPER_REAR_LEFT;
 287
 288                         send_rs_str("rear_left\n");
 289
 290 //                      deb_led_on(LEDY);
 291                 }
 292                 else{
 293                         sw |= CAN_BUMPER_REAR_LEFT;
 294
 295 //                      deb_led_off(LEDY);
 296                 }
 297
 298                 if (IO0PIN & (1<<BUMPER_REAR_RIGHT)){
 299                         sw &= ~CAN_BUMPER_REAR_RIGHT;
 300
 301                         send_rs_str("rear_right\n");
 302
 303 //                      deb_led_on(LEDY);
 304                 }
 305                 else{
 306                         sw |= CAN_BUMPER_REAR_RIGHT;
 307
 308 //                      deb_led_off(LEDY);
 309                 }
 310
 311                 if (IO0PIN & (1<<BUMPER_LEFT)){
 312
 313                         sw &= ~CAN_BUMPER_LEFT;
 314
 315                         send_rs_str("left\n");
 316
 317 //                      deb_led_on(LEDB);
 318                 }
 319                 else{
 320
 321                     sw |= CAN_BUMPER_LEFT;
 322
 323 //                  deb_led_off(LEDB);
 324                 }
 325                 if (IO0PIN & (1<<BUMPER_RIGHT)){
 326                         sw &= ~CAN_BUMPER_RIGHT;
 327                         send_rs_str("right\n");
 328 //                      deb_led_on(LEDG);
 329                 }
 330                 else{
 331                         sw |= CAN_BUMPER_RIGHT;
 332
 333 //                      deb_led_off(LEDG);
 334                 }
 335                 if (IO0PIN & (1<<BUMPER_LEFT_ACROSS)){
 336                         sw &= ~CAN_BUMPER_LEFT_ACROSS;
 337                         send_rs_str("left_across\n");
 338 //                      deb_led_on(LEDR);
 339                 }
 340                 else{
 341                         sw |= CAN_BUMPER_LEFT_ACROSS;
 342
 343 //                      deb_led_off(LEDR);
 344                 }
 345                 if (IO0PIN & (1<<BUMPER_RIGHT_ACROSS)){
 346                         sw &= ~CAN_BUMPER_RIGHT_ACROSS;
 347                         send_rs_str("right_across\n");
 348 //                      deb_led_on(LEDY);
 349                 }
 350                 else{
 351                         sw |= CAN_BUMPER_RIGHT_ACROSS;
 352
 353 //                      deb_led_off(LEDY);
 354                 }
 355
 356                 if (sw & (CAN_BUMPER_REAR_LEFT | CAN_BUMPER_REAR_RIGHT | CAN_BUMPER_LEFT | CAN_BUMPER_RIGHT | CAN_BUMPER_LEFT_ACROSS | CAN_BUMPER_RIGHT_ACROSS))
 357                         deb_led_on(LEDR);
 358                 else
 359                         deb_led_off(LEDR);
 360
 361 //              send_rs_int(IO1PIN);
 362 //              send_rs_int(sw);
 363 //              send_rs_str("\n");
 364 //
 365
 366                 tx_msg.id = CAN_ROBOT_BUMPERS;
 367                 tx_msg.dlc = 1;
 368                 tx_msg.flags = 0;
 369                 tx_msg.data[0] = sw;
 370
 371                 can_tx_msg(&tx_msg);
 372         }
 373 }
 374
 375
 376 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 377 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 378 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 379 int main(void)
 380 {
 381         uint32_t main_time = timer_usec;
 382         uint32_t status_time_left = timer_usec;
 383         uint32_t status_time_right = timer_usec;
 384
 385
 386         init_periphery();
 387
 388         SET_PIN(PINSEL1, 1, PINSEL_0);
 389         SET_PIN(PINSEL1, 2, PINSEL_0);
 390         SET_PIN(PINSEL1, 3, PINSEL_0);
 391         SET_PIN(PINSEL0, BUMPER_REAR_LEFT, PINSEL_0);
 392         SET_PIN(PINSEL0, BUMPER_REAR_RIGHT, PINSEL_0);
 393         SET_PIN(PINSEL0, BUMPER_RIGHT, PINSEL_0);
 394
 395 //      IO0DIR &= ~((1<<BUMPER_REAR) | (1<<BUMPER_RIGHT) | (1<<BUMPER_LEFT) | (1<<BUMPER_RIGHT_ACROSS) | (1<<BUMPER_LEFT_ACROSS));
 396
 397         send_rs_str("Jaws started\n");
 398
 399         //close jaw, max_pos, open jaw min_pos
 400         //letf jaw potenciometer on ADC 0, header J32 on board
 401         fsm_jaw_left.max_pos=0x320; //max 0x324
 402         fsm_jaw_left.min_pos=0xB0; //min 0xC3
 403         fsm_jaw_left.act_pos = adc_val[0];
 404
 405         //close jaw, max_pos, open jaw min_pos
 406         //letf jaw potenciometer on ADC 1, header J33 on board
 407         fsm_jaw_right.max_pos=0x380; //max 0x382
 408         fsm_jaw_right.min_pos=0x010; //min 0xF5
 409         fsm_jaw_right.act_pos = adc_val[1];
 410
 411         //left jaw engine is engine A, conector MOTA on board
 412         fsm_jaw_left.engine_en = &engine_A_en;
 413         fsm_jaw_left.engine_dir = &engine_A_dir;
 414         fsm_jaw_left.engine_pwm = &engine_A_pwm;
 415
 416         fsm_jaw_left.can_id=CAN_JAW_LEFT_STATUS;
 417
 418         //right jaw engine is engine B, conector MOTB on board
 419         fsm_jaw_right.engine_en = &engine_B_en;
 420         fsm_jaw_right.engine_dir = &engine_B_dir;
 421         fsm_jaw_right.engine_pwm = &engine_B_pwm;
 422
 423         fsm_jaw_right.can_id=CAN_JAW_RIGHT_STATUS;
 424
 425         init_fsm(&fsm_jaw_right, &fsm_jaw_init);
 426         init_fsm(&fsm_jaw_left, &fsm_jaw_init);
 427
 428 /*      test_vhn(); */
 429 /*      return; */
 430
 431         while(1){
 432                 if(timer_usec >= main_time + 1000)
 433                 {
 434                         main_time = timer_usec;
 435
 436                         //dbg_print_time();
 437
 438                         //fsm_jaw_left.act_pos = adc_val[0];
 439                         //run_fsm(&fsm_jaw_left);
 440
 441                         //fsm_jaw_right.act_pos = adc_val[1];
 442                         //run_fsm(&fsm_jaw_right);
 443
 444                 }
 445
 446                 if (timer_msec >= status_time_left + 100 /*|| //repeat sending message every 100 ms
 447                     fsm_jaw_left.trigger_can_send*/) {   //or when something important happen
 448                         //fsm_jaw_left.trigger_can_send = false;
 449                         status_time_left = timer_msec; //save new time, when message was sent
 450                         can_send_status(&fsm_jaw_left);
 451
 452                 }
 453
 454                 if (timer_msec >= status_time_right + 100 /*|| //repeat sending message every 100 ms
 455                     fsm_jaw_right.trigger_can_send*/) {   //or when something important happen
 456                         //fsm_jaw_right.trigger_can_send = false;
 457                         status_time_right = timer_msec; //save new time, when message was sent
 458                         can_send_status(&fsm_jaw_right);
 459
 460                 }
 461
 462                 handle_bumper();
 463
 464                 blink_led();
 465         }
 466 }
 467
 468 /** @} */