//*****************************************************************************
//
//! Control robot to go straight forward by following wall
//!
//!
//! \return true if left/right wall is detected
//! false if no left/right wall is detected
//
static bool Forward()
{
LED1_OFF();LED2_ON();LED3_OFF();
//bluetooth_print("%d\r\n",avrSpeed);
if ((!isWallLeft) || (!isWallRight))
{
forwardUpdateByWall();
return true;
}
forwardUpdate();
if (avrSpeed<AVG_SPEED_FWD_FAST-20)
avrSpeed+=20;
else if (avrSpeed>AVG_SPEED_FWD_FAST)
avrSpeed=AVG_SPEED_FWD_FAST;
if (isWallRight)
{
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_RIGHT);
}
else if (isWallLeft)
{
pid_wallfollow(leftError,rightError, avrSpeed,WALL_FOLLOW_LEFT);
}
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
return false;
}
void LED_SLOW(void) //LED慢闪
{
P0DIR &= ~0X80; //set p0.7 output
LED3_OFF();
P0DIR |= 0X80; //关闭GPIO
}
void LED_DOWN(void) //LED灭
{
P0DIR &= ~0X80; //set p0.7 output
LED3_OFF();
P0DIR |= 0X80; //关闭GPIO
}
void ButtonRightHandler(void)
{
if (system_GetState() == SYSTEM_CALIB_SENSOR)
{
switch(IR_Calib_Step)
{
case 0:
LED1_ON();
LED2_OFF();
LED3_OFF();
IR_set_calib_value(IR_CALIB_BASE_LEFT);
IR_set_calib_value(IR_CALIB_BASE_RIGHT);
break;
case 1:
IR_set_calib_value(IR_CALIB_BASE_FRONT_LEFT);
IR_set_calib_value(IR_CALIB_BASE_FRONT_RIGHT);
LED1_OFF();
LED2_ON();
LED3_OFF();
break;
case 2:
IR_set_calib_value(IR_CALIB_MAX_LEFT);
LED1_ON();
LED2_ON();
LED3_OFF();
break;
case 3:
IR_set_calib_value(IR_CALIB_MAX_RIGHT);
LED1_OFF();
LED2_OFF();
LED3_ON();
break;
case 4:
IR_set_calib_value(IR_CALIB_MAX_FRONT_LEFT);
IR_set_calib_value(IR_CALIB_MAX_FRONT_RIGHT);
LED1_ON();
LED2_OFF();
LED3_ON();
break;
}
IR_Calib_Step++;
IR_Calib_Step %= 4;
}
}
void LED_Display_init(void)
{
ROM_SysCtlPeripheralEnable(LED1_PERIPHERAL);
ROM_SysCtlPeripheralEnable(LED2_PERIPHERAL);
ROM_SysCtlPeripheralEnable(LED3_PERIPHERAL);
ROM_GPIOPinTypeGPIOOutput(LED1_PORT, LED1_PIN);
ROM_GPIOPinTypeGPIOOutput(LED2_PORT, LED2_PIN);
ROM_GPIOPinTypeGPIOOutput(LED3_PORT, LED3_PIN);
LED1_OFF();
LED2_OFF();
LED3_OFF();
}
void system_Process_System_State(void)
{
switch (system_GetState())
{
case SYSTEM_POWER_UP:
break;
case SYSTEM_INITIALIZE:
break;
case SYSTEM_CALIB_SENSOR:
break;
case SYSTEM_SAVE_CALIB_SENSOR:
break;
case SYSTEM_ESTIMATE_MOTOR_MODEL:
ProcessSpeedControl();
break;
case SYSTEM_SAVE_MOTOR_MODEL:
break;
case SYSTEM_WAIT_TO_RUN:
break;
case SYSTEM_RUN_SOLVE_MAZE:
pid_Wallfollow_process();
ProcessSpeedControl();
break;
case SYSTEM_RUN_IMAGE_PROCESSING:
LED1_ON();
ProcessSpeedControl();
break;
case SYSTEM_ERROR:
speed_Enable_Hbridge(false);
system_Enable_BoostCircuit(false);
IntMasterDisable();
while (1)
{
LED1_ON();
LED2_ON();
LED3_ON();
ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
LED1_OFF();
LED2_OFF();
LED3_OFF();
ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
}
// break;
}
}
void system_Process_System_State(void)
{
switch (system_GetState())
{
case SYSTEM_POWER_UP:
break;
case SYSTEM_INITIALIZE:
break;
case SYSTEM_ESTIMATE_MOTOR_MODEL:
break;
case SYSTEM_SAVE_MOTOR_MODEL:
break;
case SYSTEM_WAIT_TO_RUN:
break;
case SYSTEM_RUN_BALANCE:
loop();
break;
case SYSTEM_RUN_IMAGE_PROCESSING:
LED1_ON();
break;
case SYSTEM_ERROR:
speed_Enable_Hbridge(false);
system_Enable_BoostCircuit(false);
IntMasterDisable();
while (1)
{
LED1_ON();
LED2_ON();
LED3_ON();
ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
LED1_OFF();
LED2_OFF();
LED3_OFF();
ROM_SysCtlDelay(ROM_SysCtlClockGet() / 3);
}
}
}
int board_early_init_f(void)
{
unsigned long sdrreg;
/* TBS: Setup the GPIO access for the user LEDs */
mfsdr(sdr_pfc0, sdrreg);
mtsdr(sdr_pfc0, (sdrreg & ~0x00000100) | 0x00000E00);
out32(CFG_GPIO_BASE + 0x018, (USR_LED0 | USR_LED1 | USR_LED2 | USR_LED3));
LED0_OFF();
LED1_OFF();
LED2_OFF();
LED3_OFF();
/*--------------------------------------------------------------------
* Setup the external bus controller/chip selects
*-------------------------------------------------------------------*/
/* set the bus controller */
mtebc (pb0ap, 0x04055200); /* FLASH/SRAM */
mtebc (pb0cr, 0xfff18000); /* BAS=0xfff 1MB R/W 8-bit */
mtebc (pb1ap, 0x04055200); /* FLASH/SRAM */
mtebc (pb1cr, 0xfe098000); /* BAS=0xff8 16MB R/W 8-bit */
/*--------------------------------------------------------------------
* Setup the interrupt controller polarities, triggers, etc.
*-------------------------------------------------------------------*/
mtdcr (uic0sr, 0xffffffff); /* clear all */
mtdcr (uic0er, 0x00000000); /* disable all */
mtdcr (uic0cr, 0x00000003); /* SMI & UIC1 crit are critical */
mtdcr (uic0pr, 0xfffffe00); /* per ref-board manual */
mtdcr (uic0tr, 0x01c00000); /* per ref-board manual */
mtdcr (uic0vr, 0x00000001); /* int31 highest, base=0x000 */
mtdcr (uic0sr, 0xffffffff); /* clear all */
mtdcr (uic1sr, 0xffffffff); /* clear all */
mtdcr (uic1er, 0x00000000); /* disable all */
mtdcr (uic1cr, 0x00000000); /* all non-critical */
mtdcr (uic1pr, 0xffffc0ff); /* per ref-board manual */
mtdcr (uic1tr, 0x00ff8000); /* per ref-board manual */
mtdcr (uic1vr, 0x00000001); /* int31 highest, base=0x000 */
mtdcr (uic1sr, 0xffffffff); /* clear all */
mtdcr (uic2sr, 0xffffffff); /* clear all */
mtdcr (uic2er, 0x00000000); /* disable all */
mtdcr (uic2cr, 0x00000000); /* all non-critical */
mtdcr (uic2pr, 0xffffffff); /* per ref-board manual */
mtdcr (uic2tr, 0x00ff8c0f); /* per ref-board manual */
mtdcr (uic2vr, 0x00000001); /* int31 highest, base=0x000 */
mtdcr (uic2sr, 0xffffffff); /* clear all */
mtdcr (uicb0sr, 0xfc000000); /* clear all */
mtdcr (uicb0er, 0x00000000); /* disable all */
mtdcr (uicb0cr, 0x00000000); /* all non-critical */
mtdcr (uicb0pr, 0xfc000000); /* */
mtdcr (uicb0tr, 0x00000000); /* */
mtdcr (uicb0vr, 0x00000001); /* */
LED0_ON();
return 0;
}
//*****************************************************************************
//
//! Control two motor to make robot turn left 90 degree
//!
//! \param fwdPulse is the distance robot will go straight before turn right
//!, the robot will stand between the next cell of maze.
//! \param avrSpeedLeft is the speed of left motor.
//! \param avrSpeedRight is the speed of right motor.
//! \param turnPulse is the total pulse of two encoder after turn
//! \param resetEnc is reset value for encoder after turning 90 degree, ignore this if you don't want to estimate position
//! \return true if finish
//! false if not
//
//*****************************************************************************
static bool TurnLeft(int fwdPulse,int avrSpeedLeft,int avrSpeedRight,int turnPulse,
int resetEnc)
{
static int vt,vp;
LED1_ON();LED2_OFF();LED3_OFF();
// bluetooth_print("LS %d\r\n",CtrlStep);
switch (CtrlStep)
{
case 1:
posLeftTmp=qei_getPosLeft();
CtrlStep++;
avrSpeedTmp=avrSpeed;
case 2://go straight
if ((abs(qei_getPosLeft()-posLeftTmp)<fwdPulse) ||
(isWallFrontLeft && isWallFrontRight &&
(IR_GetIrDetectorValue(3)>IR_get_calib_value(IR_CALIB_BASE_FRONT_RIGHT))&&
(IR_GetIrDetectorValue(0)>IR_get_calib_value(IR_CALIB_BASE_FRONT_LEFT))))
{
if (qei_getPosLeft()<fwdPulse+posLeftTmp)
avrSpeed = ((abs(fwdPulse + posLeftTmp - qei_getPosLeft()) / (fwdPulse / avrSpeedTmp)) / 2)
+ (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
else
avrSpeed = (abs(avrSpeedLeft) + abs(avrSpeedRight)) / 2;
if (isWallRight)
pid_wallfollow(leftError,rightError,avrSpeed,WALL_FOLLOW_RIGHT);
else
{
speed_set(MOTOR_RIGHT, avrSpeed);
speed_set(MOTOR_LEFT, avrSpeed);
}
}
else
{
#ifdef TEST_TURNLEFT_MOVE1
speed_Enable_Hbridge(false);
#endif
pid_reset(&pid_wall_right);
pid_reset(&pid_wall_left);
forwardUpdate();
CtrlStep++;
avrSpeed=avrSpeedTmp;
}
break;
case 3:
posLeftTmp=qei_getPosLeft();
posRightTmp=qei_getPosRight();
CtrlStep++;
vp=1;
vt=1;
case 4://turn 90 degree
if (abs(qei_getPosLeft()-posLeftTmp) + abs(qei_getPosRight()-posRightTmp) < turnPulse)
{
speed_set(MOTOR_RIGHT, avrSpeedRight);
speed_set(MOTOR_LEFT, -avrSpeedLeft);
if((abs(qei_getPosRight()-posRightTmp)>(turnPulse*0.8*vp/8)) && (vp<9))
{
if (avrSpeedRight>=24)
avrSpeedRight-=24;
vp++;
}
if((abs(qei_getPosLeft()-posLeftTmp)>(turnPulse*0.2*vt/8)) && (vt<9))
{
if (avrSpeedLeft>=4)
avrSpeedLeft-=4;
vt++;
}
}
else
{
#ifdef TEST_TURNLEFT_TURN
speed_Enable_Hbridge(false);
#endif
currentDir=(currentDir+3)%4;
clearPosition();
qei_setPosLeft(resetEnc);
qei_setPosRight(resetEnc);
forwardUpdate();
CtrlStep=1;
pid_reset(&pid_wall_left);
pid_reset(&pid_wall_right);
speed_set(MOTOR_LEFT, avrSpeed);
speed_set(MOTOR_RIGHT, avrSpeed);
return true;
}
break;
}
return false;
}
//------------------------------------------------------------------------------
/// Application entry point. Configures the DBGU, PIT, TC0, LEDs and buttons
/// and makes LED\#1 blink in its infinite loop, using the Wait function.
/// \return Unused (ANSI-C compatibility).
//------------------------------------------------------------------------------
int main(void)
{
// DBGU configuration
TRACE_CONFIGURE(DBGU_STANDARD, 115200, BOARD_MCK);
TRACE_INFO_WP("\n\r");
TRACE_INFO("Getting new Started Project --\n\r");
TRACE_INFO("%s\n\r", BOARD_NAME);
TRACE_INFO("Compiled: %s %s --\n\r", __DATE__, __TIME__);
//Configure Reset Controller
AT91C_BASE_RSTC->RSTC_RMR= 0xa5<<24;
// Configure EMAC PINS
PIO_Configure(emacRstPins, PIO_LISTSIZE(emacRstPins));
// Execute reset
RSTC_SetExtResetLength(0xd);
RSTC_ExtReset();
// Wait for end hardware reset
while (!RSTC_GetNrstLevel());
TRACE_INFO("init Flash\n\r");
flash_init();
TRACE_INFO("init Timer\n\r");
// Configure timer 0
ticks=0;
extern void ISR_Timer0();
AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC0);
AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKDIS;
AT91C_BASE_TC0->TC_IDR = 0xFFFFFFFF;
AT91C_BASE_TC0->TC_SR;
AT91C_BASE_TC0->TC_CMR = AT91C_TC_CLKS_TIMER_DIV5_CLOCK | AT91C_TC_CPCTRG;
AT91C_BASE_TC0->TC_RC = 375;
AT91C_BASE_TC0->TC_IER = AT91C_TC_CPCS;
AIC_ConfigureIT(AT91C_ID_TC0, AT91C_AIC_PRIOR_LOWEST, ISR_Timer0);
AIC_EnableIT(AT91C_ID_TC0);
AT91C_BASE_TC0->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
// Configure timer 1
extern void ISR_Timer1();
AT91C_BASE_PMC->PMC_PCER = (1 << AT91C_ID_TC1);
AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKDIS; //Stop clock
AT91C_BASE_TC1->TC_IDR = 0xFFFFFFFF; //Disable Interrupts
AT91C_BASE_TC1->TC_SR; //Read Status register
AT91C_BASE_TC1->TC_CMR = AT91C_TC_CLKS_TIMER_DIV4_CLOCK | AT91C_TC_CPCTRG; // Timer1: 2,666us = 48MHz/128
AT91C_BASE_TC1->TC_RC = 0xffff;
AT91C_BASE_TC1->TC_IER = AT91C_TC_CPCS;
AIC_ConfigureIT(AT91C_ID_TC1, 1, ISR_Timer1);
AT91C_BASE_TC1->TC_CCR = AT91C_TC_CLKEN | AT91C_TC_SWTRG;
led_init();
TRACE_INFO("init EEprom\n\r");
eeprom_init();
rb_reset(&TTY_Rx_Buffer);
rb_reset(&TTY_Tx_Buffer);
input_handle_func = analyze_ttydata;
LED_OFF();
LED2_OFF();
LED3_OFF();
spi_init();
fht_init();
tx_init();
#ifdef HAS_ETHERNET
ethernet_init();
#endif
TRACE_INFO("init USB\n\r");
CDCDSerialDriver_Initialize();
USBD_Connect();
wdt_enable(WDTO_2S);
fastrf_on=0;
display_channel = DISPLAY_USB;
TRACE_INFO("init Complete\n\r");
checkFrequency();
// Main loop
while (1) {
CDC_Task();
Minute_Task();
RfAnalyze_Task();
#ifdef HAS_FASTRF
FastRF_Task();
#endif
#ifdef HAS_RF_ROUTER
rf_router_task();
#endif
#ifdef HAS_ASKSIN
rf_asksin_task();
#endif
#ifdef HAS_MORITZ
rf_moritz_task();
#endif
#ifdef HAS_RWE
rf_rwe_task();
#endif
#ifdef HAS_MBUS
rf_mbus_task();
#endif
#ifdef HAS_MAICO
rf_maico_task();
#endif
#ifdef HAS_ETHERNET
Ethernet_Task();
#endif
#ifdef DBGU_UNIT_IN
if(DBGU_IsRxReady()){
unsigned char volatile * const ram = (unsigned char *) AT91C_ISRAM;
unsigned char x;
x=DBGU_GetChar();
switch(x) {
case 'd':
puts("USB disconnect\n\r");
USBD_Disconnect();
break;
case 'c':
USBD_Connect();
puts("USB Connect\n\r");
break;
case 'r':
//Configure Reset Controller
AT91C_BASE_RSTC->RSTC_RMR=AT91C_RSTC_URSTEN | 0xa5<<24;
break;
case 'S':
USBD_Disconnect();
my_delay_ms(250);
my_delay_ms(250);
//Reset
*ram = 0xaa;
AT91C_BASE_RSTC->RSTC_RCR = AT91C_RSTC_PROCRST | AT91C_RSTC_PERRST | AT91C_RSTC_EXTRST | 0xA5<<24;
while (1);
break;
default:
rb_put(&TTY_Tx_Buffer, x);
}
}
#endif
if (USBD_GetState() == USBD_STATE_CONFIGURED) {
if( USBState == STATE_IDLE ) {
CDCDSerialDriver_Read(usbBuffer,
DATABUFFERSIZE,
(TransferCallback) UsbDataReceived,
0);
LED3_ON();
USBState=STATE_RX;
}
}
if( USBState == STATE_SUSPEND ) {
TRACE_INFO("suspend !\n\r");
USBState = STATE_IDLE;
}
if( USBState == STATE_RESUME ) {
TRACE_INFO("resume !\n\r");
USBState = STATE_IDLE;
}
}
}
//------------------------------------------------------------------------------
/// Invoked when the USB device gets suspended. By default, turns off all LEDs.
//------------------------------------------------------------------------------
void USBDCallbacks_Suspended(void)
{
LED3_OFF();
USBState = STATE_SUSPEND;
}
int board_early_init_f(void)
{
unsigned long sdrreg;
/*
* Enable GPIO for pins 18 - 24
* 18 = SEEPROM_WP
* 19 = #M_RST
* 20 = #MONARCH
* 21 = #LED_ALARM
* 22 = #LED_ACT
* 23 = #LED_STATUS1
* 24 = #LED_STATUS2
*/
mfsdr(SDR0_PFC0, sdrreg);
mtsdr(SDR0_PFC0, (sdrreg & ~SDR0_PFC0_TRE_ENABLE) | 0x00003e00);
out32(CONFIG_SYS_GPIO_BASE + 0x018, (USR_LED0 | USR_LED1 | USR_LED2 | USR_LED3));
LED0_OFF();
LED1_OFF();
LED2_OFF();
LED3_OFF();
/* Setup the external bus controller/chip selects */
mtebc(PB0AP, 0x04055200); /* 16MB Strata FLASH */
mtebc(PB0CR, 0xff098000); /* BAS=0xff0 16MB R/W 8-bit */
mtebc(PB1AP, 0x04055200); /* 512KB Socketed AMD FLASH */
mtebc(PB1CR, 0xfe018000); /* BAS=0xfe0 1MB R/W 8-bit */
mtebc(PB6AP, 0x05006400); /* 32-64MB AMD MirrorBit FLASH */
mtebc(PB6CR, 0xf00da000); /* BAS=0xf00 64MB R/W i6-bit */
mtebc(PB7AP, 0x05006400); /* 32-64MB AMD MirrorBit FLASH */
mtebc(PB7CR, 0xf40da000); /* BAS=0xf40 64MB R/W 16-bit */
/*
* Setup the interrupt controller polarities, triggers, etc.
*
* Because of the interrupt handling rework to handle 440GX interrupts
* with the common code, we needed to change names of the UIC registers.
* Here the new relationship:
*
* U-Boot name 440GX name
* -----------------------
* UIC0 UICB0
* UIC1 UIC0
* UIC2 UIC1
* UIC3 UIC2
*/
mtdcr(UIC1SR, 0xffffffff); /* clear all */
mtdcr(UIC1ER, 0x00000000); /* disable all */
mtdcr(UIC1CR, 0x00000003); /* SMI & UIC1 crit are critical */
mtdcr(UIC1PR, 0xfffffe00); /* per ref-board manual */
mtdcr(UIC1TR, 0x01c00000); /* per ref-board manual */
mtdcr(UIC1VR, 0x00000001); /* int31 highest, base=0x000 */
mtdcr(UIC1SR, 0xffffffff); /* clear all */
mtdcr(UIC2SR, 0xffffffff); /* clear all */
mtdcr(UIC2ER, 0x00000000); /* disable all */
mtdcr(UIC2CR, 0x00000000); /* all non-critical */
mtdcr(UIC2PR, 0xffffc0ff); /* per ref-board manual */
mtdcr(UIC2TR, 0x00ff8000); /* per ref-board manual */
mtdcr(UIC2VR, 0x00000001); /* int31 highest, base=0x000 */
mtdcr(UIC2SR, 0xffffffff); /* clear all */
mtdcr(UIC3SR, 0xffffffff); /* clear all */
mtdcr(UIC3ER, 0x00000000); /* disable all */
mtdcr(UIC3CR, 0x00000000); /* all non-critical */
mtdcr(UIC3PR, 0xffffffff); /* per ref-board manual */
mtdcr(UIC3TR, 0x00ff8c0f); /* per ref-board manual */
mtdcr(UIC3VR, 0x00000001); /* int31 highest, base=0x000 */
mtdcr(UIC3SR, 0xffffffff); /* clear all */
mtdcr(UIC0SR, 0xfc000000); /* clear all */
mtdcr(UIC0ER, 0x00000000); /* disable all */
mtdcr(UIC0CR, 0x00000000); /* all non-critical */
mtdcr(UIC0PR, 0xfc000000); /* */
mtdcr(UIC0TR, 0x00000000); /* */
mtdcr(UIC0VR, 0x00000001); /* */
LED0_ON();
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;
}
