#include <string.h>
#include <deb_led.h>
#include <can_ids.h>
+#include <can_msg_def.h>
#include <servo.h>
#include <powswitch.h>
#include <engine.h>
#include <adc.h>
#include <uart_nozen.h>
+#include "robobagr.h"
#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);
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();
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:
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:
--- /dev/null
+#include <lpc21xx.h> /* LPC21xx definitions */
+#include <errno.h>
+#include <periph/can.h>
+#include <string.h>
+#include <deb_led.h>
+#include <can_ids.h>
+#include <can_msg_def.h>
+#include <servo.h>
+#include <powswitch.h>
+#include <engine.h>
+#include <adc.h>
+#include <uart_nozen.h>
+#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;
+}
+
+
+
projects / eurobot / public.git / commitdiff