Напряжение питания: 5V DC
Ток покоя: <2mA
Эффективный угол: <15°
Диапазон измерения расстояния: 2–400 cm
Разрешение: 0.3 cm
#include "Arduino.h" #include "EasyRo.h" EasyRo::EasyRo(int MOTOR_LEFT_OUT1, int MOTOR_LEFT_OUT2, int MOTOR_LEFT_PWM, int MOTOR_RIGHT_OUT1, int MOTOR_RIGHT_OUT2, int MOTOR_RIGHT_PWM) { pinMode(MOTOR_LEFT_OUT1, OUTPUT); pinMode(MOTOR_LEFT_OUT2, OUTPUT); pinMode(MOTOR_LEFT_PWM, OUTPUT); pinMode(MOTOR_RIGHT_OUT1, OUTPUT); pinMode(MOTOR_RIGHT_OUT2, OUTPUT); pinMode(MOTOR_RIGHT_PWM, OUTPUT); PIN_MOTOR_LEFT_OUT1 = MOTOR_LEFT_OUT1; PIN_MOTOR_LEFT_OUT2 = MOTOR_LEFT_OUT2; PIN_MOTOR_LEFT_PWM = MOTOR_LEFT_PWM; PIN_MOTOR_RIGHT_OUT1 = MOTOR_RIGHT_OUT1; PIN_MOTOR_RIGHT_OUT2 = MOTOR_RIGHT_OUT2; PIN_MOTOR_RIGHT_PWM = MOTOR_RIGHT_PWM; } void EasyRo::FORWARD(int SPEED) { digitalWrite(PIN_MOTOR_LEFT_OUT1, HIGH); digitalWrite(PIN_MOTOR_LEFT_OUT2, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT1, HIGH); digitalWrite(PIN_MOTOR_RIGHT_OUT2, LOW); for(int i = 0; i < SPEED; i++) { analogWrite(PIN_MOTOR_LEFT_PWM, SPEED); analogWrite(PIN_MOTOR_RIGHT_PWM, SPEED); } } void EasyRo::BACKWARD(int SPEED) { digitalWrite(PIN_MOTOR_LEFT_OUT1, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT2, HIGH); digitalWrite(PIN_MOTOR_RIGHT_OUT1, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT2, HIGH); for(int i = 0; i < SPEED; i++) { analogWrite(PIN_MOTOR_LEFT_PWM, SPEED); analogWrite(PIN_MOTOR_RIGHT_PWM, SPEED); } } void EasyRo::FORWARD_LEFT(int SPEED) { digitalWrite(PIN_MOTOR_RIGHT_OUT1, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT2, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT1, HIGH); digitalWrite(PIN_MOTOR_LEFT_OUT2, LOW); for(int i = 0; i < SPEED; i++) { analogWrite(PIN_MOTOR_LEFT_PWM, SPEED); } } void EasyRo::BACKWARD_LEFT(int SPEED) { digitalWrite(PIN_MOTOR_RIGHT_OUT1, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT2, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT1, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT2, HIGH); for(int i = 0; i < SPEED; i++) { analogWrite(PIN_MOTOR_LEFT_PWM, SPEED); } } void EasyRo::FORWARD_RIGHT(int SPEED) { digitalWrite(PIN_MOTOR_LEFT_OUT1, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT2, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT1, HIGH); digitalWrite(PIN_MOTOR_RIGHT_OUT2, LOW); for(int i = 0; i < SPEED; i++) { analogWrite(PIN_MOTOR_RIGHT_PWM, SPEED); } } void EasyRo::BACKWARD_RIGHT(int SPEED) { digitalWrite(PIN_MOTOR_LEFT_OUT1, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT2, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT1, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT2, HIGH); for(int i = 0; i < SPEED; i++) { analogWrite(PIN_MOTOR_RIGHT_PWM, SPEED); } } void EasyRo::STOP(int SPEED) { for(int i = SPEED; i >=5 ; i++) { analogWrite(PIN_MOTOR_LEFT_PWM, SPEED); analogWrite(PIN_MOTOR_RIGHT_PWM, SPEED); } digitalWrite(PIN_MOTOR_LEFT_OUT1, LOW); digitalWrite(PIN_MOTOR_LEFT_OUT2, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT1, LOW); digitalWrite(PIN_MOTOR_RIGHT_OUT2, LOW); }
Сам скетч:
#include <EasyRo.h>
#include <math.h>
#define LEFT_MOTOR_OUT_1 13
#define LEFT_MOTOR_OUT_2 12
#define LEFT_MOTOR_PWM 11
#define RIGHT_MOTOR_OUT_1 8
#define RIGHT_MOTOR_OUT_2 7
#define RIGHT_MOTOR_PWM 6
#define TRIG 4
#define ECHO 3
int time = 0;
int dist = 0;
int left = 0;
int right = 0;
EasyRo robot(LEFT_MOTOR_OUT_1,LEFT_MOTOR_OUT_2,LEFT_MOTOR_PWM,
RIGHT_MOTOR_OUT_1,RIGHT_MOTOR_OUT_2,RIGHT_MOTOR_PWM);
void setup()
{
pinMode(TRIG, OUTPUT);
pinMode(ECHO, INPUT);
Serial.begin(9600);
}
void loop()
{
robot.FORWARD(255);
if(GetDist() <= 5) // Если расстояние до объекта 5 или менее сантиметров то
{
robot.BACKWARD(255); // Движемся назад
delay(1000);//Останавливаем программу на 1000 милисекунд
robot.BACKWARD_LEFT(255); // Движемся только левым колесом назад
delay(500);
left = GetDist(); // Получаем расстояние до ближайшего объекта в левой стороне
robot.FORWARD_LEFT(255); // Движемся только левым колесом вперед
delay(1000); //Останавливаем программу на 1000 милисекунд
right = GetDist(); // Получаем расстояние до ближайшего объекта в правой стороне
if(left > right) // Если в левой стороне расстояние до объекта больше то
{
robot.BACKWARD_LEFT(255); // Движемся только левым колесом назад
delay(1000); //Останавливаем программу на 1000 милисекунд
robot.FORWARD(255); // Двигаемся вперед
}
}
}
int GetDist() // Получаем дистанцию до объекта в сантиметрах
{
reget:
digitalWrite(TRIG,HIGH);
delayMicroseconds(10);
digitalWrite(TRIG, LOW);
delayMicroseconds(100);
time = pulseIn(ECHO,HIGH);
dist = time/58;
if(dist < 0)
{
goto reget;
}
Serial.println(dist);
delay(10);
return dist;
}
