roboforum.ru

#include <BTCA2ALite.h>
#include <Servo.h> // Добавляем библиотеку

    Servo myservoH; // Горизонтальная серва углы от 25 до 145 (центровка 85)
    Servo myservoV; // Вертикальная серва углы от 0 до 35
    const int H_SERVO_PIN = 7; // Горизонтальная серва подключена к пину 7
    const int V_SERVO_PIN = 8; //Пин вертикального сервопривода 8
    const float H_L_ANGLE = 180; // Максимальные угл поворота башни налево
    const float H_R_ANGLE = 0;  // Максимальные угл поворота башни направо
    const float H_DEF_ANGLE = 89; // Дефолтный угол (центровка)
    const float V_U_ANGLE = 70;  // Максимальные угл поворота башни вверх
    const float V_D_ANGLE = 4; // Максимальные угл поворота башни вниз
    const float V_DEF_ANGLE = 55; // Дефолтный угол (центровка)
    //Размер шага V_STEP и H_STEP побираем экспериментально для достижения нужной скорости поворота серв
    const float V_STEP = 0.007; // Шаг - знаение отвечающее за скорость вращения по вертикали.
    const float H_STEP = 0.012; // Шаг - знаение отвечающее за скорость вращения по горизонтали
    float curVAngle = V_DEF_ANGLE; //Переменная текущего угла верт. сервы
    float curHAngle = H_DEF_ANGLE; //Переменная текущего угла гор. сервы
    bool moveServoVUp = false; //Флаг движения верт. сервы вверх
    bool moveServoVDown = false; //Флаг движения верт. сервы вниз
    bool moveServoHLeft = false; //Флаг движения гор. сервы влево
    bool moveServoHRight = false; //Флаг движения гор. сервы вправо

btca2aLite btca2aLite; // Конструктор btca2aLite

// Для удобства подключения, затвор мосфета подключен к аналоговому входу A0, однако,
    // входы A0-A6 можно использовать как цифровые, в коде будут нумероваться как 14-19
    int SHOT = 12; 

int Speed = 80; // Текущая максимальная скорость
int TSpeed = 150; // Скорость тупящего мотора при повороте
int RLSpeed = 200; // Скорость танкоаого разворота

#define Dir_L 2
#define Pwm_L 3
#define Brake_L 4

#define Dir_R 10
#define Pwm_R 11
#define Brake_R 9




enum States
    {
      WAITING,
      READING,
      RUNNING,
      ERROR,
      TIMEOUT
    };
     
    States state;
    States onWait();
    States onRead();
    States onRun();
    States onError();
     
    States onTimeout();
     
    void performServo();



void setup(){
  Serial.begin(9600); // Скорость BT и порта должна быть 9600
  //Моторы
  pinMode (Pwm_L, OUTPUT);
  pinMode (Dir_L, OUTPUT);
  pinMode (Brake_L, OUTPUT);

  pinMode (Pwm_R, OUTPUT);
  pinMode (Dir_R, OUTPUT);
  pinMode (Brake_R, OUTPUT);

  digitalWrite (Pwm_L, LOW);
  digitalWrite (Pwm_R, LOW);

  digitalWrite (Brake_L, LOW);
  digitalWrite (Brake_R, LOW);
 
 
  // устанавливаем пин как вывод управления сервой 
      myservoH.attach(H_SERVO_PIN);
      myservoV.attach(V_SERVO_PIN);
      myservoH.write(curHAngle);
      myservoV.write(curVAngle);
 

  //Светодиод
  pinMode(13, OUTPUT); // Инициализируем пин светодиода
  digitalWrite(13, HIGH); // Светодиод выключен
}

