From ffead2db682634e5f13468fc85751d50bc2aa4ae Mon Sep 17 00:00:00 2001 From: Jiri Kubias Date: Thu, 1 May 2008 00:32:12 +0200 Subject: [PATCH] eb_robobagr - toomany changes for comment, sorry short description: added cmu protocol, auto on light, error handling, adc sending, servo handling --- src/robobagr/Makefile.omk | 2 +- src/robobagr/main.c | 345 ++++++++------------------------------ src/robobagr/robobagr.c | 337 +++++++++++++++++++++++++++++++++++++ src/robobagr/robobagr.h | 39 +++++ 4 files changed, 449 insertions(+), 274 deletions(-) create mode 100644 src/robobagr/robobagr.c create mode 100644 src/robobagr/robobagr.h diff --git a/src/robobagr/Makefile.omk b/src/robobagr/Makefile.omk index a4c16080..f8a8a326 100644 --- a/src/robobagr/Makefile.omk +++ b/src/robobagr/Makefile.omk @@ -2,7 +2,7 @@ bin_PROGRAMS = eb_robobagr -eb_robobagr_SOURCES = main.c +eb_robobagr_SOURCES = main.c robobagr.c eb_robobagr_LIBS = can ebb uart_nozen diff --git a/src/robobagr/main.c b/src/robobagr/main.c index 66ed3f59..a58e0d01 100644 --- a/src/robobagr/main.c +++ b/src/robobagr/main.c @@ -6,11 +6,13 @@ #include #include #include +#include #include #include #include #include #include +#include "robobagr.h" @@ -20,91 +22,62 @@ #define UART0_ISR 1 #define UART1_ISR 2 +#define ADC_ISR 3 #define SERVO_ISR 5 +#define ERROR_CMU_MAX 10 #define CR 0x0d + can_msg_t msg; - -unsigned int cmu_buff[30]; -unsigned int cmu_buff_p = 0; - -struct Color{ - unsigned char red; - unsigned char green; - unsigned char blue; - unsigned char d_red; - unsigned char d_green; - unsigned char d_blue; -} ; - -struct Color color; - - - - - -void dummy_wait(void) -{ - int i = 1000000; - - while(--i); -} - -int time_blink; - -void led_blik() -{ - if (time_blink == 0) time_blink = time_ms + 500; - - if (time_ms > time_blink) - { - deb_led_change(LEDG); - time_blink = time_ms + 500; - } - -} - - - +unsigned int time_timeout = 0; void can_rx(can_msg_t *msg) { can_msg_t rx_msg; memcpy(&rx_msg, msg, sizeof(can_msg_t)); - - - switch (rx_msg.id) { case CAN_SERVO: - + time_timeout = time_ms + 2000; + deb_led_on(LEDB); set_servo(0, rx_msg.data[0]); set_servo(1, rx_msg.data[1]); set_servo(2, rx_msg.data[2]); - break; - + case CAN_DRIVES: + time_timeout = time_ms + 2000; + set_robobagr(rx_msg.data[2]); + + case CAN_PWR_ALERT: + if ( rx_msg.data[0] == 2 ) go_to_sleep(); + break; default: break; } + deb_led_off(LEDB); } + + + + void init_perip(void) // inicializace periferii mikroprocesoru { init_servo(SERVO_ISR); + init_adc(ADC_ISR); init_pow_switch(); init_engine_A(); - engine_A_en(ENGINE_EN_ON); - engine_A_dir(ENGINE_DIR_FW); - engine_A_pwm(20); - pow_switch_on(); + engine_A_en(ENGINE_EN_OFF); + engine_A_dir(ENGINE_DIR_BW); + engine_A_pwm(0); + pow_switch_off(); //init_engine_B(); UART_init( UART0,UART0_ISR, 19200, UART_BITS_8, UART_STOP_BIT_1, UART_PARIT_OFF, 0); @@ -112,242 +85,57 @@ void init_perip(void) // inicializace periferii mikroprocesoru can_init_baudrate(CAN_SPEED, CAN_ISR, can_rx); } - -int cmu_timeout; - -int cmu_init(void) +void send_status(unsigned char status) { - - if (cmu_timeout < time_ms) return 2; - while (UART_new_data(UART1)) - { - int data = read_UART_data(UART1); - write_UART_data( UART0, data ); - if (data == ':') return 1; - } - - return 0; - + msg.id = CAN_BAGR_STATUS; + msg.flags = 0; + msg.dlc = 1; + msg.data[0] = status; + while (can_tx_msg(&msg)); } -void cmu_parse_message(void) -{ - unsigned int val = 0; - unsigned int i = 0; - unsigned int iter = 1; - unsigned int colors[6]; - - - - for (i = 0; i < 6; i++) - { - colors[i] = 0; - } - - - - if (cmu_buff[0] != 'S') return; - if (cmu_buff[1] != ' ') return; - - - - for ( val = cmu_buff_p ; val > 0 ; --val) - { - - if ((cmu_buff[val] >= '0' ) & (cmu_buff[val] <= '9' )) break; - - - } - for (i =0; i < 6 ;i++) +void ui(void) +{ + while(1) { - iter = 1; - - for ( ; val > 0 ; --val) - { - if (cmu_buff[val] >= '0') - { - colors[i] += (cmu_buff[val] - '0') * iter ; - iter *= 10; - } - else - { - --val; - break; - } - } + while (UART_new_data(UART1)) + write_UART_data( UART0, read_UART_data(UART1)); + while (UART_new_data(UART0)) + write_UART_data( UART1, read_UART_data(UART0)); } - - - - - - color.red = colors[5]; - color.green = colors[4]; - color.blue = colors[3]; - color.d_red = colors[2]; - color.d_green =colors[1]; - color.d_blue =colors[0]; } -void cmu_set_param(void) -{ - char param[] = "CR 18 44\r"; - int i =0; - - for( i = 0 ; i < 9; i++){ - write_UART_data( UART1, param[i] ); - write_UART_data( UART0, param[i] ); - } - -} -int cmu_ack(void) -{ - - if (cmu_timeout < time_ms) return 2; - while (UART_new_data(UART1)) - { - cmu_buff[cmu_buff_p]= read_UART_data(UART1); - write_UART_data( UART0, cmu_buff[cmu_buff_p]); - - if (cmu_buff[cmu_buff_p] == ':') - { - - int i =0; - for ( i = 0; i <= cmu_buff_p ; i++) - { - if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;} - if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;} - } - cmu_buff_p = 0; - return 2; - } - ++cmu_buff_p; - } - - return 0; -} -void cmu_set_gm(void) -{ - char param[] = "GM\r"; - int i =0; - - for( i = 0 ; i < 3; i++){ - write_UART_data( UART1, param[i] ); - write_UART_data( UART0, param[i] ); - } +int main (void) { -} +int a = 0; + init_perip(); // sys init MCU -int cmu_ack_gm() -{ - if (cmu_timeout < time_ms) return 2; - while (UART_new_data(UART1)) - { - cmu_buff[cmu_buff_p]= read_UART_data(UART1); - write_UART_data( UART0, cmu_buff[cmu_buff_p]); - - if (cmu_buff[cmu_buff_p] == '\r') - { - - int i =0; - for ( i = 0; i <= cmu_buff_p ; i++) - { - write_UART_data( UART0, cmu_buff[i]); - - if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;} - if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;} - } - cmu_buff_p = 0; - return 2; - } - ++cmu_buff_p; - } - return 0; -} - + set_servo(0, 0x80); + set_servo(1, 0x80); + set_servo(2, 0x80); -void atoim(unsigned int val) -{ - write_UART_data( UART0, ' ' ); - write_UART_data( UART0, (val / 100 + 48) ); - write_UART_data( UART0, ((val % 100) / 10 + 48) ); - write_UART_data( UART0, ((val % 100) % 10 + 48) ); - -} + a = time_ms + 2000; -int cmu_gm(void) -{ - int test; - + while(time_ms < a) - if (UART_new_data(UART1)) - { - test = read_UART_data(UART1); - if (test == '\r') - { - - if (cmu_buff_p < 13) // if recieved chars is not full message - { - cmu_buff_p = 0; - //return 2; - } - - cmu_parse_message(); - - atoim(color.red ); - atoim(color.green ); - atoim(color.blue ); - atoim(color.d_red ); - atoim(color.d_green ); - atoim(color.d_blue ); - write_UART_data( UART0, '\n'); - - cmu_buff_p = 0; - } - else - { - cmu_buff[cmu_buff_p] = test; - write_UART_data( UART0, cmu_buff[cmu_buff_p]); - ++cmu_buff_p; - } - } - return 0; + engine_A_en(ENGINE_EN_ON); -} - - -void ui(void) -{ - while(1) - { - while (UART_new_data(UART1)) - write_UART_data( UART0, read_UART_data(UART1)); - while (UART_new_data(UART0)) - write_UART_data( UART1, read_UART_data(UART0)); - } -} - -int main (void) { - + send_status(BAGR_STAT_BOOTING); - init_perip(); // sys init MCU - - set_servo(0, 0x80); - set_servo(1, 0x80); - set_servo(2, 0x80); int state = 0; int ret =0; + unsigned int error_counter = 0; cmu_timeout = time_ms + 6000; // ui(); @@ -355,21 +143,16 @@ int main (void) { write_UART_data( UART1, '\r'); // this gives some response if cmucam is running // needed for cmu_init + time_timeout = time_ms + 2000; while(1) - { - - - if (UART_new_data(UART0)) - { - engine_A_en(ENGINE_EN_OFF); - engine_A_pwm(0); - } - + { led_blik(); + send_adc(); + + if(time_ms > time_timeout) engine_A_pwm(0); - switch (state) { case 0: @@ -402,18 +185,34 @@ int main (void) { case 4: ret = cmu_ack_gm(); - if (ret == 1) state = 5; + if (ret == 1) + { + state = 5; + pow_switch_on(); + send_status(BAGR_STAT_OK); + //engine_A_pwm(30); + } if (ret == 2) state = 9; break; case 5: ret = cmu_gm(); - deb_led_on(LEDY); + if(ret == 2) state = 9; break; case 9: deb_led_on(LEDR); + write_UART_data( UART1, '\r'); // this gives some response if cmucam is running + ++error_counter; + if (error_counter > ERROR_CMU_MAX) + { + send_status(BAGR_STAT_CMU_ERROR); + error_counter = 0; + } + + state = 0; + break; default: diff --git a/src/robobagr/robobagr.c b/src/robobagr/robobagr.c new file mode 100644 index 00000000..5984496c --- /dev/null +++ b/src/robobagr/robobagr.c @@ -0,0 +1,337 @@ +#include /* LPC21xx definitions */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "robobagr.h" + + +int time_blink; + +unsigned int cmu_buff[30]; +unsigned int cmu_buff_p = 0; + + +can_msg_t msg; + +struct Color color; + + + +void led_blik() +{ + if (time_blink == 0) time_blink = time_ms + 500; + + if (time_ms > time_blink) + { + deb_led_change(LEDG); + time_blink = time_ms + 500; + } +} + +void set_robobagr( unsigned char pwm) +{ + + unsigned int speed = 0; + + if ( pwm > 100 ) + speed = pwm - 100; + else + speed = 100 - pwm; + + engine_A_pwm(speed); + +} + + +void go_to_sleep() +{ +// send_status(BAGR_STAT_POWERSAFE); + engine_A_en(ENGINE_EN_OFF); // engines off + engine_A_pwm(0); + engine_B_en(ENGINE_EN_OFF); + engine_B_pwm(0); + + int i = 1000000; + while(--i); + pow_switch_off(); + + deb_led_off(LEDY); + deb_led_off(LEDB); + deb_led_off(LEDG); + deb_led_on(LEDR); + can_off(); + PCON = 2; // bye bye +} + + +/////////////////////////// + + + +int cmu_init(void) +{ + if (cmu_timeout < time_ms) return 2; + while (UART_new_data(UART1)) + { + int data = read_UART_data(UART1); + write_UART_data( UART0, data ); + if (data == ':') return 1; + } + + return 0; +} + +unsigned int fil_red[4]; +unsigned int fil_green[4]; +unsigned int fil_blue[4]; + +void cmu_parse_message(void) +{ + unsigned int val = 0; + unsigned int i = 0; + unsigned int iter = 1; + unsigned int colors[6]; + + for (i = 0; i < 6; i++) + { + colors[i] = 0; + } + + if (cmu_buff[0] != 'S') return; + if (cmu_buff[1] != ' ') return; + + for ( val = cmu_buff_p ; val > 0 ; --val) + { + if ((cmu_buff[val] >= '0' ) & (cmu_buff[val] <= '9' )) break; + } + + for (i =0; i < 6 ;i++) + { + iter = 1; + + for ( ; val > 0 ; --val) + { + if (cmu_buff[val] >= '0') + { + colors[i] += (cmu_buff[val] - '0') * iter ; + iter *= 10; + } + else + { + --val; + break; + } + } + } + + fil_red[0] = fil_red[1]; + fil_red[1] = fil_red[2]; + fil_red[2] = fil_red[3]; + fil_red[3] = colors[5]; + color.red = (fil_red[3] * 4 + fil_red[2] * 3 + fil_red[1] * 2 + fil_red[0] * 1) / 10; + + fil_green[0] = fil_green[1]; + fil_green[1] = fil_green[2]; + fil_green[2] = fil_green[3]; + fil_green[3] = colors[4]; + color.green = (fil_green[3] * 4 + fil_green[2] * 3 + fil_green[1] * 2 + fil_green[0] * 1) / 10; + + fil_blue[0] = fil_blue[1]; + fil_blue[1] = fil_blue[2]; + fil_blue[2] = fil_blue[3]; + fil_blue[3] = colors[3]; + color.blue = (fil_blue[3] * 4 + fil_blue[2] * 3 + fil_blue[1] * 2 + fil_blue[0] * 1) / 10; + + color.d_red = colors[2]; + color.d_green =colors[1]; + color.d_blue =colors[0]; +} + + + + +void cmu_set_param(void) +{ + char param[] = "CR 18 44\r"; + int i =0; + + for( i = 0 ; i < 9; i++){ + write_UART_data( UART1, param[i] ); + write_UART_data( UART0, param[i] ); + } + +} + + +int cmu_ack(void) +{ + if (cmu_timeout < time_ms) return 2; + while (UART_new_data(UART1)) + { + cmu_buff[cmu_buff_p]= read_UART_data(UART1); + write_UART_data( UART0, cmu_buff[cmu_buff_p]); + + if (cmu_buff[cmu_buff_p] == ':') + { + + int i =0; + for ( i = 0; i <= cmu_buff_p ; i++) + { + if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;} + if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;} + } + cmu_buff_p = 0; + return 2; + } + ++cmu_buff_p; + } + + return 0; +} + +void cmu_set_gm(void) +{ + char param[] = "GM\r"; + int i =0; + + for( i = 0 ; i < 3; i++){ + write_UART_data( UART1, param[i] ); + //write_UART_data( UART0, param[i] ); + } + +} + +int cmu_ack_gm() +{ + if (cmu_timeout < time_ms) return 2; + while (UART_new_data(UART1)) + { + cmu_buff[cmu_buff_p]= read_UART_data(UART1); + write_UART_data( UART0, cmu_buff[cmu_buff_p]); + + if (cmu_buff[cmu_buff_p] == '\r') + { + + int i =0; + for ( i = 0; i <= cmu_buff_p ; i++) + { + write_UART_data( UART0, cmu_buff[i]); + + if (cmu_buff[i] == 'N') { cmu_buff_p = 0; return 2;} + if (cmu_buff[i] == 'A') { cmu_buff_p = 0; return 1;} + } + cmu_buff_p = 0; + return 2; + } + ++cmu_buff_p; + } + + return 0; +} + + +void atoim(unsigned int val) +{ + write_UART_data( UART0, ' ' ); + write_UART_data( UART0, (val / 100 + 48) ); + write_UART_data( UART0, ((val % 100) / 10 + 48) ); + write_UART_data( UART0, ((val % 100) % 10 + 48) ); + +} + +unsigned int time_adc =0; +void send_adc() +{ + if (time_adc == 0) time_adc = time_ms + ADC_TIME; + + if (time_ms > time_adc) + { + + unsigned int data[4]; + data[0] = adc_val[0]; + data[1] = adc_val[1]; + data[2] = adc_val[2]; + data[3] = adc_val[3]; + + + + msg.id = CAN_ADC_2; + msg.flags = 0; + msg.dlc = 8; + msg.data[0] = (data[0]>>8) & 0xFF; + msg.data[1] = data[0] & 0xFF; + msg.data[2] = (data[1]>>8) & 0xFF; + msg.data[3] = data[1] & 0xFF; + msg.data[4] = (data[2]>>8) & 0xFF; + msg.data[5] = data[2] & 0xFF; + msg.data[6] = (data[3]>>8) & 0xFF; + msg.data[7] = data[3] & 0xFF; + while (can_tx_msg(&msg)); + + + time_adc = time_ms + ADC_TIME; + } + +} + + +void cmu_treshold() +{ + unsigned int val = 0; + + if (color.red > 120) val |= 1; + if (color.blue > 120) val |= 1; + + msg.id = CAN_CMU; + msg.flags = 0; + msg.dlc = 1; + msg.data[0] = val & 0xFF; + while (can_tx_msg(&msg)); +} + + + +int cmu_gm(void) +{ + int test; + + + if (UART_new_data(UART1)) + { + test = read_UART_data(UART1); + if (test == '\r') + { + + if (cmu_buff_p < 13) // if recieved chars is not full message + { + cmu_buff_p = 0; + //return 2; + } + + cmu_parse_message(); + + cmu_treshold(); + //write_UART_data( UART0, '\n'); + + cmu_buff_p = 0; + } + else + { + cmu_buff[cmu_buff_p] = test; + write_UART_data( UART0, cmu_buff[cmu_buff_p]); + ++cmu_buff_p; + } + } + return 0; +} + + + diff --git a/src/robobagr/robobagr.h b/src/robobagr/robobagr.h new file mode 100644 index 00000000..287feb92 --- /dev/null +++ b/src/robobagr/robobagr.h @@ -0,0 +1,39 @@ +#ifndef ROBOBAGR_H +#define ROBOBAGR_H + + + +#define ADC_TIME 60; + +unsigned int cmu_timeout; + + + + + +//void send_status(unsigned char status); +void led_blik(); +void set_robobagr( unsigned char pwm); +void go_to_sleep(); +int cmu_init(void); +void cmu_parse_message(void); +void cmu_set_param(void); +int cmu_ack(void); +void cmu_set_gm(void); +int cmu_ack_gm(); +void atoim(unsigned int val); +int cmu_gm(void); +void send_adc(); + + +struct Color{ + unsigned char red; + unsigned char green; + unsigned char blue; + unsigned char d_red; + unsigned char d_green; + unsigned char d_blue; +} ; + + +#endif -- 2.39.2