src/robobagr/robobagr.c

   1 #include <lpc21xx.h>                            /* LPC21xx definitions */
   2 #include <errno.h>
   3 #include <periph/can.h>
   4 #include <string.h>
   5 #include <deb_led.h>
   6 #include <can_ids.h>
   7 #include <can_msg_def.h>
   8 #include <servo.h>
   9 #include <powswitch.h>
  10 #include <engine.h>
  11 #include <adc.h>
  12 #include <uart_nozen.h>
  13 #include "robobagr.h"
  14 #include <string.h>
  15
  16
  17 #define START_PIN       15
  18
  19 int time_blink;
  20
  21 unsigned int cmu_buff[30];
  22 unsigned int cmu_buff_p = 0;
  23
  24
  25
  26 can_msg_t msg;
  27
  28 struct Color color;
  29
  30 unsigned char star_bt = 0;
  31 unsigned int time_start = 0;
  32
  33 void start_button(void)
  34 {
  35         if (star_bt > 10)
  36         {
  37                 if ((IO0PIN & (1<<START_PIN))==0) star_bt=0;
  38                 return;
  39         }
  40         if (time_start == 0) time_start = time_ms + START_TIME;
  41
  42         IO0DIR &= ~ (1<<START_PIN);
  43
  44         if (time_ms > time_start)
  45         {
  46                 time_start = time_ms + START_TIME;
  47
  48                 if (IO0PIN & (1<<START_PIN))
  49                 {
  50                         ++star_bt;
  51                         msg.data[0] = 1;
  52                         deb_led_off(LEDY);
  53                 }
  54                 else
  55                 {
  56                         deb_led_on(LEDY);
  57                         msg.data[0] = 0;
  58                 }
  59
  60                 msg.id = CAN_ROBOT_CMD;
  61                 msg.flags = 0;
  62                 msg.dlc = 1;
  63                 //msg.data[0] = (~(IO0PIN >> START_PIN)) & 1;
  64                 while (can_tx_msg(&msg));
  65
  66
  67
  68         }
  69 }
  70
  71
  72 void led_blik()
  73 {
  74         if (time_blink == 0) time_blink = time_ms + 500;
  75
  76         if (time_ms > time_blink)
  77         {
  78                 deb_led_change(LEDG);
  79                 time_blink = time_ms + 500;
  80         }
  81 }
  82
  83 void set_robobagr( unsigned char pwm)
  84 {
  85
  86         unsigned int speed = 0;
  87
  88         if ( pwm > 100 )
  89                 speed = pwm - 100;
  90         else
  91                 speed = 100 - pwm;
  92
  93         engine_A_pwm(speed);
  94
  95 }
  96
  97
  98 void go_to_sleep()
  99 {
 100 //      send_status(BAGR_STAT_POWERSAFE);
 101         engine_A_en(ENGINE_EN_OFF);  // engines off
 102         engine_A_pwm(0);
 103         engine_B_en(ENGINE_EN_OFF);
 104         engine_B_pwm(0);
 105
 106         int i = 1000000;
 107         while(--i);
 108         pow_switch_off();
 109
 110         deb_led_off(LEDY);
 111         deb_led_off(LEDB);
 112         deb_led_off(LEDG);
 113         deb_led_on(LEDR);
 114         can_off();
 115         PCON = 2;                               // bye bye
 116 }
 117
 118
 119 ///////////////////////////
 120
 121
 122
 123 int cmu_init(void)
 124 {
 125         if (cmu_timeout < time_ms) return 2;
 126         while (UART_new_data(UART1))
 127         {
 128                 int data = read_UART_data(UART1);
 129                 write_UART_data( UART0, data );
 130                 if (data == ':') return 1;
 131         }
 132
 133         return 0;
 134 }
 135
 136 unsigned int fil_red[4];
 137 unsigned int fil_green[4];
 138 unsigned int fil_blue[4];
 139
 140 void cmu_parse_message(void)
 141 {
 142         unsigned int val = 0;
 143         unsigned int i = 0;
 144         unsigned int iter = 1;
 145         unsigned int colors[6];
 146
 147         for (i = 0; i < 6; i++)
 148         {
 149                 colors[i] = 0;
 150         }
 151
 152         if (cmu_buff[0] != 'S') return;
 153         if (cmu_buff[1] != ' ') return;
 154
 155         for ( val = cmu_buff_p ; val > 0 ; --val)
 156         {
 157                 if ((cmu_buff[val] >= '0' ) & (cmu_buff[val] <= '9' )) break;
 158         }
 159
 160         for (i =0; i < 6 ;i++)
 161         {
 162                 iter = 1;
 163
 164                 for ( ; val > 0  ; --val)
 165                 {
 166                         if (cmu_buff[val] >= '0')
 167                         {
 168                                 colors[i] += (cmu_buff[val] - '0') * iter ;
 169                                 iter *= 10;
 170                         }
 171                         else
 172                         {
 173                                 --val;
 174                                 break;
 175                         }
 176                 }
 177         }
 178
 179         fil_red[0] = fil_red[1];
 180         fil_red[1] = fil_red[2];
 181         fil_red[2] = fil_red[3];
 182         fil_red[3] = colors[5];
 183         color.red = (fil_red[3] * 4 + fil_red[2] * 3 + fil_red[1] * 2 + fil_red[0] * 1) / 10;
 184
 185         fil_green[0] = fil_green[1];
 186         fil_green[1] = fil_green[2];
 187         fil_green[2] = fil_green[3];
 188         fil_green[3] = colors[4];
 189         color.green = (fil_green[3] * 4 + fil_green[2] * 3 + fil_green[1] * 2 + fil_green[0] * 1) / 10;
 190
 191         fil_blue[0] = fil_blue[1];
 192         fil_blue[1] = fil_blue[2];
 193         fil_blue[2] = fil_blue[3];
 194         fil_blue[3] = colors[3];
 195         color.blue = (fil_blue[3] * 4 + fil_blue[2] * 3 + fil_blue[1] * 2 + fil_blue[0] * 1) / 10;
 196
 197         color.d_red = colors[2];
 198         color.d_green =colors[1];
 199         color.d_blue =colors[0];
 200 }
 201
 202
 203
 204
 205 void cmu_set_param(void)
 206 {
 207         char param[] = "CR 18 44\r";
 208         int i =0;
 209
 210         for( i = 0 ; i < 9; i++){
 211                 write_UART_data( UART1, param[i] );
 212         //      write_UART_data( UART0, param[i] );
 213         }
 214
 215 }
 216
 217
 218 int cmu_ack(void)
 219 {
 220         if (cmu_timeout < time_ms) return 2;
 221         while (UART_new_data(UART1))
 222         {
 223                 cmu_buff[cmu_buff_p]= read_UART_data(UART1);
 224         //      write_UART_data( UART0, cmu_buff[cmu_buff_p]);
 225
 226                 if (cmu_buff[cmu_buff_p] == ':')
 227                 {
 228
 229                         int i =0;
 230                         for ( i = 0; i <= cmu_buff_p ; i++)
 231                         {
 232                                 if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;}
 233                                 if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;}
 234                         }
 235                         cmu_buff_p = 0;
 236                         return 2;
 237                 }
 238                 ++cmu_buff_p;
 239         }
 240
 241         return 0;
 242 }
 243
 244 void cmu_set_gm(void)
 245 {
 246         char param[] = "GM\r";
 247         int i =0;
 248
 249         for( i = 0 ; i < 3; i++){
 250                 write_UART_data( UART1, param[i] );
 251                 //write_UART_data( UART0, param[i] );
 252         }
 253
 254 }
 255
 256 int cmu_ack_gm()
 257 {
 258         if (cmu_timeout < time_ms) return 2;
 259         while (UART_new_data(UART1))
 260         {
 261                 cmu_buff[cmu_buff_p]= read_UART_data(UART1);
 262                 //write_UART_data( UART0, cmu_buff[cmu_buff_p]);
 263
 264                 if (cmu_buff[cmu_buff_p] == '\r')
 265                 {
 266
 267                         int i =0;
 268                         for ( i = 0; i <= cmu_buff_p ; i++)
 269                         {
 270                                 //write_UART_data( UART0, cmu_buff[i]);
 271
 272                                 if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;}
 273                                 if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;}
 274                         }
 275                         cmu_buff_p = 0;
 276                         return 2;
 277                 }
 278                 ++cmu_buff_p;
 279         }
 280
 281         return 0;
 282 }
 283
 284
 285 void atoim(unsigned int val)
 286 {
 287         write_UART_data( UART0, ' ' );
 288         write_UART_data( UART0, (val / 100 + 48) );
 289         write_UART_data( UART0, ((val % 100) / 10 + 48) );
 290         write_UART_data( UART0, ((val % 100) % 10 + 48) );
 291
 292 }
 293
 294 unsigned int time_adc =0;
 295 void send_adc()
 296 {
 297         if (time_adc == 0) time_adc = time_ms + ADC_TIME;
 298
 299         if (time_ms > time_adc)
 300         {
 301
 302                 unsigned int data[4];
 303                 data[0] = adc_val[0];
 304                 data[1] = adc_val[1];
 305                 data[2] = adc_val[2];
 306                 data[3] = adc_val[3];
 307
 308
 309
 310                 msg.id = CAN_ADC_2;
 311                 msg.flags = 0;
 312                 msg.dlc = 8;
 313                 msg.data[0] = (data[0]>>8) & 0xFF;
 314                 msg.data[1] = data[0] & 0xFF;
 315                 msg.data[2] = (data[1]>>8) & 0xFF;
 316                 msg.data[3] = data[1] & 0xFF;
 317                 msg.data[4] = (data[2]>>8) & 0xFF;
 318                 msg.data[5] = data[2] & 0xFF;
 319                 msg.data[6] = (data[3]>>8) & 0xFF;
 320                 msg.data[7] = data[3] & 0xFF;
 321                 while (can_tx_msg(&msg));
 322
 323
 324                 time_adc = time_ms + ADC_TIME;
 325         }
 326
 327 }
 328
 329
 330 void cmu_treshold()
 331 {
 332         unsigned int val = 0;
 333
 334         if (color.red > 120) val |= 1;
 335         if (color.blue > 120) val |= 1;
 336
 337         msg.id = CAN_CMU;
 338         msg.flags = 0;
 339         msg.dlc = 1;
 340         msg.data[0] = val & 0xFF;
 341         while (can_tx_msg(&msg));
 342 }
 343
 344
 345
 346 int cmu_gm(void)
 347 {
 348         int test;
 349
 350
 351         if (UART_new_data(UART1))
 352         {
 353                 test = read_UART_data(UART1);
 354                 if (test == '\r')
 355                 {
 356
 357                         if (cmu_buff_p < 13)   // if recieved chars is not full message
 358                         {
 359                                 cmu_buff_p = 0;
 360                                 //return 2;
 361                         }
 362
 363                         cmu_parse_message();
 364
 365                         cmu_treshold();
 366                         //write_UART_data( UART0, '\n');
 367
 368                         cmu_buff_p = 0;
 369                 }
 370                 else
 371                 {
 372                         cmu_buff[cmu_buff_p] = test;
 373                         write_UART_data( UART0, cmu_buff[cmu_buff_p]);
 374                         ++cmu_buff_p;
 375                 }
 376         }
 377         return 0;
 378 }
 379
 380
 381