int main(void)
{
while(1)
{
UsartInit();
}
}
int main() {
UsartInit();
/* Se habilitan las interrupciones globales */
sei();
UsartPutString("Ejemplo simple de ECO con interrupciones en el ATmega16.\n");
UsartPutString("Presione cualquier tecla:\n");
while(1) {}
return 0;
}
/**
* @brief Program entry point.
* @param none
* @retval 0
*/
int main(void) {
RCC_ClocksTypeDef RCC_Clocks;
SystemInit();
// Get the clock rate so we can set the sampling rate correctly.
RCC_GetClocksFreq(&RCC_Clocks);
ClockRate = RCC_Clocks.HCLK_Frequency;
// NOTE: STM generic
// Our timer (TIM2) uses the APB1 (PCLK1) clock.
TimerBaseClockRate = RCC_Clocks.PCLK1_Frequency;
// Set the SysTick interrupt to fire once every millisecond.
SysTick_Config(RCC_Clocks.SYSCLK_Frequency / 1000);
LedInit();
// Turn the Orange LED on, signifying WAIT...
LedSet(LED_ORANGE, LED_MODE_ON);
UsartInit();
Copyright();
uint32_t commandTicks = Ticks;
LedSet(LED_ORANGE, LED_MODE_OFF);
// Loop forever...
while (1) {
if (SamplingActive) {
LedSet(LED_RED, LED_MODE_OFF);
// Turn the Blue LED on when sampling.
LedSet(LED_BLUE, LED_MODE_ON);
SampleLoop();
LedSet(LED_BLUE, LED_MODE_OFF);
}
// Check for commands every 1/10 second.
if ((Ticks - commandTicks) >= 100) {
ProcessCommands();
commandTicks = Ticks;
}
}
return 0;
}
int main(void)
{
int data;
int index=0;
char buffer[100];
Config32MHzClock();
CLK.PSCTRL = 0x00; // no division on peripheral clock
UsartInit();
UsartWriteString("\r\n\r\nBOOT: Starting...\r\n");
//Analog Mux
PORTF.DIR = 0b11111111; //Output
PORTF.OUTCLR = PIN6_bm;
PORTF.OUTCLR = PIN4_bm | PIN7_bm;
PORTF.OUTSET = PIN5_bm;
PORTF.OUTCLR = PIN0_bm | PIN1_bm | PIN2_bm | PIN3_bm;
//PORTF.OUTSET = PIN1_bm;
PORTF.OUTCLR = PIN6_bm;
PORTF.OUTSET = PIN4_bm;
PORTF.OUTCLR = PIN5_bm;
PORTF.OUTSET = PIN7_bm; //WR
UsartWriteString("BOOT: Analog Mux configured...\r\n");
//ADC
PORTQ.DIR = 0b1111; //Output
PORTQ.OUTCLR = PIN1_bm | PIN3_bm; //PWRDWN pins
PORTQ.OUTSET = PIN0_bm | PIN2_bm; //ENCODE pins
//Reading Values
PORTD.DIR = 0b00000000; //Input
PORTE.DIR = 0b00000000; //Input
UsartWriteString("BOOT: ADC configured...\r\n");
//TFT LCD
TFT_init();
Delay100ms(10);
Delay100ms(10);
Delay100ms(10);
Delay100ms(10);
Delay100ms(10);
TSLCDFillRect(0,TS_SIZE_X-1,0,TS_SIZE_Y-1,TS_COL_BLUE,TS_MODE_NORMAL);
/*
TSLCDFillRect(0,TS_SIZE_X-1,0,70,TS_COL_WHITE,TS_MODE_NORMAL);
TSLCDSetFontColor(TS_COL_BLUE);
TSLCDPrintStr(2,6,"Testing ELT240320TP with AVR",TS_MODE_NORMAL);
TSLCDFillRect(20,80,90,130,TS_COL_BLACK,TS_MODE_NORMAL);
TSLCDFillRect(30,90,100,140,TS_COL_YELLOW,TS_MODE_NORMAL);
TSLCDFillRect(20,80,160,200,TS_COL_BLACK,TS_MODE_NORMAL);
TSLCDFillRect(30,90,170,210,TS_COL_RED,TS_MODE_NORMAL);
TSLCDFillRect(195,205,71,TS_SIZE_Y-1,TS_COL_WHITE,TS_MODE_NORMAL);
TSLCDFillCirc(200,155,60,TS_COL_WHITE,TS_MODE_NORMAL);
TSLCDFillCirc(200,155,50,TS_COL_BLUE,TS_MODE_NORMAL);
TSLCDFillCirc(200,155,40,TS_COL_BLACK,TS_MODE_NORMAL);
TSLCDFillCirc(200,155,30,TS_COL_RED,TS_MODE_NORMAL);
*/
UsartWriteString("BOOT: TFT LCD configured...\r\n");
//Timing Lines
//INT0: breakoutbar6 / A0 / frame
//INT1: slicebin0 / A1 / pixel
PORTA.INTCTRL = (PORTA.INTCTRL & ~PORT_INT0LVL_gm ) | PORT_INT0LVL_LO_gc;
PORTA.INTCTRL = (PORTA.INTCTRL & ~PORT_INT1LVL_gm ) | PORT_INT1LVL_LO_gc;
//Detect rising edge only
PORTA.PIN0CTRL |= 0x01;
PORTA.PIN0CTRL &= ~0x06;
PORTA.PIN1CTRL |= 0x01;
PORTA.PIN1CTRL &= ~0x06;
PORTA.INT0MASK = PIN0_bm;
PORTA.INT1MASK = PIN1_bm;
UsartWriteString("BOOT: Interrupts configured...\r\n");
//EBI/SDRAM
ebi_setup_port(12, 0, 0, EBI_PORT_3PORT | EBI_PORT_SDRAM);
/*
* Configure the EBI chip select for an 8 MB SDRAM located at
* \ref BOARD_EBI_SDRAM_BASE.
*/
ebi_cs_set_mode(&cs_config, EBI_CS_MODE_SDRAM_gc);
ebi_cs_set_address_size(&cs_config, EBI_CS_ASPACE_8MB_gc);
ebi_cs_set_base_address(&cs_config, BOARD_EBI_SDRAM_BASE);
/* Configure the EBI chip select to be in SDRAM mode. */
ebi_sdram_set_mode(&cs_config, EBI_CS_SDMODE_NORMAL_gc);
/* Setup the number of SDRAM rows and columns. */
ebi_sdram_set_row_bits(&sdram_config, 12);
ebi_sdram_set_col_bits(&sdram_config, 10);
/* Further, setup the SDRAM timing. */
ebi_sdram_set_cas_latency(&sdram_config, 3);
ebi_sdram_set_mode_delay(&sdram_config, EBI_MRDLY_2CLK_gc);
ebi_sdram_set_row_cycle_delay(&sdram_config, EBI_ROWCYCDLY_7CLK_gc);
ebi_sdram_set_row_to_precharge_delay(&sdram_config, EBI_RPDLY_7CLK_gc);
ebi_sdram_set_write_recovery_delay(&sdram_config, EBI_WRDLY_1CLK_gc);
ebi_sdram_set_self_refresh_to_active_delay(&sdram_config,
EBI_ESRDLY_7CLK_gc);
ebi_sdram_set_row_to_col_delay(&sdram_config, EBI_ROWCOLDLY_7CLK_gc);
ebi_sdram_set_refresh_period(&sdram_config, BOARD_EBI_SDRAM_REFRESH);
ebi_sdram_set_initialization_delay(&sdram_config,
BOARD_EBI_SDRAM_INITDLY);
/* Write SDRAM configuration into the EBI registers. */
ebi_sdram_write_config(&sdram_config);
/* Write the chip select configuration into the EBI registers. */
ebi_cs_write_config(EBI_SDRAM_CS, &cs_config);
ebi_enable_cs(EBI_SDRAM_CS, &cs_config);
UsartWriteString("BOOT: SDRAM configured...\r\n");
do {
// Wait for SDRAM to initialize.
} while (!ebi_sdram_is_ready());
UsartWriteString("BOOT: SDRAM ready...\r\n");
status_code_t retval = ebi_test_data_bus((hugemem_ptr_t)BOARD_EBI_SDRAM_BASE);
if (retval == STATUS_OK) {
UsartWriteString("BOOT: SDRAM data bus test completed...\r\n");
} else {
UsartWriteString("BOOT: WARNING: SDRAM data bus test failed...\r\n");
}
retval = ebi_test_addr_bus((hugemem_ptr_t)BOARD_EBI_SDRAM_BASE,BOARD_EBI_SDRAM_SIZE);
if (retval == STATUS_OK) {
UsartWriteString("BOOT: SDRAM address bus test completed...\r\n");
} else {
UsartWriteString("BOOT: WARNING: SDRAM address bus test failed...\r\n");
}
//_delay_ms(1);
//SD Card testing
UsartWriteString("BOOT: SD Card testing...\r\n");
//_delay_ms(1);
FRESULT f_err_code;
static FATFS FATFS_Obj;
UsartWriteString("BOOT: SD Card: initializing disk...\r\n");
disk_initialize(0);
UsartWriteString("BOOT: SD Card: mounting disk...\r\n");
f_err_code = f_mount(0, &FATFS_Obj);
if (f_err_code == FR_OK) {
FIL fil_obj;
int test_number = 5;
UsartWriteString("BOOT: SD Card: opening file...\r\n");
f_open(&fil_obj, "asplfc.txt", FA_WRITE);
UsartWriteString("BOOT: SD Card: writing file...\r\n");
int out = f_printf(&fil_obj, "moo %d", test_number);
UsartWriteString("BOOT: SD Card: closing file...\r\n");
f_close(&fil_obj);
} else {
UsartWriteLine("BOOT: WARNING: No SD card found...");
}
//PORTQ.OUTCLR = PIN2_bm | PIN4_bm;
UsartWriteString("\n\rASP LFC firmware -- v0.1\r\n");
UsartWriteString("> ");
while(1)
{
data=UsartReadChar(); // read char
// check for carriage return and try to match/execute command
if((data == '\r')||(index==sizeof(buffer)))
{
//PORTF.OUT ^= (1<<0); // switch LED
buffer[index]=0; // null terminate
index=0; // reset buffer index
UsartWriteString("\n\r"); // echo newline
ParseCommand(buffer); // attempt to parse command
UsartWriteString("> ");
}
else if(data==8) // backspace character
{
if(index>0)
index--; // backup one character
UsartWriteChar(data);
}
else
{
buffer[index++]=data;
UsartWriteChar(data);
};
// UsartWriteChar(data); // write char
// _delay_ms(100);
// PORTF.OUT ^= (1<<0); // toggle LED
};
};
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration----------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_I2C1_Init();
MX_USART1_UART_Init();
MX_TIM16_Init();
MX_TIM17_Init();
/* USER CODE BEGIN 2 */
g_config = g_config_default;
#ifdef USE_I2C
g_i2c = I2cMaster_Init(&hi2c1);
InitializeDisplay(g_i2c);
I2cEEPROM_Init(&g_eeprom, g_i2c, EEPROMADDR, 1, 8);
#endif
#ifdef USE_SERIAL
UsartInit(&huart1);
#endif
#if defined(USE_I2C) && defined(USE_LCD)
I2cLcd_Clear(&g_lcd);
I2cLcd_PrintStr(&g_lcd, "Hello");
#endif
#if defined(USE_SERIAL) && defined(USE_EEPROM)
#define TESTSIZE 2048/8
HAL_StatusTypeDef st;
uint8_t i2cBuffer[16] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
/*
for(uint16_t i = 0; i < TESTSIZE; i += sizeof(i2cBuffer)) {
if(!(i & (sizeof(i2cBuffer)-1))) {
st = I2cEEPROM_Write(&g_eeprom, i, i2cBuffer, sizeof(i2cBuffer));
}
}
for (uint16_t i = 0; i < TESTSIZE; ++i) {
if(!(i & (sizeof(i2cBuffer)-1))) {
st = I2cEEPROM_Read(&g_eeprom, i, i2cBuffer, sizeof(i2cBuffer));
UsartSendStr("\r\n", 1);
UsartPrintUint(i, 4, 1);
UsartSendStr(" ", 1);
}
UsartPrintByte(i2cBuffer[i&(sizeof(i2cBuffer)-1)], 2, 1);
UsartSendStr(" ", 1);
}
UsartSendStr("\r\n", 1);
for(uint16_t i = 0; i < TESTSIZE; i += sizeof(i2cBuffer)) {
if(!(i & (sizeof(i2cBuffer)-1))) {
for(uint16_t j = 0; j<sizeof(i2cBuffer); ++j)
i2cBuffer[j] = i+j;
st = I2cEEPROM_Write(&g_eeprom, i, i2cBuffer, sizeof(i2cBuffer));
}
}
for (uint16_t i = 0; i < TESTSIZE; ++i) {
if(!(i & (sizeof(i2cBuffer)-1))) {
st = I2cEEPROM_Read(&g_eeprom, i, i2cBuffer, sizeof(i2cBuffer));
UsartSendStr("\r\n", 1);
UsartPrintUint(i, 4, 1);
UsartSendStr(" ", 1);
}
UsartPrintByte(i2cBuffer[i&(sizeof(i2cBuffer)-1)], 2, 1);
UsartSendStr(" ", 1);
}
UsartSendStr("\r\n", 1);
*/
{
LIVECONFIG config;
if (I2cEEPROM_Read(&g_eeprom, EESTART, &config, sizeof(config)) == HAL_OK) {
I2cMaster_WaitCallback(g_i2c);
if (config.magic == 0xA5)
g_config = config;
}
}
for (uint16_t i = 0; i < TESTSIZE; ++i) {
if(!(i & (sizeof(i2cBuffer)-1))) {
st = I2cEEPROM_Read(&g_eeprom, i, i2cBuffer, sizeof(i2cBuffer));
UsartSendStr("\r\n", 1);
UsartPrintUint(i, 4, 1);
UsartSendStr(" ", 1);
}
UsartPrintByte(i2cBuffer[i&(sizeof(i2cBuffer)-1)], 2, 1);
UsartSendStr(" ", 1);
}
UsartSendStr("\r\n", 1);
#endif
HAL_TIM_IC_Start_IT(&htim16, TIM_CHANNEL_1);
HAL_TIM_IC_Start_IT(&htim17, TIM_CHANNEL_1);
HAL_UART_Receive_IT(&huart1, g_lineBuffer, sizeof(g_lineBuffer));
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1) {
if (g_lineReceived) {
ProcessInput(&g_config, (char*) g_lineBuffer);
//DisplayInput(&i2clcd);
g_lineReceived = 0;
HAL_UART_Receive_IT(&huart1, g_lineBuffer, sizeof(g_lineBuffer));
}
if(g_statuses[0].trigger) {
DisplayResults(0);
g_statuses[0].trigger = 0;
}
if(g_statuses[1].trigger) {
DisplayResults(1);
g_statuses[1].trigger = 0;
}
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
int main( void )
{
//konfiguracija taktova
RCC_ADCCLKConfig(RCC_PCLK2_Div2);//konfigurisanje takta za ADC
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);//dovodjenje takta za DMA kontroler
RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_TIM1 | RCC_APB2Periph_ADC1, ENABLE);//dovodjenje takta portu A, B, C, tajmeru TIM1 i ADC-u
//konfiguracija portova - analogni ulazi
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_3;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOC, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
GPIO_Init(GPIOA, &GPIO_InitStructure);
//konfiguracija portova - bargraph tj. DIGIO konektor
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2 | GPIO_Pin_3 | GPIO_Pin_5 | GPIO_Pin_6;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOA, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOB, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_4 | GPIO_Pin_5 | GPIO_Pin_13;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
GPIO_Init(GPIOC, &GPIO_InitStructure);
/* DMA1 channel1 configuration ----------------------------------------------*/
DMA_DeInit(DMA1_Channel1);
DMA_InitStructure.DMA_PeripheralBaseAddr = ADC1_DR_Address;//adresa izvorista za dma prenos - DATA REGISTER ADC-a
DMA_InitStructure.DMA_MemoryBaseAddr = (u32)ADC_RegularConvertedValueTab;
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC;
DMA_InitStructure.DMA_BufferSize = 4;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
DMA_InitStructure.DMA_Priority = DMA_Priority_High;
DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel1, &DMA_InitStructure);
/* Enable DMA1 channel1 */
DMA_Cmd(DMA1_Channel1, ENABLE);
/* ADC1 configuration ------------------------------------------------------*/
ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
ADC_InitStructure.ADC_ScanConvMode = ENABLE;
ADC_InitStructure.ADC_ContinuousConvMode = DISABLE;
ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_T1_CC1;
ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfChannel = 4;
ADC_Init(ADC1, &ADC_InitStructure);
ADC_RegularChannelConfig(ADC1, ADC_Channel_12, 1, ADC_SampleTime_1Cycles5);
ADC_RegularChannelConfig(ADC1, ADC_Channel_13, 2, ADC_SampleTime_1Cycles5);
ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 3, ADC_SampleTime_1Cycles5);
ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 4, ADC_SampleTime_1Cycles5);
ADC_ExternalTrigConvCmd(ADC1, ENABLE);//omogucavanje externog triger moda
ADC_DMACmd(ADC1, ENABLE);//omogucavanje DMA prenosa za ADC
ADC_Cmd(ADC1, ENABLE);
ADC_ResetCalibration(ADC1);//adc kalibracija
while(ADC_GetResetCalibrationStatus(ADC1));
ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1));
//konfiguracija tajmera TIM1 koji radi u PWM modu, i svoj izlaz koristi za trigerovanje ADC-a
TIM_TimeBaseStructInit(&TIM_TimeBaseInitStruct);
TIM_TimeBaseInitStruct.TIM_Period = 150 - 1;
TIM_TimeBaseInitStruct.TIM_Prescaler = 0;
TIM_TimeBaseInitStruct.TIM_ClockDivision = 0x0;
TIM_TimeBaseInitStruct.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseInitStruct);
TIM_OCInitStruct.TIM_OCMode = TIM_OCMode_PWM1;
TIM_OCInitStruct.TIM_OutputState = TIM_OutputState_Enable;
TIM_OCInitStruct.TIM_Pulse = 150 / 2;
TIM_OCInitStruct.TIM_OCPolarity = TIM_OCPolarity_Low;
TIM_OC1Init(TIM1, &TIM_OCInitStruct);
TIM_Cmd(TIM1, ENABLE);//dozovla rada tajmera tek kada se konfigurisu i DMA i ADC
TIM_CtrlPWMOutputs(TIM1, ENABLE);//generisanje PWM izlaza za tajmer 1
baterija_Acc_const=0.004032;
servo_5V_const=0.004032;
/* Inicijalizacija. */
InitGPIO_Pin(GPIOB, GPIO_Pin_11, GPIO_Mode_IPU, GPIO_Speed_50MHz);
UsartInit();
/* Inicijalizacija glavnog tajmera. */
initTimerServo();//zbog ovoga se baterija meri i dok je prekidac uvucen
/* Glavna masina stanja. */
while(1)
{
switch (state_robot)
{
/* Pocetno stanje u kome se inicijalizaciju sistemi, podesava preskaler, ukljucuje UV. */
case 0: // pocetno stanje, sve inicijalizujemo i krenemo napred
if (GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_11)) // ako je ocitano Vcc, tj. krene se sa izvrsavanjem, u suprotnom ostajemo u
// istom stanju
{
/* Inicijalizacija glavnog tajmera. */
initTimer90();
/* Inicijalizacija tajmera za proveru pozicije robota. */
SysTick_Config( SysTick_Config( SystemCoreClock / 1000 ) );
/* Provera koja je stategije. */
checkStrategy();
/* Zadavanje komandi. */
issueCommand( START_RUNNING, MOTION_DEVICE_ADDRESS, 30 );
waitAck( START_RUNNING, MOTION_DEVICE_ADDRESS, 30 );
issueCommand( PRESCALER, MOTION_DEVICE_ADDRESS, 1000 );
waitAck( PRESCALER, MOTION_DEVICE_ADDRESS, 1000 );
issueCommand( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
waitAck( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 30 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 30 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 25 );
if ( FLAG_arriveOnDest ) break;
sleep( 100 );
}
state_robot++;
sleep(100);
}
break;
/* Blago okretanje da bi se izbegla ivica na sredini terena. */
case 1:
if( FLAG_strategyLeft )
{
issueCommand( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 20 );
waitAck( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 20 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 25 );
if ( FLAG_arriveOnDest ) break;
sleep( 100 );
}
}
else
{
issueCommand( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 20 );
waitAck( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 20 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 25 );
if ( FLAG_arriveOnDest ) break;
sleep( 100 );
}
}
state_robot++;
sleep(100);
break;
/* Blago pomeranje napred, ka sredini terena. */
case 2:
{
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 50 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 50 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
}
/* Blaga rotacija da bi se poravnali opet. */
case 3:
if( FLAG_strategyLeft )
{
issueCommand( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 25 );
waitAck( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 25 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
else
{
issueCommand( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 25 );
waitAck( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 25 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
state_robot++;
sleep(100);
break;
/* Blago pomeranje napred da bi pomerili kocke u sredinu terena. */
case 4:
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 10 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 10 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Vracanje unazad. */
case 5:
issueCommand( PRESCALER, MOTION_DEVICE_ADDRESS, 500 );
waitAck( PRESCALER, MOTION_DEVICE_ADDRESS, 500 );
issueCommand( MOVE_BACKWARD, MOTION_DEVICE_ADDRESS, 50 );
waitAck( MOVE_BACKWARD, MOTION_DEVICE_ADDRESS, 50 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Okretanje ka prvoj kucici, onoj daljoj od ivice terana i gasenje senzora. */
case 6:
if( FLAG_strategyLeft )
{
issueCommand( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 175 );
waitAck( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 175 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
else
{
issueCommand( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 175 );
waitAck( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 175 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
issueCommand( ULTRASOUND_OFF, MOTION_DEVICE_ADDRESS, 1 );
waitAck( ULTRASOUND_OFF, MOTION_DEVICE_ADDRESS, 1 );
state_robot++;
sleep(100);
break;
/* Zatvaranje prvih vrata. */
case 7:
issueCommand( PRESCALER, MOTION_DEVICE_ADDRESS, 700 );
waitAck( PRESCALER, MOTION_DEVICE_ADDRESS, 700 );
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 100 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 100 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Vracanje unazad. */
case 8:
issueCommand( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
waitAck( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
issueCommand( PRESCALER, MOTION_DEVICE_ADDRESS, 500 );
waitAck( PRESCALER, MOTION_DEVICE_ADDRESS, 500 );
issueCommand( MOVE_BACKWARD, MOTION_DEVICE_ADDRESS, 60 );
waitAck( MOVE_BACKWARD, MOTION_DEVICE_ADDRESS, 60 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Okretanje za 180 stepeni ka pocetnoj poziciji. */
case 9:
//issueCommand( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
//waitAck( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
if( FLAG_strategyLeft )
{
issueCommand( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 180 );
waitAck( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 180 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
else
{
issueCommand( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 180 );
waitAck( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 180 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
state_robot++;
sleep(100);
break;
/* Odlazak naspram druge kucice. */
case 10:
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 15 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 15 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Okretanje ka kucici. */
case 11:
if( FLAG_strategyLeft )
{
issueCommand( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 175 );
waitAck( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 175 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
else
{
issueCommand( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 175 );
waitAck( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 175 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
issueCommand( ULTRASOUND_OFF, MOTION_DEVICE_ADDRESS, 1 );
waitAck( ULTRASOUND_OFF, MOTION_DEVICE_ADDRESS, 1 );
state_robot++;
sleep(100);
break;
/* Zatvaranje druge kucice. */
case 12:
issueCommand( ULTRASOUND_OFF, MOTION_DEVICE_ADDRESS, 1 );
waitAck( ULTRASOUND_OFF, MOTION_DEVICE_ADDRESS, 1 );
issueCommand( PRESCALER, MOTION_DEVICE_ADDRESS, 700 );
waitAck( PRESCALER, MOTION_DEVICE_ADDRESS, 700 );
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 60 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 60 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Vracanje unazad. */
case 13:
issueCommand( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
waitAck( ULTRASOUND_ON, MOTION_DEVICE_ADDRESS, 1 );
issueCommand( PRESCALER, MOTION_DEVICE_ADDRESS, 500 );
waitAck( PRESCALER, MOTION_DEVICE_ADDRESS, 500 );
issueCommand( MOVE_BACKWARD, MOTION_DEVICE_ADDRESS, 60 );
waitAck( MOVE_BACKWARD, MOTION_DEVICE_ADDRESS, 60 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Okretanje ka centru naseg dela terena. */
case 14:
if( FLAG_strategyLeft )
{
issueCommand( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 270 );
waitAck( ROTATE_LEFT, MOTION_DEVICE_ADDRESS, 270 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
else
{
issueCommand( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 270 );
waitAck( ROTATE_RIGHT, MOTION_DEVICE_ADDRESS, 270 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
}
state_robot++;
sleep(100);
break;
case 15:
issueCommand( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 40 );
waitAck( MOVE_FORWARD, MOTION_DEVICE_ADDRESS, 40 );
while( TRUE )
{
issueCommand( CHECK_ARRIVE, MOTION_DEVICE_ADDRESS, 1 );
sleep( 100 );
if ( FLAG_arriveOnDest ) break;
}
state_robot++;
sleep(100);
break;
/* Podrazumevano stanje u kome se ne radi nista. */
default:
break;
}
}
}
