Exemplo n.º 1
0
//==================================== 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();
}
Exemplo n.º 2
0
void uLCD_144::setPenSize(int size) {
  // 0 = solid, 1 = wire
  Serial.write((byte)0x70);
  Serial.write((byte)size);

  waitAck();
}
Exemplo n.º 3
0
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();
}
Exemplo n.º 4
0
void uLCD_144::setFont(int font) {
  // 0 = small, 1 = med, 2 = large
  Serial.write((byte)0x46);
  Serial.write((byte)font);

  waitAck();
}
Exemplo n.º 5
0
void uLCD_144::setOpacity(int opacity) {
  // 0 = transparent, 1 = opaque
  Serial.write((byte)0x4f);
  Serial.write((byte)opacity);

  waitAck();
}
Exemplo n.º 6
0
void uLCD_144::setContrast(int contrast) {
  // range from 0 to 0x0f
  Serial.write(0x59);
  Serial.write(0x02);
  Serial.write(contrast);
  waitAck();  
}
Exemplo n.º 7
0
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;
}
Exemplo n.º 8
0
void uLCD_144_SPE::setFont(int font) {
  Serial.print((char)0xFF);
  Serial.print((char)0x7D);
  Serial.print((char)0x00);
  Serial.print((char)0x00);
  
  waitAck();
}
Exemplo n.º 9
0
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();
}
Exemplo n.º 10
0
void uLCD_144_SPE::setYspacing(int pixels) {
  Serial.print((char)0xFF);
  Serial.print((char)0x79);
  Serial.print((char)0x00);
  Serial.print((char)pixels);

  waitAck();
}
Exemplo n.º 11
0
// 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();
}
Exemplo n.º 12
0
// 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();
}
Exemplo n.º 13
0
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();
}
Exemplo n.º 14
0
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();
}
Exemplo n.º 15
0
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(); 
}
Exemplo n.º 16
0
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();
}
Exemplo n.º 17
0
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);
    }
}
Exemplo n.º 18
0
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();
}
Exemplo n.º 19
0
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();
}
Exemplo n.º 20
0
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();
}
Exemplo n.º 21
0
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();
}
Exemplo n.º 22
0
//==================================== 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();
}
Exemplo n.º 23
0
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();
}
Exemplo n.º 24
0
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();
}
Exemplo n.º 25
0
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;
		}
Exemplo n.º 26
0
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;
    }
  }
}
Exemplo n.º 28
0
void uLCD_144::clrScreen() {
  Serial.write((byte)0x45);
  waitAck();
}