//==================================== SCREEN FUNCTION LIBRARY ========================
//-------------- define all function members ----------
uLCD_144::uLCD_144(int baud) {
Serial.begin(baud);
// display requires some time to init before sending auto-baud command
delay(1000);
Serial.write((byte)0x55);
isAlive_=waitAck();
}
void uLCD_144::setPenSize(int size) {
// 0 = solid, 1 = wire
Serial.write((byte)0x70);
Serial.write((byte)size);
waitAck();
}
void uLCD_144::setBgColor(byte red, byte green, byte blue) {
Serial.write((byte)0x42); // set screen color
Serial.write((byte)getMSB(red, green, blue));
Serial.write((byte)getLSB(red, green, blue));
waitAck();
}
void uLCD_144::setFont(int font) {
// 0 = small, 1 = med, 2 = large
Serial.write((byte)0x46);
Serial.write((byte)font);
waitAck();
}
void uLCD_144::setOpacity(int opacity) {
// 0 = transparent, 1 = opaque
Serial.write((byte)0x4f);
Serial.write((byte)opacity);
waitAck();
}
void uLCD_144::setContrast(int contrast) {
// range from 0 to 0x0f
Serial.write(0x59);
Serial.write(0x02);
Serial.write(contrast);
waitAck();
}
bool DDAProtocol :: exec()
{
int errorCount = ERROR_COUNT;
while(!m_terminated)
{
if(rxPacket())
{
errorCount = ERROR_COUNT;
unsigned txSize = m_txData.size();
if(!txSize)
{
//Send ACK
unsigned char tx = ACK;
serial_write(m_serial, &tx, 1);
}
else
{
txPacket();
waitAck();
}
handleData(m_rxData);
m_rxData.clear();
}
else
{
if(--errorCount <= 0)
return false;
}
}
return true;
}
void uLCD_144_SPE::setFont(int font) {
Serial.print((char)0xFF);
Serial.print((char)0x7D);
Serial.print((char)0x00);
Serial.print((char)0x00);
waitAck();
}
void uLCD_144_SPE::setFGcolor(byte red, byte green, byte blue) {
Serial.print((char)0xFF);
Serial.print((char)0x7F);
Serial.print((char)getMSB(red, green, blue));
Serial.print((char)getLSB(red, green, blue));
waitAck();
}
void uLCD_144_SPE::setYspacing(int pixels) {
Serial.print((char)0xFF);
Serial.print((char)0x79);
Serial.print((char)0x00);
Serial.print((char)pixels);
waitAck();
}
// draw pixel
void uLCD_144::drawPixel(int x, int y, byte red, byte green, byte blue) {
Serial.write(0x50);
Serial.write(x);
Serial.write(y);
Serial.write(getMSB(red, green, blue));
Serial.write(getLSB(red, green, blue));
waitAck();
}
// draw circle
void uLCD_144::drawCircle(int x, int y, int r, byte red, byte green, byte blue) {
Serial.write(0x43);
Serial.write(x);
Serial.write(y);
Serial.write(r);
Serial.write(getMSB(red, green, blue));
Serial.write(getLSB(red, green, blue));
waitAck();
}
void uLCD_144_SPE::setOpacity(int opacity) {
// 0 = transparent, 1 = opaque
Serial.print((char)0xFF);
Serial.print((char)0x77);
Serial.print((char)0x00);
Serial.print((char)opacity);
waitAck();
}
void uLCD_144_SPE::moveCursor(int col, int row) {
Serial.print((char)0xFF);
Serial.print((char)0xE4);
Serial.print((char)0x00);
Serial.print((char)row);
Serial.print((char)0x00);
Serial.print((char)col);
waitAck();
}
void uLCD_144::drawRect(int x1, int y1, int x2, int y2, byte red, byte green, byte blue) {
Serial.write(0x72);
Serial.write(x1);
Serial.write(y1);
Serial.write(x2);
Serial.write(y2);
Serial.write(getMSB(red, green, blue));
Serial.write(getLSB(red, green, blue));
waitAck();
}
void uLCD_144::printStr(int col, int row, int font, byte red, byte green, byte blue, const char *str) {
Serial.write(0x73);
Serial.write(col);
Serial.write(row);
Serial.write(font);
Serial.write(getMSB(red, green, blue));
Serial.write(getLSB(red, green, blue));
Serial.print(str);
Serial.write((uint8_t)0);
waitAck();
}
void Server::sendMessage( const int leftMotor, const int rightMotor, const int pen_up ){
if(USE_BRICK){
char command[200];
sprintf(command, "python server.py send %d %d %d", leftMotor, rightMotor, pen_up);
system(command);
waitAck();
}else{
//Simulate brick calls with a sleep
sleep(500);
}
}
void uLCD_144::drawLine(int x1, int y1, int x2, int y2, byte red, byte green, byte blue) {
Serial.write((byte)0x4C);
Serial.write((byte)x1);
Serial.write((byte)y1);
Serial.write((byte)x2);
Serial.write((byte)y2);
Serial.write((byte)getMSB(red, green, blue));
Serial.write((byte)getLSB(red, green, blue));
waitAck();
}
void uLCD_144::printStr(int col, int row, int font, byte red, byte green, byte blue, const __FlashStringHelper *str) {
Serial.write((byte)0x73);
Serial.write((byte)col);
Serial.write((byte)row);
Serial.write((byte)font);
Serial.write((byte)getMSB(red, green, blue));
Serial.write((byte)getLSB(red, green, blue));
Serial.print(str);
Serial.write((byte)0);
waitAck();
}
void uLCD_144_SPE::printStr(int col, int row, int font, byte red, byte green, byte blue, const prog_char *str, int type) { // has to have a different sig from the above function
moveCursor(col, row);
setFont(0);
setFGcolor(red, green, blue);
setYspacing(2);
Serial.print((char)0x00);
Serial.print((char)0x06);
Serial.print(str);
Serial.print((char)0);
waitAck();
}
void uLCD_144_SPE::printStr(int col, int row, int font, byte red, byte green, byte blue, const char *str) {
moveCursor(col, row);
setFont(0);
setFGcolor(red, green, blue);
setYspacing(2);
Serial.print((char)0x00);
Serial.print((char)0x06);
Serial.print(str);
Serial.print((char)0);
waitAck();
}
//==================================== SCREEN FUNCTION LIBRARY ========================
//-------------- define all function members ----------
uLCD_144_SPE::uLCD_144_SPE(int baud) {
Serial.flush();
delay(3000);
Serial.begin(baud);
// clr screen here - do NOT call function as we need to get an Ack here
Serial.print((char)0xFF);
Serial.print((char)0xD7);
isAlive_=waitAck();
}
void uLCD_144::printGRStr(int x, int y, int font, byte red, byte green, byte blue, int w, int h, const char *str) {
Serial.write(0x53);
Serial.write(x);
Serial.write(y);
Serial.write(font);
Serial.write(getMSB(red, green, blue));
Serial.write(getLSB(red, green, blue));
Serial.write(w);
Serial.write(h);
Serial.print(str);
Serial.write((uint8_t)0);
waitAck();
}
void uLCD_144::printGRStr(int x, int y, int font, byte red, byte green, byte blue, int w, int h, const char *str) {
Serial.write((byte)0x53);
Serial.write((byte)x);
Serial.write((byte)y);
Serial.write((byte)font);
Serial.write((byte)getMSB(red, green, blue));
Serial.write((byte)getLSB(red, green, blue));
Serial.write((byte)w);
Serial.write((byte)h);
Serial.print(str);
Serial.write((byte)0);
waitAck();
}
int stm32CmdGet()
{
int ret = STM_ERR;
if ( sendCommand(0x00)==STM_OK ) {
int i;
unsigned char num;
serialRead(board.serial_fd, &num, 1); // number of bytes
serialRead(board.serial_fd, &board.version, 1);
serialRead(board.serial_fd, board.cmd, num);
if ( waitAck()==STM_OK ) {
printf("Bootloader version: %x.%x\nCommand set-[", board.version >> 4, board.version & 0x0F);
for ( i=0; i < num; i++ )
printf(" %02X", board.cmd[i]);
printf("]\n");
ret = STM_OK;
}
void uLCD_144_SPE::clrScreen() {
Serial.print((char)0xFF);
Serial.print((char)0xD7);
waitAck();
}
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;
}
}
}
void uLCD_144::clrScreen() {
Serial.write((byte)0x45);
waitAck();
}
