int main(void)
{
// Global init
sei();
uart_init();
fdevopen(uart1_dev_send, uart1_dev_recv);
// Error configuration
error_register_emerg(log_event);
error_register_error(log_event);
error_register_warning(log_event);
error_register_notice(log_event);
error_register_debug(log_event);
log_level = ERROR_SEVERITY_DEBUG;
// Clear screen
printf("%c[2J",0x1B);
printf("%c[0;0H",0x1B);
// Test
fpga_init();
wait_ms(100);
_SFR_MEM8(0x1800) = 1;
wait_ms(100);
_SFR_MEM8(0x1800) = 0;
NOTICE(0, "ADNS9500 init");
adns9500_init();
NOTICE(0, "ADNS9500 boot");
adns9500_boot();
NOTICE(0,"ADNS9500 > AUTO");
adns9500_set_mode(ADNS9500_BHVR_MODE_AUTOMATIC);
adns9500_encoders_t e;
while(1)
{
adns9500_encoders_get_value(&e);
printf("%ld %ld %ld %ld %ld %ld | %2.2X %2.2X %2.2X | %2.2X\n",
e.vectors[0], e.vectors[1], e.vectors[2],
e.vectors[3], e.vectors[4], e.vectors[5],
e.squals[0], e.squals[1], e.squals[2],
e.fault);
wait_ms(100);
}
NOTICE(0, "DONE");
while(1) nop();
return 0;
}
int main(void)
{
//wait_ms(3000); /// XXX hack to give time to the person that tests the system to take a cofee
uart_init();
uart_com_init();
fdevopen(uart0_dev_send, uart0_dev_recv);
//--------------------------------------------------------
// Error configuration
error_register_emerg(log_event);
error_register_error(log_event);
error_register_warning(log_event);
error_register_notice(log_event);
error_register_debug(log_event);
log_level = ERROR_SEVERITY_NOTICE;
log_level = ERROR_SEVERITY_DEBUG;
sei();
printf("%c[2J",0x1B);
printf("%c[0;0H",0x1B);
printf("Robotter 2011 - Galipeur - R3D2-2K10");
printf("Compiled "__DATE__" at "__TIME__".");
//NOTICE(0,"Initializing r3d2");
r3d2_init();
//NOTICE(0,"Initializing leds");
init_led();
//NOTICE(0,"Initializing scheduler");
scheduler_init();
scheduler_add_periodical_event_priority(&r3d2_monitor, NULL,
300,
50);
scheduler_add_periodical_event_priority(&send_periodic_position_msg, NULL,
1000,
60);
PORTA = ~(0x55);
NOTICE(0,"Strike '?' for help");
while (1)
{
uart_com_monitor();
}
}
int main(void)
{
robot.verbosity_level = ERROR_SEVERITY_NOTICE;
/* Setup UART speed, must be first. */
cvra_set_uart_speed(COMPC_BASE, PIO_FREQ, 57600);
cvra_set_uart_speed(COMDEBUG_BASE, PIO_FREQ, 115200);
// cvra_set_uart_speed(COMBT2_BASE, 9600);
/* Inits the logging system. */
error_register_emerg(mylog);
error_register_error(mylog);
error_register_warning(mylog);
error_register_notice(mylog);
/* Enabling this one will cause a lot of logs from subsystems to show up. */
//error_register_debug(mylog);
OSTaskCreateExt(init_task,
NULL,
&init_task_stk[TASK_STACKSIZE-1],
INIT_TASK_PRIORITY,
INIT_TASK_PRIORITY,
&init_task_stk[0],
TASK_STACKSIZE,
NULL, NULL);
OSStart();
for (;;);
/* We will never reach it. */
return 0;
}
int main(void)
{
#ifndef HOST_VERSION
/* brake */
BRAKE_DDR();
BRAKE_OFF();
/* CPLD reset on PG3 */
DDRG |= 1<<3;
PORTG &= ~(1<<3); /* implicit */
/* LEDS */
DDRJ |= 0x0c;
DDRL = 0xc0;
LED1_OFF();
LED2_OFF();
LED3_OFF();
LED4_OFF();
#endif
memset(&gen, 0, sizeof(gen));
memset(&mainboard, 0, sizeof(mainboard));
mainboard.flags = DO_ENCODERS | DO_CS | DO_RS |
DO_POS | DO_POWER | DO_BD | DO_ERRBLOCKING;
ballboard.lcob = I2C_COB_NONE;
ballboard.rcob = I2C_COB_NONE;
/* UART */
uart_init();
uart_register_rx_event(CMDLINE_UART, emergency);
#ifndef HOST_VERSION
#if CMDLINE_UART == 3
fdevopen(uart3_dev_send, uart3_dev_recv);
#elif CMDLINE_UART == 1
fdevopen(uart1_dev_send, uart1_dev_recv);
#endif
/* check eeprom to avoid to run the bad program */
if (eeprom_read_byte(EEPROM_MAGIC_ADDRESS) !=
EEPROM_MAGIC_MAINBOARD) {
int c;
sei();
printf_P(PSTR("Bad eeprom value ('f' to force)\r\n"));
c = uart_recv(CMDLINE_UART);
if (c == 'f')
eeprom_write_byte(EEPROM_MAGIC_ADDRESS, EEPROM_MAGIC_MAINBOARD);
wait_ms(100);
bootloader();
}
#endif /* ! HOST_VERSION */
/* LOGS */
error_register_emerg(mylog);
error_register_error(mylog);
error_register_warning(mylog);
error_register_notice(mylog);
error_register_debug(mylog);
#ifndef HOST_VERSION
/* SPI + ENCODERS */
encoders_spi_init(); /* this will also init spi hardware */
/* I2C */
i2c_init(I2C_MODE_MASTER, I2C_MAINBOARD_ADDR);
i2c_protocol_init();
i2c_register_recv_event(i2c_recvevent);
i2c_register_send_event(i2c_sendevent);
/* TIMER */
timer_init();
timer0_register_OV_intr(main_timer_interrupt);
/* PWM */
PWM_NG_TIMER_16BITS_INIT(1, TIMER_16_MODE_PWM_10,
TIMER1_PRESCALER_DIV_1);
PWM_NG_TIMER_16BITS_INIT(4, TIMER_16_MODE_PWM_10,
TIMER4_PRESCALER_DIV_1);
PWM_NG_INIT16(&gen.pwm1_4A, 4, A, 10, PWM_NG_MODE_SIGNED,
&PORTD, 4);
PWM_NG_INIT16(&gen.pwm2_4B, 4, B, 10, PWM_NG_MODE_SIGNED |
PWM_NG_MODE_SIGN_INVERTED, &PORTD, 5);
PWM_NG_INIT16(&gen.pwm3_1A, 1, A, 10, PWM_NG_MODE_SIGNED,
&PORTD, 6);
PWM_NG_INIT16(&gen.pwm4_1B, 1, B, 10, PWM_NG_MODE_SIGNED,
&PORTD, 7);
/* servos */
PWM_NG_TIMER_16BITS_INIT(3, TIMER_16_MODE_PWM_10,
TIMER1_PRESCALER_DIV_256);
PWM_NG_INIT16(&gen.servo1, 3, C, 10, PWM_NG_MODE_NORMAL,
NULL, 0);
PWM_NG_TIMER_16BITS_INIT(5, TIMER_16_MODE_PWM_10,
TIMER1_PRESCALER_DIV_256);
PWM_NG_INIT16(&gen.servo2, 5, A, 10, PWM_NG_MODE_NORMAL,
NULL, 0);
PWM_NG_INIT16(&gen.servo3, 5, B, 10, PWM_NG_MODE_NORMAL,
NULL, 0);
PWM_NG_INIT16(&gen.servo4, 5, C, 10, PWM_NG_MODE_NORMAL,
NULL, 0);
support_balls_deploy(); /* init pwm for servos */
#endif /* !HOST_VERSION */
/* SCHEDULER */
scheduler_init();
#ifdef HOST_VERSION
hostsim_init();
robotsim_init();
#endif
#ifndef HOST_VERSION
scheduler_add_periodical_event_priority(do_led_blink, NULL,
100000L / SCHEDULER_UNIT,
LED_PRIO);
#endif /* !HOST_VERSION */
/* all cs management */
microb_cs_init();
/* TIME */
time_init(TIME_PRIO);
/* sensors, will also init hardware adc */
sensor_init();
#ifndef HOST_VERSION
/* start i2c slave polling */
scheduler_add_periodical_event_priority(i2c_poll_slaves, NULL,
8000L / SCHEDULER_UNIT, I2C_POLL_PRIO);
#endif /* !HOST_VERSION */
/* strat */
gen.logs[0] = E_USER_STRAT;
gen.log_level = 5;
/* strat-related event */
scheduler_add_periodical_event_priority(strat_event, NULL,
25000L / SCHEDULER_UNIT,
STRAT_PRIO);
#ifndef HOST_VERSION
/* eeprom time monitor */
scheduler_add_periodical_event_priority(do_time_monitor, NULL,
1000000L / SCHEDULER_UNIT,
EEPROM_TIME_PRIO);
#endif /* !HOST_VERSION */
sei();
strat_db_init();
printf_P(PSTR("\r\n"));
printf_P(PSTR("Respect et robustesse.\r\n"));
#ifndef HOST_VERSION
{
uint16_t seconds;
seconds = eeprom_read_word(EEPROM_TIME_ADDRESS);
printf_P(PSTR("Running since %d mn %d\r\n"), seconds/60, seconds%60);
}
#endif
#ifdef HOST_VERSION
strat_reset_pos(400, COLOR_Y(400), COLOR_A(-90));
#endif
cmdline_interact();
return 0;
}
int main(void)
{
// ADNS configuration
adns6010_configuration_t adns_config;
//--------------------------------------------------------------------------
// Booting
// Turn interruptions ON
sei();
// Initialize UART
uart_init();
fdevopen(uart1_dev_send, uart1_dev_recv);
//--------------------------------------------------------
// Error configuration
error_register_emerg(log_event);
error_register_error(log_event);
error_register_warning(log_event);
error_register_notice(log_event);
error_register_debug(log_event);
log_level = ERROR_SEVERITY_NOTICE;
//log_level = ERROR_SEVERITY_DEBUG;
// Clear screen
printf("%c[2J",0x1B);
printf("%c[0;0H",0x1B);
// Some advertisment :p
NOTICE(0,"Robotter 2009 - Galipeur - UNIOC-NG ADNS6010 CALIBRATION");
NOTICE(0,"Compiled "__DATE__" at "__TIME__".");
//--------------------------------------------------------
// Initialize scheduler
scheduler_init();
//--------------------------------------------------------
// Initialize time
time_init(160);
//--------------------------------------------------------
// Initialize FPGA
fpga_init();
// turn off led
_SFR_MEM8(0x1800) = 1;
//--------------------------------------------------------
// ADNS6010
//--------------------------------------------------------
NOTICE(0,"Initializing ADNS6010s");
adns6010_init();
#ifndef ADNS_OVERRIDE
NOTICE(0,"Checking ADNS6010s firmware");
adns6010_checkFirmware();
// ADNS CONFIGURATION
adns_config.res = ADNS6010_RES_2000;
adns_config.shutter = ADNS6010_SHUTTER_OFF;
adns_config.power = 0x11;
NOTICE(0,"Checking ADNS6010s SPI communication");
adns6010_checkSPI();
NOTICE(0,"Booting ADNS6010s");
adns6010_boot(&adns_config);
NOTICE(0,"Checking ADNS6010s");
adns6010_checks();
NOTICE(0,"ADNS6010s are GO");
#endif
//--------------------------------------------------------
NOTICE(0,"Initializing ADCs");
adc_init();
// For ploting purposes
NOTICE(0,"<PLOTMARK>");
// Set ADNS6010 system to automatic
adns6010_setMode(ADNS6010_BHVR_MODE_AUTOMATIC);
// Safe key event
scheduler_add_periodical_event_priority(&safe_key_pressed, NULL, 100, 50);
//----------------------------------------------------------------------
NOTICE(0,"Any key to go");
char cal_name = 'x';
uint8_t c;
while(1)
{
c = cli_getkey();
if(c != -1)
break;
}
//----------------------------------------------------------------------
//----------------------------------------------------------------------
int i,k;
NOTICE(0,"Go");
// Initialize control systems
cs_initialize();
cs_vx=0;
cs_vy=0;
cs_omega=0;
// remove break
motor_cs_break(0);
event_cs =
scheduler_add_periodical_event_priority(&cs_update, NULL, 25, 100);
NOTICE(0,"Press 'r' for auto line calibration / 'l' for ADNS line calibration / 'm' for motor encoders line calibration / 'a' for angle calibration / 'n' non-auto angle calibration / 'k' motor non-auto angle calibration");
c = cli_getkey();
//-----------------------------------------------------------------------------
// MOTOR ENCODERS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='m')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
NOTICE(0,"ME ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
motor_encoders_get_value(&motor_zero);
wait_ms(200);
NOTICE(0,"ME zero values = (%ld,%ld,%ld)",
motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
cs_vx=0;
cs_vy=0;
cs_omega=0;
// perform calibration, positive direction
motor_line_calibration(1);
NOTICE(0,"ME ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
motor_encoders_get_value(&motor_zero);
wait_ms(200);
NOTICE(0,"ME zero values = (%ld,%ld,%ld)",
motor_zero.vectors[0],motor_zero.vectors[1],motor_zero.vectors[2]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
// perform calibration, negative direction
motor_line_calibration(-1);
// output calibration data
NOTICE(0,"CALIBRATION DONE, printint scilab matrix :");
printf("%c=[\n",cal_name);
for(i=0;i<12;i++)
for(k=0;k<3;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==2)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// ADNS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='r')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
for(i=0;i<20;i++)
{
setmotors_course(robot_angle, 1);
wait_ms(1000);
setmotors_course(robot_angle, -1);
wait_ms(1000);
adns6010_encoders_get_value(&adns_zero);
}
}
//-----------------------------------------------------------------------------
// ADNS LINE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='l')
{
NOTICE(0,"Direction : '1' <- 0 ; '2' <- 2Pi/3 ; '3' <- 4Pi/3");
c = cli_getkey();
switch(c)
{
case '1': cal_name = 'A'; robot_angle = 0; break;
case '2': cal_name = 'B'; robot_angle = -2*M_PI/3; break;
case '3': cal_name = 'C'; robot_angle = -4*M_PI/3; break;
default : cal_name = 'A'; robot_angle = 0; break;
}
NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
adns6010_encoders_get_value(&adns_zero);
wait_ms(200);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
cs_vx=0;
cs_vy=0;
cs_omega=0;
// perform calibration, positive direction
line_calibration(1);
NOTICE(0,"ADNS ZERO : Place robot in position zero then press a key");
setmotors_course(robot_angle, 0);
while(!cli_getkey());
adns6010_encoders_get_value(&adns_zero);
wait_ms(200);
NOTICE(0,"ADNS zero values = (%ld,%ld,%ld,%ld,%ld,%ld)",
adns_zero.vectors[0],adns_zero.vectors[1],adns_zero.vectors[2],
adns_zero.vectors[3],adns_zero.vectors[4],adns_zero.vectors[5]);
NOTICE(0,"P(init), press a key to continue");
while(!cli_getkey());
// perform calibration, negative direction
line_calibration(-1);
// output calibration data
NOTICE(0,"CALIBRATION DONE, printint scilab matrix :");
printf("%c=[\n",cal_name);
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='a' )
{
NOTICE(0,"Angle calibration is fully automatic, robot will do approx. 3 turns");
NOTICE(0,"Press a key to start, calibration will start ~5s after.");
while(!cli_getkey());
NOTICE(0,"Starting in 5s...");
wait_ms(5000);
angle_calibration(1);
angle_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
int i,k;
for(i=0;i<12;i++)
{
printf("u_r%d = [0 0 %7.1f];\n",i,calibration_data_compass[i]);
}
printf("// ");
for(i=0;i<12;i++)
if(i==11)
printf("u_r11];\n");
else
printf("u_r%d;",i);
}
//-----------------------------------------------------------------------------
// MOTOR NON-AUTO ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='k' )
{
NOTICE(0,"key to go");
while(!cli_getkey());
motor_angle_na_calibration(1);
motor_angle_na_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<3;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==2)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
//-----------------------------------------------------------------------------
// NON-AUTO ANGLE CALIBRATION
//-----------------------------------------------------------------------------
if( c=='n' )
{
NOTICE(0,"key to go");
while(!cli_getkey());
angle_na_calibration(1);
angle_na_calibration(-1);
NOTICE(0,"Angle calibration done, press a key for results matrix : ");
while(!cli_getkey());
printf("R=[\n");
for(i=0;i<12;i++)
for(k=0;k<6;k++)
{
printf("%7.1f",calibration_data[i][k]);
if(k==5)
printf(";\n");
else
printf(" ");
}
printf("];\n\n");
}
EMERG(0,"Program ended");
while(1);
return 0;
}