Прототип робота сумо на базе ардуино nano

15.04.2012
By

Вот такой получился прототипчик :)

комплектующие:

два серводвигателя постоянного вращения DF15SR

два литий полимерных элемента питания по 3,7 вольт с контроллером заряда соединенных последовательно

один датчик дистанции SR04

кусок планя для лего монтажа, чуть-чуть деталей лего

два колеса от детского трактора

монтажный двусторонний скотч

термопистолет

код программы:

//скетч для робота сумо с
// двумя моторами DF15SR
//едет к препятствию, которое расположено  ближе чем distance_long и останавливается перед ним на расстоянии distance_short (в сантиметрах)

#include <Servo.h>
Servo servo_r;  // создаем два объекта серво - правое и левое колесо
Servo servo_l;

// определяем пины подключения для дистанционного сенсора
const int triggerPin = 11; // b3 на плате сенсоршилд для нано
const int echoPin = 12; // b4 на плате сенсоршилд для нано

const int distance_long = 50;
const int distance_short = 5;

void setup()
{
  // ШИМ у nano есть на выводах 3.5.6.9.10.11 - при помощи analogWrite() его можно использовать
  servo_r.attach(9);  // назначаем правому колесу вывод (b1) у шилда для нано
  servo_l.attach(10);  // левому колесу вывод (b2) у шилда для нано
  servo_r.write(93);   // останавливаем сервы
  servo_l.write(93);
  Serial.begin(9600); // инициируем компорт для отладки скетча
} 

void loop()
{
    /*
    // у DF15SR значание 93 останавливает колесо, от нуля до 93 в одну сторону и от 93 до 180 в другую сторону
    servo_r.write(86);             // крутим правую серву в одну сторону (ближе к 93 медленнее, чем дальше тем быстрее в обе стороны)
    servo_l.write(100);            // крутим левую серву  в другую сторону (ближе к 93 медленнее, чем дальше тем быстрее в обе стороны)
    delay(3000);                   // в течении секунды
    servo_r.write(93);             // останавливаем сервы
    servo_l.write(93);
    delay(3000);                   // на секунду
    servo_r.write(100);            // крутим сервы в другую сторону
    servo_l.write(86);
    delay(3000);                   // в течении секунды
    servo_r.write(93);             // останавливаем сервы
    servo_l.write(93);
    delay(3000);
    */

   // создаем переменные для продолжительности пинга и дистанции в сантиметрах
   long duration, cm; 

   // The device is triggered by a HIGH pulse of 2 or more microseconds.
   // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
    pinMode(triggerPin, OUTPUT);
    digitalWrite(triggerPin, LOW);
    delayMicroseconds(2);
    digitalWrite(triggerPin, HIGH);
    delayMicroseconds(5);
    digitalWrite(triggerPin, LOW);  // The echo pin is used to read the signal from the device: a HIGH
   // pulse whose duration is the time (in microseconds) from the sending
   // of the ping to the reception of its echo off of an object.
    pinMode(echoPin, INPUT);
    duration = pulseIn(echoPin, HIGH);
   // convert the time into a distance
    cm = microsecondsToCentimeters(duration);

   if (cm <= distance_long && cm > distance_short)
   {
   // едем прямо к препятствию, которое расположено ближе distance_long
   servo_r.write(86);
   servo_l.write(100);
   delay(500);
   }

   if (cm > distance_long)
   {
   // ищем препятствие , которое будет расположено на расстоянии distance_long и ближе крутясь на месте
   servo_r.write(100);
   servo_l.write(100);
   delay(100);
   }

   if (cm <= distance_short)
   {
   // останавливамся перед препятствием, если оно ближе чем distance_short
   servo_r.write(93);
   servo_l.write(93);
   delay(500);
   }
   /*
   Serial.print(cm);
   Serial.print("cm");
   Serial.println();
   delay(100);
   */
} 

long microsecondsToCentimeters(long microseconds)
{
  // ***  THIS NEEDS TO BE CHECKED FOR THE HC-SR04  ***
  return microseconds / 29 / 2;
}

 

Добавить комментарий