Faced with the same problem.
Faced with the same problem. native code, and the program is not working as it should, so I had to redo another application under the control of: BTCA2A
Bluetooth Module HC-06
https://www.youtube.com/watch?v=af9dH_uKXtU
/ Sketch for control via Bluetooth, Wild Thumper 6WD, to Arduino compatible controller TREX controller. realized the movement and shooting with the jet, the capture claw arm.
// For questions and suggestions write to mail [email protected]
// New images and video can be found on www.shoualyans.com
#include <BTCA2ALite.h>// Добавляем библиотеку блютус программы
#include <Servo.h> // Добавляем библиотеку серво
Servo myservoH; // Горизонтальная серва углы от 0 до 180 (центровка 89)
Servo myservoV; // Вертикальная серва углы от 1 до 45
Servo myservoZ; // серва Клешня
Servo myservoR; // серва рука клешни
const int Z_SERVO_PIN = 5; // добовляем пин захвата
const int R_SERVO_PIN = 6; // добовляем пин руки манипулятора
const int H_SERVO_PIN = 7; // Горизонтальная серва подключена к пину 7
const int V_SERVO_PIN = 8; //Пин вертикального сервопривода 8
const float Z_L_ANGLE = 116; // Максимальные угл поворота башни налево
const float Z_R_ANGLE = 45; // Максимальные угл поворота башни направо
const float Z_DEF_ANGLE = 50; // Дефолтный угол (центровка)
const float R_L_ANGLE = 170; // Максимальные угл поднятия руки
const float R_R_ANGLE = 3; // Максимальные угл наклона руки
const float R_DEF_ANGLE = 160; // Дефолтный угол руки
const float H_L_ANGLE = 180; // Максимальные угл поворота башни налево
const float H_R_ANGLE = 0; // Максимальные угл поворота башни направо
const float H_DEF_ANGLE = 84; // Дефолтный угол (центровка)
const float V_U_ANGLE = 175; // Максимальные угл поворота башни вверх
const float V_D_ANGLE = 125; // Максимальные угл поворота башни вниз
const float V_DEF_ANGLE = 160; // Дефолтный угол (центровка)
//Размер шага V_STEP и H_STEP побираем экспериментально для достижения нужной скорости поворота серв
const float V_STEP = 0.012; // Шаг - знаение отвечающее за скорость вращения по вертикали.
const float H_STEP = 0.012; // Шаг - знаение отвечающее за скорость вращения по горизонтали
const float Z_STEP = 0.012; // шаг захвата
const float R_STEP = 0.012; // шаг руки
float curVAngle = V_DEF_ANGLE; //Переменная текущего угла верт. сервы
float curHAngle = H_DEF_ANGLE; //Переменная текущего угла гор. сервы
float curZAngle = Z_DEF_ANGLE; //
float curRAngle = R_DEF_ANGLE; //
bool moveServoVUp = false; //Флаг движения верт. сервы вверх
bool moveServoVDown = false; //Флаг движения верт. сервы вниз
bool moveServoHLeft = false; //Флаг движения гор. сервы влево
bool moveServoHRight = false; //Флаг движения гор. сервы вправо
bool moveServoZLeft = false; //Флаг движения гор. сервы влево
bool moveServoZRight = false; //Флаг движения гор. сервы вправо
bool moveServoRLeft = false; //Флаг движения гор. сервы влево
bool moveServoRRight = false; //Флаг движения гор. сервы вправо
btca2aLite btca2aLite; // Конструктор btca2aLite
// задаем пины для левого мотора
#define Dir_L 2
#define Pwm_L 3
#define Brake_L 4
// задаем пины для правого мотора
#define Dir_R 10
#define Pwm_R 11
#define Brake_R 9
// Задаем скорости для движения
byte MaxSpeed = 230; // Максимальная скорость
byte MinSpeed = 90; // Минимальная скорость
int forw_back = 0;
int righ_left = 0;
byte step_fb = 5; // Шаг изменения скорости движения вперед - назад
byte step_stop = 10; // Шаг изменения скорости остановки
byte step_rl = 10; // Шаг изменения скорости движения вправо - влево
byte step_dir = 10; // Шаг изменения скорости выравнивания
#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);
myservoZ.attach(Z_SERVO_PIN);
myservoR.attach(R_SERVO_PIN);
myservoH.write(curHAngle);
myservoV.write(curVAngle);
myservoZ.write(curZAngle);
myservoR.write(curRAngle);
//Светодиод
pinMode(13, OUTPUT); // Инициализируем пин светодиода
digitalWrite(13, HIGH); // Светодиод выключен
}
void loop(){
btca2aLite.ReadCommand(); // Считывание принятых команд (кодов кнопок)
//Вызываем функцию управление сервами
performServo();
//этот код для меня темный лес, мне его дали для плавного старта и торможения моторов. если ехать вперед и резко нажать кнопку назад, машина продолжит ехать вперед, плавно остановиться и только потом поедет назад, это спасает редуктора от разрушения.
//если ктото знает что тут к чему, прокоментируйте пожалуйста, чтоб я тоже понимал :)
static unsigned long pre_millis = 0;
if(millis()-pre_millis > 50)
{
pre_millis = millis();
if(btca2aLite.ButtonPressed(KEYCODE_DPAD_UP))
{
forw_back += step_fb;
if(forw_back > 0 && forw_back < MinSpeed) forw_back = MinSpeed;
}
else if(btca2aLite.ButtonPressed(KEYCODE_DPAD_DOWN))
{
forw_back -= step_fb;
if(forw_back < 0 && forw_back > MinSpeed*-1) forw_back = MinSpeed*-1;
}
else
{
if(forw_back < 0) forw_back += step_stop;
else if(forw_back > 0) forw_back -= step_stop;
if(abs(forw_back) < MinSpeed) forw_back = 0;
}
forw_back = constrain(forw_back, MaxSpeed*-1, MaxSpeed);
if(btca2aLite.ButtonPressed(KEYCODE_DPAD_RIGHT))
{
righ_left += step_rl;
if(righ_left > 0 && righ_left < MinSpeed) righ_left = MinSpeed;
}
else if(btca2aLite.ButtonPressed(KEYCODE_DPAD_LEFT))
{
righ_left -= step_rl;
if(righ_left < 0 && righ_left > MinSpeed*-1) righ_left = MinSpeed*-1;
}
else
{
if(righ_left < 0) righ_left += step_dir;
else if(righ_left > 0) righ_left -= step_dir;
if(abs(righ_left) < MinSpeed) righ_left = 0;
}
righ_left = constrain(righ_left, MaxSpeed*-1, MaxSpeed);
int pwm_l = forw_back + righ_left;
int pwm_r = forw_back - righ_left;
pwm_l = constrain(pwm_l, MaxSpeed*-1, MaxSpeed);
pwm_r = constrain(pwm_r, MaxSpeed*-1, MaxSpeed);
digitalWrite(Dir_L, (pwm_l<0)?0:1);
digitalWrite(Dir_R, (pwm_r<0)?0:1);
analogWrite(Pwm_L, abs(pwm_l));
analogWrite(Pwm_R, abs(pwm_r));
}
//дальше идут кнопки управления
//-------— включение насоса —-------------
if (btca2aLite.ButtonPressed(KEYCODE_BUTTON_SELECT))
{
digitalWrite (SHOT, HIGH);
}
//-------— выключение насоса происходит автоматически после отпускания кнопки—-------------
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_SELECT))
{
digitalWrite (SHOT, LOW);
}
if(btca2aLite.Button(KEYCODE_BUTTON_START)) // алгоритм автоматического хватания предметов
{
// машина делает шаг вперед
digitalWrite (Dir_L, HIGH);
analogWrite (Pwm_L, 150);
digitalWrite (Dir_R, HIGH);
analogWrite (Pwm_R, 150);
delay (100);
// потом стоп
analogWrite (Pwm_L, 0);
analogWrite (Pwm_R, 0);
delay (500);
myservoZ.write(45);; // Клешня разжимается
myservoR.write(3);; // рука клешни опускаем
delay(2000); /* ждем 2 секунды, */
myservoZ.write(116);; // Клешня сжимаем
delay(1000); /* ждем 1 секунду, */
myservoR.write(170);; // рука клешни поднимаем
delay(600); /* ждем */
myservoZ.write(45);; // Клешня разжимаем
}
//--------— Повороты серв при нажатии кнопок —-------------
if(btca2aLite.Button(KEYCODE_BUTTON_X)) // поворот водомета вниз
{
moveServoVDown = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_A)) // поворот водомета направо
{
moveServoHRight = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_B)) // поворот водомета вверх
{
moveServoVUp = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_Y)) // поворот водомета налево
{
moveServoHLeft = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_R1)) // зхахват сжать
{
moveServoZLeft = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_R2)) // захват разжать
{
moveServoZRight = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_L1)) // рука клешни вниз
{
moveServoRLeft = true;
}
if(btca2aLite.Button(KEYCODE_BUTTON_L2)) // рука клешни вверх
{
moveServoRRight = 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;
}
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_R2))
{
moveServoZRight = false;
}
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_R1))
{
moveServoZLeft = false;
}
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_L2))
{
moveServoRRight = false;
}
if(btca2aLite.ButtonReleased(KEYCODE_BUTTON_L1))
{
moveServoRLeft = 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 (moveServoZLeft && curZAngle + Z_STEP <= Z_L_ANGLE)
{
//К текущему гор. углу прибавляем гор. шаг
curZAngle += Z_STEP;
}
//Установлен флаг движения вправо и текущий угол не минимален
else if (moveServoZRight && curZAngle - Z_STEP >= Z_R_ANGLE)
{
//От текущего угла отнимаем гор. шаг
curZAngle -= Z_STEP;
}
//Установлен
if (moveServoRLeft && curRAngle + R_STEP <= R_L_ANGLE)
{
//К текущему гор. углу прибавляем гор. шаг
curRAngle += R_STEP;
}
//Установлен флаг движения вправо и текущий угол не минимален
else if (moveServoRRight && curRAngle - R_STEP >= R_R_ANGLE)
{
//От текущего угла отнимаем гор. шаг
curRAngle -= R_STEP;
}
//Если было движние по вертикали
if (moveServoVUp || moveServoVDown)
{
myservoV.write(curVAngle);
}
//Если было движение по горизонтали
if (moveServoHLeft || moveServoHRight)
{
myservoH.write(curHAngle);
}
if (moveServoZLeft || moveServoZRight)
{
myservoZ.write(curZAngle);
}
if (moveServoRLeft || moveServoRRight)
{
myservoR.write(curRAngle);
}
}
