CyberWrt модуль "Робот"

[tanir196]

#include <CyberLib.h>  // Подключаем библиотеку
#include <Servo.h> // Подключаем библиотеку сервоприводов

Servo myservo1;
Servo myservo2;

long previousMillis; // Нужно для таймера
int LedStep = 0; // Счетчик для LED
int i;

#define robot_go {D4_Low; D5_High; D6_High; D7_Low;} 
#define robot_back {D4_High; D5_High; D6_High; D7_High;} 
#define robot_stop {D4_Low; D5_Low; D6_Low; D7_Low;} 
#define robot_rotation_right {D4_Low; D5_High; D6_High; D7_High;}
#define robot_rotation_left {D4_High; D5_High; D6_High; D7_Low;}
#define LED_ON {D13_High;} 
#define LED_OFF {D13_Low;} 
#define Headlamp_ON {D8_Low;} 
#define Headlamp_OFF {D8_High;}
#define Buzzer {tone(11, 494, 500);}
#define init {D4_Out; D5_Out; D6_Out; D7_Out; D8_Out; D13_Out;} 
uint8_t inByte;


void setup()  
{ 
  myservo1.attach(9); // Подключение сервоприводов к порту
  myservo2.attach(10); // Подключение сервоприводов к порту
  D11_Out;  D11_Low; // Динамик 
  Headlamp_OFF; // Фары выкл по умолчанию
  for(uint8_t i=0; i<12; i++) beep(80, random(100, 2000)); //звуковое оповещение готовности робота 
  init;  // Инициализация портов
  //Buzzer; // Инициализация портов динамика
  UART_Init(57600);// Инициализация порта для связи с роутером  
  wdt_enable (WDTO_500MS); 
}  

void loop()  
{ 
    unsigned long currentMillis = millis(); // Обновление таймера
      if (LedStep == 0 && currentMillis - previousMillis > 500){ // Задержка 0,5 сек. 
      previousMillis = currentMillis; // обновление таймер
      LED_ON;   // Включить
      LedStep = 1; // Счетчик шагов
    }

    if (LedStep == 1 && currentMillis - previousMillis > 500){ // Задержка 0,5 сек.
      previousMillis = currentMillis; // обновление таймер
      LED_OFF;  // Выключить
      LedStep = 2; // Счетчик шагов
    }

    if (LedStep == 2 && currentMillis - previousMillis > 500){ // Задержка 0,5 сек.
      LedStep = 0; // Счетчик шагов
    }
  
  if (UART_ReadByte(inByte)) //Еесли что то пришло
  {
    switch (inByte)  // Смотрим какая команда пришла
    {  
        case 'x': // Остоновка робота
          robot_stop;
        break; 
        
        case 'W': // Движение вперед
          robot_go;  
        break;  
        
        case 'D': // Повопорт влево
          robot_rotation_left; 
        break;

        case 'A': // Поворот вправо
          robot_rotation_right; 
        break; 
        
        case 'S': // Движение назад
          robot_back; 
        break; 
        
        case 'U': // Серво поднимается
          myservo1.write(i -= 20);  
        break; 
        
        case 'J': // Серво опускается
          myservo1.write(i += 20);
        break; 
        
        case 'H': // Серво поворачивается влево
         myservo2.write(i += 20); 
        break; 
        
        case 'K': // Серво поворачивается вправо    
         myservo2.write(i -= 20);
        break; 
        
        case 'Y': // Серво поворачивается 85
         myservo1.write(85);  
         myservo2.write(85);
        break; 
        
         case 'F': // Включить фары
         Headlamp_ON;
        break;
        
        case 'V': // Выключить фары
         Headlamp_OFF;
        break;
        
        case 'I': // Гудок
         Buzzer;
        break;
    }          
  } 
 wdt_reset();
}