/**
* Function to process debugging module
* Checks for debbunging commands and things like this
*/
void debug_process(void){
#ifdef CFG_SUART_RX
// check for recieved commands
if( suart_rxFlag ){
uint8_t cmd = suart_getc();
#if !defined(CFG_CODE_LEVEL_AVG)
uint16_t tmp;
#endif
// process command
switch(cmd){
case 'f':
motor_set_speed(MOTOR_ADDR_L, 0x50, MOTOR_FORWARD);
motor_set_speed(MOTOR_ADDR_R, 0x40, MOTOR_FORWARD);
break;
case'b':
motor_set_speed(MOTOR_ADDR_L, 0x40, MOTOR_BACKWARD);
motor_set_speed(MOTOR_ADDR_R, 0x43, MOTOR_BACKWARD);
break;
case 'z':
motor_set_speed(MOTOR_ADDR_L, 0x00, MOTOR_BRAKE);
motor_set_speed(MOTOR_ADDR_R, 0x00, MOTOR_BRAKE);
break;
#if !defined(CFG_CODE_LEVEL_AVG)
case 'g':
led_set_allcolors();
led_all_on();
break;
case 'h':
led_set_nocolors();
led_all_off();
break;
#endif
case 's':
led_on(LED_STATUS);
break;
case 'r':
led_off(LED_STATUS);
break;
case 'c':
motor_set_speed(MOTOR_ADDR_L, MOTOR_SPEED_HALF, MOTOR_FORWARD);
motor_set_speed(MOTOR_ADDR_R, MOTOR_SPEED_HALF, MOTOR_BACKWARD);
break;
case 'a':
motor_set_speed(MOTOR_ADDR_L, MOTOR_SPEED_HALF, MOTOR_BACKWARD);
motor_set_speed(MOTOR_ADDR_R, MOTOR_SPEED_HALF, MOTOR_FORWARD);
break;
#if !defined(CFG_CODE_LEVEL_AVG)
case 'm':
debug_send_c( motor_get_speed(MOTOR_ADDR_L), TRUE );
break;
case 'n':
debug_send_c( motor_get_speed(MOTOR_ADDR_R), TRUE );
break;
case 'o':
debug_send_c( motor_get_direction(MOTOR_ADDR_L), TRUE );
break;
case 'p':
debug_send_c( motor_get_direction(MOTOR_ADDR_R), TRUE );
break;
case 'd':
debug_send_c( motor_get_fault(MOTOR_ADDR_L), TRUE );
break;
case 'e':
debug_send_c( motor_get_fault(MOTOR_ADDR_R), TRUE );
break;
case 'u':
motor_clear_fault( MOTOR_ADDR_L );
break;
case 'v':
motor_clear_fault( MOTOR_ADDR_R );
break;
#endif
case 'i':
debug_send_c( sys_robotID, TRUE );
break;
case 'j':
debug_send_msg( SYS_PUBLISHER, TRUE );
break;
case 'k':
debug_send_msg( SYS_VERSION, TRUE );
break;
case 'l':
debug_send_msg( SYS_NAME, TRUE );
break;
case 'M':
debug_send_c( motor_test(), TRUE );
break;
#if !defined(CFG_CODE_LEVEL_AVG)
case 'T':
tmp = monitor_read_temp(MONITOR_ADDR_1);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, FALSE );
tmp = monitor_read_temp(MONITOR_ADDR_2);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'U':
tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U1);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'V':
tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U4);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'W':
tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U2);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'X':
tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_U3);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'I':
tmp = monitor_read_current(MONITOR_ADDR_1, MONITOR_I1);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'J':
tmp = monitor_read_current(MONITOR_ADDR_1, MONITOR_I2);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'A':
tmp = monitor_read_voltage(MONITOR_ADDR_1, MONITOR_UVCC);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, FALSE );
tmp = monitor_read_voltage(MONITOR_ADDR_2, MONITOR_UVCC);
debug_send_c_wait( tmp>>8, FALSE );
debug_send_c_wait( tmp, TRUE );
break;
case 'R':
animation_set_mode(ANIMATION_MODE_RED_PULSED);
break;
case 'S':
animation_set_mode(ANIMATION_MODE_STROBE);
break;
case 'F':
animation_set_mode(ANIMATION_MODE_FADE);
break;
case 'L':
animation_set_mode(ANIMATION_MODE_LADY);
break;
case 'N':
animation_set_mode(ANIMATION_MODE_NONE);
break;
#endif
case '?':
debug_send_help();
break;
default:
debug_send_c( 0xff, TRUE );
break;
}
}
#endif /* CFG_SUART_RX */
}
int main(void)
{
const char* str;
/* initialize stuff commen for both base and node */
config_load();
lcd_init();
rtc_init();
adc_init();
uart_init();
//Timer2 används för att hålla våran radio-timeslot (kanske bara behövs när vi är nod?.
timer2_init();
/* set portC as output and all leds off */
DDRC = 0xFF;
PORTC = 0xff;
/* lets initialize modules specific for the mode */
if(config.flags.mode == CONFIG_MODE_BASE)
{
suart_init();
str = "\n00init\nSystem is now online!\n";
while(*str)
{
while(suart_putc(*str) == FALSE);
str++;
}
}
else
{
response_wait_time = atoi(config.group) * 4;
// response_wait_time = 16;
}
/* in our answer the two first byte is always the group number */
memcpy((void*)answer, (void*)config.group, CONFIG_GRP_LEN);
/* all is initialized, lets roll */
sei();
/* configure the mode button pin as input */
MODE_BUTTON_DDR &= ~_BV(MODE_BUTTON_PIN);
/* loop until the mode button pin is low */
while(bit_is_set(MODE_BUTTON_PORT, MODE_BUTTON_PIN))
{
uint8_t buffer[UART_FIFO_SIZE];
/* uart data (radio), parse it */
if(uart.stopchars)
{
uart.stopchars--;
/* copy it to our stack */
uint8_t i = 0;
while ((buffer[i] = uart_getc()) != PROTOCOL_STOPCHAR)
i++;
/*
* node -> parse it
* base -> pass along to the suart (to computer)
*/
if(config.flags.mode == CONFIG_MODE_BASE)
{
int j;
for(j=0;j<i;j++)
while(suart_putc(buffer[j]) == FALSE);
}
else {
if (cmd_parse(buffer, i)) {
// enable the send timer
TCNT2 = 0;
TIMSK |= _BV(OCIE2);
}
}
}
/*
* suart data (from computer)
* We only get this as base, so answer the command
* if it's addressed to us, and send to all nodes.
*/
if(suart.stopchars) {
suart.stopchars--;
uint8_t i = 0;
while ((buffer[i] = suart_getc()) != PROTOCOL_STOPCHAR)
i++;
if (cmd_parse(buffer, i)) {
i = 0;
if (command_parsed == 1) {
do {
suart_putc(answer[i]);
} while (answer[i++] != PROTOCOL_STOPCHAR);
command_parsed = 0;
}
}
//Send to radio
i = 0;
do {
uart_putc(buffer[i]);
} while (buffer[i++] != PROTOCOL_STOPCHAR);
}
}
/* we are closing down, do not disturb */
cli();
/* this is safe because we know that the mode is just one bit */
config.flags.mode = !config.flags.mode;
config_save();
rtc_save();
/* use the watchdog to get a nice clean reset */
wdt_enable(WDTO_15MS);
while(1);
}
