src/robobagr/main.c

   1
   2
   3 #include <lpc21xx.h>                            /* LPC21xx definitions */
   4 #include <errno.h>
   5 #include <periph/can.h>
   6 #include <string.h>
   7 #include <deb_led.h>
   8 #include <can_ids.h>
   9 #include <servo.h>
  10 #include <powswitch.h>
  11 #include <engine.h>
  12 #include <adc.h>
  13 #include <uart_nozen.h>
  14
  15
  16
  17
  18 #define CAN_SPEED       1000000
  19 #define CAN_ISR         0
  20
  21 #define UART0_ISR       1
  22 #define UART1_ISR       2
  23
  24 #define SERVO_ISR       5
  25
  26 #define CR 0x0d
  27
  28 can_msg_t msg;
  29
  30 unsigned int cmu_buff[30];
  31 unsigned int cmu_buff_p = 0;
  32
  33 struct Color{
  34   unsigned char red;
  35   unsigned char green;
  36   unsigned char blue;
  37   unsigned char d_red;
  38   unsigned char d_green;
  39   unsigned char d_blue;
  40 } ;
  41
  42 struct Color color;
  43
  44
  45
  46
  47
  48 void dummy_wait(void)
  49 {
  50     int i = 1000000;
  51
  52     while(--i);
  53 }
  54
  55 int time_blink;
  56
  57 void led_blik()
  58 {
  59         if (time_blink == 0) time_blink = time_ms + 500;
  60
  61         if (time_ms > time_blink)
  62         {
  63                 deb_led_change(LEDG);
  64                 time_blink = time_ms + 500;
  65         }
  66
  67 }
  68
  69
  70
  71
  72 void can_rx(can_msg_t *msg) {
  73         can_msg_t rx_msg;
  74
  75         memcpy(&rx_msg, msg, sizeof(can_msg_t));
  76
  77
  78
  79
  80         switch (rx_msg.id) {
  81                 case CAN_SERVO:
  82
  83                         set_servo(0, rx_msg.data[0]);
  84                         set_servo(1, rx_msg.data[1]);
  85                         set_servo(2, rx_msg.data[2]);
  86
  87                         break;
  88
  89
  90
  91                 default:
  92                         break;
  93         }
  94 }
  95
  96
  97
  98 void init_perip(void)      // inicializace periferii mikroprocesoru
  99 {
 100         init_servo(SERVO_ISR);
 101         init_pow_switch();
 102
 103         init_engine_A();
 104         engine_A_en(ENGINE_EN_ON);
 105         engine_A_dir(ENGINE_DIR_FW);
 106         engine_A_pwm(20);
 107         pow_switch_on();
 108         //init_engine_B();
 109
 110         UART_init( UART0,UART0_ISR, 19200, UART_BITS_8, UART_STOP_BIT_1,  UART_PARIT_OFF, 0);
 111         UART_init( UART1,UART1_ISR, 115100, UART_BITS_8, UART_STOP_BIT_1,  UART_PARIT_OFF, 0);
 112         can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx);
 113 }
 114
 115
 116 int cmu_timeout;
 117
 118 int cmu_init(void)
 119 {
 120
 121         if (cmu_timeout < time_ms) return 2;
 122         while (UART_new_data(UART1))
 123         {
 124                 int data = read_UART_data(UART1);
 125                 write_UART_data( UART0, data );
 126                 if (data == ':') return 1;
 127         }
 128
 129         return 0;
 130
 131 }
 132
 133 void cmu_parse_message(void)
 134 {
 135         unsigned int val = 0;
 136         unsigned int i = 0;
 137         unsigned int iter = 1;
 138         unsigned int colors[6];
 139
 140
 141
 142         for (i = 0; i < 6; i++)
 143         {
 144                 colors[i] = 0;
 145         }
 146
 147
 148
 149         if (cmu_buff[0] != 'S') return;
 150         if (cmu_buff[1] != ' ') return;
 151
 152
 153
 154         for ( val = cmu_buff_p ; val > 0 ; --val)
 155         {
 156
 157                 if ((cmu_buff[val] >= '0' ) & (cmu_buff[val] <= '9' )) break;
 158
 159
 160         }
 161
 162
 163         for (i =0; i < 6 ;i++)
 164         {
 165                 iter = 1;
 166
 167                 for ( ; val > 0  ; --val)
 168                 {
 169                         if (cmu_buff[val] >= '0')
 170                         {
 171                                 colors[i] += (cmu_buff[val] - '0') * iter ;
 172                                 iter *= 10;
 173                         }
 174                         else
 175                         {
 176                                 --val;
 177                                 break;
 178                         }
 179                 }
 180         }
 181
 182
 183
 184
 185
 186         color.red = colors[5];
 187         color.green = colors[4];
 188         color.blue =  colors[3];
 189         color.d_red = colors[2];
 190         color.d_green =colors[1];
 191         color.d_blue =colors[0];
 192 }
 193
 194
 195
 196
 197 void cmu_set_param(void)
 198 {
 199         char param[] = "CR 18 44\r";
 200         int i =0;
 201
 202         for( i = 0 ; i < 9; i++){
 203                 write_UART_data( UART1, param[i] );
 204                 write_UART_data( UART0, param[i] );
 205         }
 206
 207 }
 208
 209
 210 int cmu_ack(void)
 211 {
 212
 213         if (cmu_timeout < time_ms) return 2;
 214         while (UART_new_data(UART1))
 215         {
 216                 cmu_buff[cmu_buff_p]= read_UART_data(UART1);
 217                 write_UART_data( UART0, cmu_buff[cmu_buff_p]);
 218
 219                 if (cmu_buff[cmu_buff_p] == ':')
 220                 {
 221
 222                         int i =0;
 223                         for ( i = 0; i <= cmu_buff_p ; i++)
 224                         {
 225                                 if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;}
 226                                 if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;}
 227                         }
 228                         cmu_buff_p = 0;
 229                         return 2;
 230                 }
 231                 ++cmu_buff_p;
 232         }
 233
 234         return 0;
 235 }
 236
 237 void cmu_set_gm(void)
 238 {
 239         char param[] = "GM\r";
 240         int i =0;
 241
 242         for( i = 0 ; i < 3; i++){
 243                 write_UART_data( UART1, param[i] );
 244                 write_UART_data( UART0, param[i] );
 245         }
 246
 247 }
 248
 249 int cmu_ack_gm()
 250 {
 251         if (cmu_timeout < time_ms) return 2;
 252         while (UART_new_data(UART1))
 253         {
 254                 cmu_buff[cmu_buff_p]= read_UART_data(UART1);
 255                 write_UART_data( UART0, cmu_buff[cmu_buff_p]);
 256
 257                 if (cmu_buff[cmu_buff_p] == '\r')
 258                 {
 259
 260                         int i =0;
 261                         for ( i = 0; i <= cmu_buff_p ; i++)
 262                         {
 263                                 write_UART_data( UART0, cmu_buff[i]);
 264
 265                                 if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;}
 266                                 if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;}
 267                         }
 268                         cmu_buff_p = 0;
 269                         return 2;
 270                 }
 271                 ++cmu_buff_p;
 272         }
 273
 274         return 0;
 275 }
 276
 277
 278 void atoim(unsigned int val)
 279 {
 280         write_UART_data( UART0, ' ' );
 281         write_UART_data( UART0, (val / 100 + 48) );
 282         write_UART_data( UART0, ((val % 100) / 10 + 48) );
 283         write_UART_data( UART0, ((val % 100) % 10 + 48) );
 284
 285 }
 286
 287
 288 int cmu_gm(void)
 289 {
 290         int test;
 291
 292
 293         if (UART_new_data(UART1))
 294         {
 295                 test = read_UART_data(UART1);
 296                 if (test == '\r')
 297                 {
 298
 299                         if (cmu_buff_p < 13)   // if recieved chars is not full message
 300                         {
 301                                 cmu_buff_p = 0;
 302                                 //return 2;
 303                         }
 304
 305                         cmu_parse_message();
 306
 307                         atoim(color.red );
 308                         atoim(color.green );
 309                         atoim(color.blue );
 310                         atoim(color.d_red );
 311                         atoim(color.d_green );
 312                         atoim(color.d_blue );
 313                         write_UART_data( UART0, '\n');
 314
 315                         cmu_buff_p = 0;
 316                 }
 317                 else
 318                 {
 319                         cmu_buff[cmu_buff_p] = test;
 320                         write_UART_data( UART0, cmu_buff[cmu_buff_p]);
 321                         ++cmu_buff_p;
 322                 }
 323         }
 324         return 0;
 325
 326 }
 327
 328
 329 void ui(void)
 330 {
 331         while(1)
 332         {
 333                 while (UART_new_data(UART1))
 334                         write_UART_data( UART0, read_UART_data(UART1));
 335                 while (UART_new_data(UART0))
 336                         write_UART_data( UART1, read_UART_data(UART0));
 337         }
 338 }
 339
 340 int main (void)  {
 341
 342
 343         init_perip();                   // sys init MCU
 344
 345         set_servo(0, 0x80);
 346         set_servo(1, 0x80);
 347         set_servo(2, 0x80);
 348
 349         int state = 0;
 350         int ret =0;
 351         cmu_timeout = time_ms + 6000;
 352
 353 //      ui();
 354
 355         write_UART_data( UART1,  '\r');         // this gives some response if cmucam is running
 356                                                         // needed for cmu_init
 357
 358
 359         while(1)
 360         {
 361
 362
 363                 if (UART_new_data(UART0))
 364                 {
 365                         engine_A_en(ENGINE_EN_OFF);
 366                         engine_A_pwm(0);
 367                 }
 368
 369
 370                 led_blik();
 371
 372
 373                 switch (state)
 374                 {
 375                         case 0:
 376
 377                                 ret = cmu_init();
 378                                 if(ret == 1) state = 1;
 379                                 if(ret == 2) state = 9;
 380                                 break;
 381
 382                         case 1:
 383                                 cmu_set_param();
 384                                 state = 2;
 385                                 cmu_timeout = time_ms + 400;
 386                                 //cmu_buff_p = 0;
 387                                 break;
 388
 389                         case 2:
 390                                 ret = cmu_ack();
 391                                 if (ret ==  1) state = 3;
 392                                 if (ret ==  2) state = 9;
 393                                 break;
 394
 395                         case 3:
 396
 397                                 cmu_set_gm();
 398                                 state = 4;
 399                                 cmu_timeout = time_ms + 400;
 400                                 //cmu_buff_p = 0;
 401                                 break;
 402
 403                         case 4:
 404                                 ret = cmu_ack_gm();
 405                                 if (ret ==  1) state = 5;
 406                                 if (ret ==  2) state = 9;
 407                                 break;
 408
 409                         case 5:
 410                                 ret = cmu_gm();
 411                                 deb_led_on(LEDY);
 412                                 if(ret == 2) state = 9;
 413                                 break;
 414
 415
 416                         case 9: deb_led_on(LEDR);
 417                                 break;
 418
 419                         default:
 420                                 deb_led_on(LEDR);
 421                                 deb_led_on(LEDY);
 422                                 break;
 423                 }
 424         }
 425 }
 426 /*              msg.id = 0xAA;
 427                 msg.flags = 0;
 428                 msg.dlc = 1;
 429                 msg.data[0] = 0x55;
 430
 431
 432
 433         //      while(can_tx_msg(&msg));*/
 434
 435 /*              while (UART_new_data(UART1))
 436                 {
 437                         write_UART_data( UART0, read_UART_data(UART1));
 438                 }
 439
 440                 while (UART_new_data(UART0))
 441                 {
 442                         write_UART_data( UART1, read_UART_data(UART0));
 443                 }*/
 444 //      test = UART_test_err(UART1);
 445 //
 446 //      if(test == -1)
 447 //      {
 448 //              deb_led_on(LEDY);
 449 //              while(1);
 450 //      }
 451 //
 452 //      if(test == -2)
 453 //      {
 454 //              deb_led_on(LEDB);
 455 //              deb_led_on(LEDY);
 456 //              while(1);
 457 //      }
 458
 459