void loop(){

  btca2aLite.ReadCommand(); // Считывание принятых команд (кодов кнопок)


  //---------- Движение Вперед —-------------
  if (btca2aLite.ButtonPressed(KEYCODE_DPAD_UP)) // Едет вперед если нажат курсор ВПЕРЕД
  {
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)){ // Поворот направо при нажатии курсора ВПЕРЕД и ВЛЕВО
      digitalWrite (Dir_L, HIGH);
      analogWrite (Pwm_L, TSpeed);
      digitalWrite (Dir_R, HIGH);
      analogWrite (Pwm_R, Speed); 
    }
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT)){ // Поворот налево при нажатии курсора ВПЕРЕД и ВПРАВО
      digitalWrite (Dir_L, HIGH);
      analogWrite (Pwm_L, Speed);
      digitalWrite (Dir_R, HIGH);
      analogWrite (Pwm_R, TSpeed);
    }
    digitalWrite (Dir_L, HIGH);
    analogWrite (Pwm_L, Speed);
    digitalWrite (Dir_R, HIGH);
    analogWrite (Pwm_R, Speed);
  }

  //--------- Движение Назад —-------------
  if (btca2aLite.ButtonPressed(KEYCODE_DPAD_DOWN)) // Едет назад если нажат курсор НАЗАД
  {
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)){ // Поворот налево при нажатии курсора НАЗАД и ВЛЕВО
      digitalWrite (Dir_L, LOW);
      analogWrite (Pwm_L, TSpeed);
      digitalWrite (Dir_R, LOW);
      analogWrite (Pwm_R, Speed);
    }
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT)){ // Поворот направо при нажатии курсора НАЗАД и ВПРАВО
      digitalWrite (Dir_L, LOW);
      analogWrite (Pwm_L, Speed);
      digitalWrite (Dir_R, LOW);
      analogWrite (Pwm_R, TSpeed);
    }
    digitalWrite (Dir_L, LOW);
    analogWrite (Pwm_L, Speed);
    digitalWrite (Dir_R, LOW);
    analogWrite (Pwm_R, Speed);
  }

  //--------- Движение Влево (Танковый разворот) —-------------
  if (btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT )) // Едет вперед если нажат курсор ВПЕРЕД
  {
    digitalWrite (Dir_L, LOW);
    analogWrite (Pwm_L, RLSpeed);
    digitalWrite (Dir_R, HIGH);
    analogWrite (Pwm_R, RLSpeed);
  }

  //--------- Движение Вправо (Танковый разворот) —-------------
  if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)) // Едет вперед если нажат курсор ВПЕРЕД
  {
    digitalWrite (Dir_L, HIGH);
    analogWrite (Pwm_L, RLSpeed);
    digitalWrite (Dir_R, LOW);
    analogWrite (Pwm_R, RLSpeed);
  }

   //--------- движение горизонтальной сервы —-------------
  if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)) // Едет вперед если нажат курсор ВПЕРЕД
  {
    digitalWrite (Dir_L, HIGH);
    analogWrite (Pwm_L, RLSpeed);
    digitalWrite (Dir_R, LOW);
    analogWrite (Pwm_R, RLSpeed);
  }


    //Вызываем функцию управление сервами
      performServo();


  //---------- Переключение скоростей —-------------

if(btca2aLite.Button(KEYCODE_BUTTON_X)) // servoV вниз
   {
  moveServoVDown = true;
   }
   if(btca2aLite.Button(KEYCODE_BUTTON_Y)) // servoH движется к 25
   {
    moveServoHRight = true;
   }
   if(btca2aLite.Button(KEYCODE_BUTTON_B)) // servoV вверх
   {
     moveServoVUp = true;
     
   }

   if(btca2aLite.Button(KEYCODE_BUTTON_A)) // servoH движется к 145
   {
    moveServoHLeft = true;
   }



  //------------------ Стоп —---------------------

  // Если не нажата ни одна кнопка курсора происходит остановка, скорость 0 для каждого мотора
  if (btca2aLite.ButtonReleased(KEYCODE_DPAD_UP) && btca2aLite.ButtonReleased(KEYCODE_DPAD_DOWN) && btca2aLite.ButtonReleased(KEYCODE_DPAD_LEFT) && btca2aLite.ButtonReleased(KEYCODE_DPAD_RIGHT)){
    analogWrite (Pwm_L, 0);
    analogWrite (Pwm_R, 0);
  }


