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