roboforum.ru

// motor A
int dir1PinA = 13;
int dir2PinA = 12;
int speedPinA = 10;
// motor B
// motor A
int dir1PinB = 11;
int dir2PinB = 8;
int speedPinB = 9;
unsigned long time;
int speed;
int dir;
int delay_count;
int left_distance;
int rigth_distance;
int front_distance;

#include <Servo.h>
#include <Ultrasonic.h>

Servo myservo;  // create servo object to control a servo
                // a maximum of eight servo objects can be created
Ultrasonic ultrasonic(6, 5); // номера пинов к которым подключены выводы Trig и Echo, соответственно


void setup() {
  pinMode(dir1PinA, OUTPUT);
  pinMode(dir2PinA, OUTPUT);
  pinMode(speedPinA, OUTPUT);
  pinMode(dir1PinB, OUTPUT);
  pinMode(dir2PinB, OUTPUT);
  pinMode(speedPinB, OUTPUT);
  time = millis();
  speed = 0; // максимум 255
  dir = 1;
delay_count=0;
myservo.attach(1);  // attaches the servo on pin 7 to the servo object
myservo.write(90);  // ставим сервопривод смотрящим вперед
Serial.begin(9600);                           // start the serial port
Serial.println("START");                     
}

void forward() {
digitalWrite(dir1PinA, HIGH);
digitalWrite(dir2PinA, LOW);
digitalWrite(dir1PinB, HIGH);
digitalWrite(dir2PinB, LOW);
}

void back() {
digitalWrite(dir1PinA, LOW);
digitalWrite(dir2PinA, HIGH);
digitalWrite(dir1PinB, LOW);
digitalWrite(dir2PinB, HIGH);
}

void stop_dc() {
analogWrite(speedPinA,0);
analogWrite(speedPinB,0);
// digitalWrite(dir1PinA, LOW);
// digitalWrite(dir2PinA, LOW);
// digitalWrite(dir1PinB, LOW);
// digitalWrite(dir2PinB, LOW);
}

void left() {
digitalWrite(dir1PinA, HIGH);
digitalWrite(dir2PinA, LOW);
digitalWrite(dir1PinB, LOW);
digitalWrite(dir2PinB, HIGH);
}

void rigth() {
digitalWrite(dir1PinA, LOW);
digitalWrite(dir2PinA, HIGH);
digitalWrite(dir1PinB, HIGH);
digitalWrite(dir2PinB, LOW);
}

void loop() {
//delay(1000);
analogWrite(speedPinA,speed);
analogWrite(speedPinB,speed);

  // set direction
  if (1 == dir) {
   forward();
  }
  else if (0 == dir) {
   back();
  }
  else if (2 == dir) {
   left();
  }
  else if (3 == dir) {
   rigth();
  }

if (millis() - time > 5000) {
time = millis();
speed += 20;
if (speed > 255) {
  speed = 0;
}
}

front_distance=ultrasonic.Ranging(CM); // получаем расстояние до объекта в сантиметрах
Serial.write("FRONT: ");
Serial.println(front_distance);                      // print the distance

  if ((front_distance<=30) && (front_distance!=0)) {
   Serial.write("SPEED: ");
   Serial.println(speed);
   stop_dc(); // останавливаем робота
   myservo.write(0);  // ставим сервопривод смотрящим влево
   delay(500);
   left_distance=ultrasonic.Ranging(CM); // получаем расстояние до объекта в сантиметрах
   //delay(30);
   Serial.write("LEFT: ");
   Serial.println(left_distance);                      // print the distance
   myservo.write(180);  // ставим сервопривод смотрящим влево
   delay(500);
   rigth_distance=ultrasonic.Ranging(CM); // получаем расстояние до объекта в сантиметрах
   //delay(30);
   Serial.write("RIGTH: ");
   Serial.println(rigth_distance);                      // print the distance
   myservo.write(90);  // ставим сервопривод смотрящим вперед
   delay(500);
  if (left_distance>rigth_distance) {
   Serial.println("TURN_LEFT START");
    left(); //поворачиваем влево
    delay(1000);
    stop_dc(); // останавливаем робота
    dir=1; // едем прямо
    Serial.println("TURN_LEFT STOP");
  }else {
    Serial.println("TURN_RIGTH START");
    rigth(); //поворачиваем влево
    delay(1000);
    stop_dc(); // останавливаем робота
    dir=1; // едем прямо
    Serial.println("TURN_RIGTH STOP");
  }
}

}