//Функция управления сервоприводами (вызывается в теле loop на каждой итерации)
    //Выполняет изменения положения серв на подобранный шаг согласно установленным флагам движения
    //Флаги устанавливаются согласно поступившим символам управления сервами
    void performServo()
    {
      //Установлен флаг движения вверх и текущий угол не максимальный
      if (moveServoVUp && curVAngle + V_STEP <= V_U_ANGLE)
      {
        //К текущему верт. углу прибавляем шаг
        curVAngle += V_STEP;
      }
      //Установлен флаг движения вниз и текущий угол не минимален
      else if (moveServoVDown && curVAngle - V_STEP >= V_D_ANGLE)
      {
        //От текущего верт. угла отнимаем шаг
        curVAngle -= V_STEP;
      }
      //Установлен флаг движения влево и текущий угол не максимален
      if (moveServoHLeft && curHAngle + H_STEP <= H_L_ANGLE)
      {
        //К текущему гор. углу прибавляем гор. шаг
        curHAngle += H_STEP;
      }
      //Установлен флаг движения вправо и текущий угол не минимален
      else if (moveServoHRight && curHAngle - H_STEP >= H_R_ANGLE)
      {
        //От текущего угла отнимаем гор. шаг
        curHAngle -= H_STEP;
      }
     
      //Если было движние по вертикали
      if (moveServoVUp || moveServoVDown)
      {
            myservoV.write(curVAngle);
      }
      //Если было движение по горизонтали
      if (moveServoHLeft || moveServoHRight)
      {
        myservoH.write(curHAngle);
      }
    }

// Скетч для управления через блютус, Wild Thumper 6WD, на ардуино совместимом контролере TREX controller. реализовано движение и стрельба с водомета.


#include <BTCA2ALite.h>// Добавляем библиотеку блютус программы
#include <Servo.h> // Добавляем библиотеку серво

Servo myservoH; // Горизонтальная серва углы от 0 до 180 (центровка 89)
Servo myservoV; // Вертикальная серва углы от 1 до 45
const int H_SERVO_PIN = 7; // Горизонтальная серва подключена к пину 7
const int V_SERVO_PIN = 8; //Пин вертикального сервопривода 8
const float H_L_ANGLE = 180; // Максимальные угл поворота башни налево
const float H_R_ANGLE = 0; // Максимальные угл поворота башни направо
const float H_DEF_ANGLE = 89; // Дефолтный угол (центровка)
const float V_U_ANGLE = 45; // Максимальные угл поворота башни вверх
const float V_D_ANGLE = 1; // Максимальные угл поворота башни вниз
const float V_DEF_ANGLE = 15; // Дефолтный угол (центровка)
//Размер шага V_STEP и H_STEP побираем экспериментально для достижения нужной скорости поворота серв
const float V_STEP = 0.007; // Шаг - знаение отвечающее за скорость вращения по вертикали.
const float H_STEP = 0.012; // Шаг - знаение отвечающее за скорость вращения по горизонтали
float curVAngle = V_DEF_ANGLE; //Переменная текущего угла верт. сервы
float curHAngle = H_DEF_ANGLE; //Переменная текущего угла гор. сервы
bool moveServoVUp = false; //Флаг движения верт. сервы вверх
bool moveServoVDown = false; //Флаг движения верт. сервы вниз
bool moveServoHLeft = false; //Флаг движения гор. сервы влево
bool moveServoHRight = false; //Флаг движения гор. сервы вправо

btca2aLite btca2aLite; // Конструктор btca2aLite

// Задаем скорости для движения
int Speed = 80; // Текущая максимальная скорость
int TSpeed = 150; // Скорость медленного мотора при повороте
int RLSpeed = 200; // Скорость танкоаого разворота

// задаем пины для левого мотора
#define Dir_L 2
#define Pwm_L 3
#define Brake_L 4
// задаем пины для правого мотора
#define Dir_R 10
#define Pwm_R 11
#define Brake_R 9

