예제 #1
0
파일: main.c 프로젝트: teyssieuman/eurobot
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;
}
예제 #2
0
파일: main.c 프로젝트: teyssieuman/eurobot
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();
	}
}
예제 #3
0
파일: main.c 프로젝트: cvra/debra-old
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;
}
예제 #4
0
파일: main.c 프로젝트: onitake/aversive
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;
}
예제 #5
0
파일: main.c 프로젝트: teyssieuman/eurobot
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;
}