#define SHOT  12 // пин для водомета

enum States
{
    WAITING,
    READING,
    RUNNING,
    ERROR,
    TIMEOUT
};

States state;
States onWait();
States onRead();
States onRun();
States onError();

States onTimeout();

void performServo(); // добовляем сервы


void setup(){
    Serial.begin(9600); // Скорость BT и порта должна быть 9600
    //Моторы
    pinMode (Pwm_L, OUTPUT);
    pinMode (Dir_L, OUTPUT);
    pinMode (Brake_L, OUTPUT);
   
    pinMode (Pwm_R, OUTPUT);
    pinMode (Dir_R, OUTPUT);
    pinMode (Brake_R, OUTPUT);
   
    digitalWrite (Pwm_L, LOW);
    digitalWrite (Pwm_R, LOW);
   
    digitalWrite (Brake_L, LOW);
    digitalWrite (Brake_R, LOW);
 
pinMode(SHOT, OUTPUT); // задаем выход для стрельбы водой
   
    // устанавливаем пин как вывод управления сервой
    myservoH.attach(H_SERVO_PIN);
    myservoV.attach(V_SERVO_PIN);
    myservoH.write(curHAngle);
    myservoV.write(curVAngle);
   
   
    //Светодиод
    pinMode(13, OUTPUT); // Инициализируем пин светодиода
    digitalWrite(13, HIGH); // Светодиод выключен
}

void loop(){
   
    btca2aLite.ReadCommand(); // Считывание принятых команд (кодов кнопок)
    //Вызываем функцию управление сервами
    performServo();
   
   
   
    //--------— Движение Вперед —-------------
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_UP)) // Едет вперед если нажат курсор ВПЕРЕД
    {
        if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)){ // Поворот направо при нажатии курсора ВПЕРЕД и ВЛЕВО
            digitalWrite (Dir_L, HIGH);
            analogWrite (Pwm_L, TSpeed);
            digitalWrite (Dir_R, HIGH);
            analogWrite (Pwm_R, Speed);
        }
        if (btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT)){ // Поворот налево при нажатии курсора ВПЕРЕД и ВПРАВО
            digitalWrite (Dir_L, HIGH);
            analogWrite (Pwm_L, Speed);
            digitalWrite (Dir_R, HIGH);
            analogWrite (Pwm_R, TSpeed);
        }
        digitalWrite (Dir_L, HIGH);
        analogWrite (Pwm_L, Speed);
        digitalWrite (Dir_R, HIGH);
        analogWrite (Pwm_R, Speed);
    }
   
    //-------— Движение Назад —-------------
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_DOWN)) // Едет назад если нажат курсор НАЗАД
    {
        if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)){ // Поворот налево при нажатии курсора НАЗАД и ВЛЕВО
            digitalWrite (Dir_L, LOW);
            analogWrite (Pwm_L, TSpeed);
            digitalWrite (Dir_R, LOW);
            analogWrite (Pwm_R, Speed);
        }
        if (btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT)){ // Поворот направо при нажатии курсора НАЗАД и ВПРАВО
            digitalWrite (Dir_L, LOW);
            analogWrite (Pwm_L, Speed);
            digitalWrite (Dir_R, LOW);
            analogWrite (Pwm_R, TSpeed);
        }
        digitalWrite (Dir_L, LOW);
        analogWrite (Pwm_L, Speed);
        digitalWrite (Dir_R, LOW);
        analogWrite (Pwm_R, Speed);
    }
   
    //-------— Движение Влево (Танковый разворот) —-------------
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT )) // Едет вперед если нажат курсор ВПЕРЕД
    {
        digitalWrite (Dir_L, LOW);
        analogWrite (Pwm_L, RLSpeed);
        digitalWrite (Dir_R, HIGH);
        analogWrite (Pwm_R, RLSpeed);
    }
   
    //-------— Движение Вправо (Танковый разворот) —-------------
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)) // Едет вперед если нажат курсор ВПЕРЕД
    {
        digitalWrite (Dir_L, HIGH);
        analogWrite (Pwm_L, RLSpeed);
        digitalWrite (Dir_R, LOW);
        analogWrite (Pwm_R, RLSpeed);
    }
   
    //-------— движение в право —-------------
    if (btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT)) // Едет вперед если нажат курсор ВПЕРЕД
    {
        digitalWrite (Dir_L, HIGH);
        analogWrite (Pwm_L, RLSpeed);
        digitalWrite (Dir_R, LOW);
        analogWrite (Pwm_R, RLSpeed);
    }
   
   
    //-------— включение насоса —-------------
   if (btca2aLite.ButtonPressed(KEYCODE_BUTTON_SELECT))
    {
     digitalWrite (SHOT, HIGH);
     }
    //-------— выключение насоса происходит автоматически после отпускания кнопки—-------------
   if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_SELECT))
    {
   digitalWrite (SHOT, LOW);
    }
   
   
    //--------— Повороты серв при нажатии кнопок —-------------
   
    if(btca2aLite.Button(KEYCODE_BUTTON_X)) // servoV вниз
    {
        moveServoVDown = true;
             
    }
    if(btca2aLite.Button(KEYCODE_BUTTON_A)) // servoH движется к 0
    {
        moveServoHRight = true;
    }
    if(btca2aLite.Button(KEYCODE_BUTTON_B)) // servoV вверх
    {
        moveServoVUp = true;
             
    }
   
    if(btca2aLite.Button(KEYCODE_BUTTON_Y)) // servoH движется к 180
    {
        moveServoHLeft = true;
    }
   
//--------— чтобы сервы не продолжали движения при отжатых кнопках —-------------   
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_X))
{
moveServoVDown = false;
}
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_B))
{
moveServoVUp = false;
}

if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_A))
{
moveServoHRight = false;
}
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_Y))
{
moveServoHLeft = false;
}
   
   
    //----------------— Стоп —---------------------
   
    // Если не нажата ни одна кнопка курсора происходит остановка, скорость 0 для каждого мотора
    if (btca2aLite.ButtonReleased(KEYCODE_DPAD_UP) && btca2aLite.ButtonReleased(KEYCODE_DPAD_DOWN) && btca2aLite.ButtonReleased(KEYCODE_DPAD_LEFT) && btca2aLite.ButtonReleased(KEYCODE_DPAD_RIGHT)){
        analogWrite (Pwm_L, 0);
        analogWrite (Pwm_R, 0);
    }
}



//Функция управления сервоприводами (вызывается в теле loop на каждой итерации)
//Выполняет изменения положения серв на подобранный шаг согласно установленным флагам движения
//Флаги устанавливаются согласно поступившим символам управления сервами
void performServo()
{
    //Установлен флаг движения вверх и текущий угол не максимальный
    if (moveServoVUp && curVAngle + V_STEP <= V_U_ANGLE)
    {
        //К текущему верт. углу прибавляем шаг
        curVAngle += V_STEP;
    }
    //Установлен флаг движения вниз и текущий угол не минимален
    else if (moveServoVDown && curVAngle - V_STEP >= V_D_ANGLE)
    {
        //От текущего верт. угла отнимаем шаг
        curVAngle -= V_STEP;
    }
    //Установлен флаг движения влево и текущий угол не максимален
    if (moveServoHLeft && curHAngle + H_STEP <= H_L_ANGLE)
    {
        //К текущему гор. углу прибавляем гор. шаг
        curHAngle += H_STEP;
    }
    //Установлен флаг движения вправо и текущий угол не минимален
    else if (moveServoHRight && curHAngle - H_STEP >= H_R_ANGLE)
    {
        //От текущего угла отнимаем гор. шаг
        curHAngle -= H_STEP;
    }
   
    //Если было движние по вертикали
    if (moveServoVUp || moveServoVDown)
    {
        myservoV.write(curVAngle);
    }
    //Если было движение по горизонтали
    if (moveServoHLeft || moveServoHRight)
    {
        myservoH.write(curHAngle);
    }
}