Робот-черепаха для рисования

Целью этого проекта является создание робота для рисования, которого можно легко собрать. Черепашка может рисовать мелками, фломастером или ручкой. Высокая точность перемещений достигается за счёт использования шаговых моторов.

Детали

Для создания нам понадобятся:

1 шт., DFRduino Uno или другая с таким форм-фактором, например DFRduino Leonardo
1 шт., шилд с двумя драйверами шаговых моторов
2 шт., шаговый мотор типоразмера Nema17, к примеру, JK42HS40-1304F
1 шт., микросервопривод Feetech FS90R или подобный (Feetech FS90MG, DFRobot DF9GMS и т.д.)
3 шт., литиевые аккумуляторы 18650
3 шт., держатель литиевого аккумулятора 18650
1 шт., зарядное устройство для литиевых аккумуляторов
1 шт., комплект (артикул FIT0387) из двух переходников для соединения колёс с валом мотора (не обязательно, если использовать другие колёса)

2 шт., резиновое кольцо для колеса (приблизительный диаметр 105мм)
1 шт., 1” (2.54мм) шарик или готовая шаровая опора

Крепёж:

2 шт., винт M3x8мм с полукруглой головкой
14 шт., гайка М3
10 шт., M3x6мм винт с полукруглой головкой
4 шт., M3x6мм винт с плоской головкой
4 шт., #2 x 1/4 резьбонарезной винт

Детали, 3D-печать

Часть деталей для этого робота я решил спроектировать самостоятельно и затем распечатать их на 3D-принтере. STL файлы и исходные модели (FreeCad и OpenSCAD) можно скачать тут.

Шаровую опору можно сделать самостоятельно или взять готовую. Я использовал самодельный, т.к. у меня был 1 дюймовый стальной шарик и хотелось научиться создавать такие модели.

Если будете использовать готовую шаровую опору, не забудьте перепроверить расположение крепёжных отверстий у шаровой опоры и при необходимости адаптируйте модель нижней крышки робота под Вашу шаровую опору.

Колёса с большим диаметром (11см) нужны, чтобы обеспечить достаточный зазор для батарей. Для закрепления колёс на валу использованы переходники, которые выпускала DFRobot. Если у Вас таких нет, их конечно можно начертить самостоятельно, но проще будет сделать другое колесо, которое закрепляется на прямую на вал. Для более надёжной фиксации на колесе можно сделать примерно так:

Резиновое кольцо нужного диаметра иногда сложно достать. Если сделать внешнюю часть плоской, то вместо резинового кольца, на колесо можно наклеить полоску из материала с хорошим сцеплением:

Сборка шасси

Запрессовываем (или затягиваем винтом) M3 гайку в нижнюю пластину шасси:

С помощью винтов M3 закрепляем моторы так, чтобы электрические разъемы были обращены к заднему (более короткому) концу:

Устанавливаем держатели батарей с помощью винтов с плоской головкой:

Вставляем стальной шарик в держатель (при необходимости можно нагреть феном, чтобы смягчить), затем закрепляем шаровую опору на корпусе:

На вал устанавливаем переходник для колеса:

М3 винтами и гайками соединяем ствол, верхнюю крышку для моторов и крепление для сервомотора:

Теперь прикручиваем эту крышку к моторам:

В моём случае, одеть колёса и резиновые кольца можно сразу:

Если используются напечатанные колёса, одеваемые на вал, в зависимости от использованного пластика и формы, чтобы не расшатывать лишний раз, возможно лучше будет одеть колёса позже, после установки всей электроники.

Электроника

Прикручиваем Arduino к корпусу:

К Arduino подключаем шилд драйверов моторов:

Питание организовано следующим образом:

Не всегда удобно доставать и вставлять аккумулятор, чтобы подать питание или обесточить. Где на фото выше на проводе отмечено красным, там можно дополнительно установить выключатель.

Шилд драйверов моторов может быть запитан от 8 до 35В для ревизии с драйверами A4988 и от 8,2 до 45В для ревизии с драйверами DRV8835.

Номинальное напряжение шаговых моторов очень часто или совсем не указывают или указывают небольшое значение, к примеру, для JK42HS40-1304F указано 3,4 В. Если указано номинальное напряжение, это не значит, что подавать больше нельзя. Для шаговых моторов вполне нормально, когда используемое напряжение в разы больше номинального. К примеру шаговые моторы типоразмера Nema17 на подобии используемых, очень часто питают от 12В или 24В. А вот указываемый ток превышать не стоит – мотор может выйти из строя.

Для DFRduino Uno рекомендуемое входное напряжение 7-12В (максимальное больше) при подключении питания через штекер или 5В, если запитывать по USB. На других платах диапазон питающего напряжения может отличаться, в том числе и в меньшую сторону. Если на используемой Вами плате для стабилизатора питания максимальное напряжение ниже 12В или Вы не уверены, какой максимум, можно или использовать вместо трёх аккумуляторов два. Или три, но перед Arduino установить дополнительный стабилизатор питания, на выходе которого установить допустим 6-7В.

Номинальное напряжение 18650 литиевых аккумуляторов 3,7В. У полностью заряженных напряжение примерно 4-4,2В. Если подключить последовательно три аккумулятора, напряжение будет примерно 11-12В. Припой держатели батарей параллельно, как показано на рисунке выше.

Некоторое время для меня было проблематично правильно подключить моторы, разобраться помогло описание на Wiki. Для ревизии шилда с драйвером A4988:

Для ревизии шилда с драйвером DRV8825:

Подключаем моторы к шилду:

Arduino будет программироваться через USB и на первое время можно запитать Arduino не от аккумулятора. А позже, когда убедитесь, что всё правильно подключено и работает, можно уже будет подключить примерно так:

Приведенные в Wiki примеры скетчей помогли проверить работоспособность и правильность подключения, но это не то, что нужно для данного робота. Нам нужно будет не просто вращать колёса, а ещё контролировать скорость вращения и для экономии энергии отключать питание моторов, когда они не используются.

Я нашел другой пример:

//////////////////////////////////////////////////////////////////
//©2011 bildr
//Released under the MIT License – Please reuse change and share
//Using the easy stepper with your arduino
//use rotate and/or rotateDeg to controll stepper motor
//speed is any number from .01 -> 1 with 1 being fastest –
//Slower Speed == Stronger movement
/////////////////////////////////////////////////////////////////

#define DIR_PIN 2
#define STEP_PIN 3

void setup()
{
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
}

void loop()
{
  //rotate a specific number of degrees
  rotateDeg(360, 1);
  delay(1000);

  rotateDeg(-360, .1); //reverse
  delay(1000);

  //rotate a specific number of microsteps (8 microsteps per step)
  //a 200 step stepper would take 1600 micro steps for one full revolution
  rotate(1600, .5);
  delay(1000);

  rotate(-1600, .25); //reverse
  delay(1000);
}

void rotate(int steps, float speed)
{
  //rotate a specific number of microsteps (8 microsteps per step) – (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (steps > 0) ? HIGH : LOW;
  steps = abs(steps);

  digitalWrite(DIR_PIN, dir);

  float usDelay = (1 / speed) * 70;

  for (int i = 0; i < steps; i++)
  {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay);
  }
}

void rotateDeg(float deg, float speed)
{
  //rotate a specific number of degrees (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (deg > 0) ? HIGH : LOW;
  digitalWrite(DIR_PIN, dir);

  int steps = abs(deg) * (1 / 0.225);
  float usDelay = (1 / speed) * 70;

  for (int i = 0; i < steps; i++)
  {
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay);
  }
}

Данный скетч управляет только одним шаговым мотором и используется для другого драйвера мотора, но позволяет управлять скоростью:

Осталось только немного подправить код под используемый шилд:

//////////////////////////////////////////////////////////////////
//©2011 bildr
//Released under the MIT License – Please reuse change and share
//Using the easy stepper with your arduino
//use rotate and/or rotateDeg to controll stepper motor
//speed is any number from .01 -> 1 with 1 being fastest –
//Slower Speed == Stronger movement
//http://bildr.org/2011/06/easydriver/

/*
MS1 MS2 MS3 
LOW LOW LOW FULL
HI  LOW LOW HALF
LOW HI  LOW QUARTER
HI  HI  LOW EIGHT     <----
HI  HI  HI  SIXTEENTH

1/8 Step = 1600 steps per revolution

FIT0278 Hybrid stepper 1.8 deg/step
  2B Blue
  2A Red
  1A Black
  1B Green

*/ 

/////////////////////////////////////////////////////////////////

// Drives M1 only, use DIR_PIN 4 and STEP_PIN 5 for M2
#define DIR_PIN 7
#define STEP_PIN 6

void setup() {
  pinMode(DIR_PIN, OUTPUT);
  pinMode(STEP_PIN, OUTPUT);
}


void loop(){
  //rotate a specific number of degrees
  rotateDeg(360, 1);
  delay(1000);

  rotateDeg(-360, .1); //reverse
  delay(1000);

  //rotate a specific number of microsteps (8 microsteps per step)
  //a 200 step stepper would take 1600 micro steps for one full revolution
  rotate(1600, .5);
  delay(1000);

  rotate(-1600, .25); //reverse
  delay(1000);
}


void rotate(int steps, float speed){
  //rotate a specific number of microsteps (8 microsteps per step) – (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  steps = abs(steps);

  digitalWrite(DIR_PIN, dir);

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){ 
    digitalWrite(STEP_PIN, HIGH); 
    delayMicroseconds(usDelay); 
    digitalWrite(STEP_PIN, LOW); 
    delayMicroseconds(usDelay); 
  } 
} 

void rotateDeg(float deg, float speed){ 
  //rotate a specific number of degrees (negitive for reverse movement) 
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (deg > 0)? HIGH:LOW;
  digitalWrite(DIR_PIN, dir);

  int steps = abs(deg)*(1/0.225);
  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){ 
    digitalWrite(STEP_PIN, HIGH); 
    delayMicroseconds(usDelay); 
    digitalWrite(STEP_PIN, LOW); 
    delayMicroseconds(usDelay); 
  } 
}

Сервопривод используется для поднятия и опускания ручки, мелка или фломастера при рисовании. Перед тем как закрепить сервопривод на корпусе, нужно на валу правильно закрепить качалку. Наденьте качалку на сервопривод и аккуратно, чтобы не сломать шестерёнки, проверните вал против часовой стрелки до упора. Посмотрите на изображение:

Провод из сервопривода выходит с той стороны, что ближе к руке. Если провернув вал, качалка не перпендикулярна корпусу, как на фото выше, не проворачивая вал снимите её и оденьте как показано на фото.

Теперь прикрутим сервопривод к корпусу:

И подключим провод к шилду:

Калибровка

Что бы перемещения были более точными, нужно более-менее точно знать внешний диаметр колёс и расстояния между колёсами. Измеряем диаметр колеса от внешних краев резинового уплотнительного кольца:

Измеряем расстояние между колёсами (от центра резиновых колец):

В скетче DFRobot_calibrate.ino:

#include <Servo.h>

typedef int var;        // map javascript datatype to integer

// setup servo
int servoPin = 3;
int PEN_DOWN = 170;     // angle of servo when pen is down
int PEN_UP = 90;        // angle of servo when pen is up
Servo penServo;

int M1dirpin = 4;
int M1steppin = 5;
int M2dirpin = 7;
int M2steppin = 6;

float wheel_dia = 115;  //   mm (increase = decrease distance
float wheel_base = 127.5;  //   mm (increase = spiral in)
int steps_rev = 1600;      //
float speed = 0.05;          // 0.1 - 1

int LED = 13;

void setup() {
  pinMode(M1dirpin, OUTPUT);
  pinMode(M1steppin, OUTPUT);
  pinMode(M2dirpin, OUTPUT);
  pinMode(M2steppin, OUTPUT);

  pinMode(LED, OUTPUT);

  penServo.attach(servoPin);
  penUp();
  Serial.begin(9600);
  while (!Serial) {} // wait for serial port to connect. Needed for Leonardo only
  Serial.println("hello");
}


void loop()
{
  int length = 100;
  int angle = 90;

  penDown();

  for (int i = 0; i < 4; i++)
  {
    moveForward(length);
    turnRight(angle);
    moveForward(length);
    turnRight(angle);
    moveForward(length);
    turnRight(angle);
    moveForward(length);
    delay(2000);
  }
  
  penUp();
  moveForward(100);
  done();
  while (1);
}


// ----- HELPER FUNCTIONS -----------
int step(float distance) {
  float steps = distance * steps_rev / (wheel_dia * 3.1412); //24.61
  return int(steps);
}


void moveForward(float distance) {
  move(distance, HIGH, LOW);
}


void moveBackward(float distance) {
  move(distance, LOW, HIGH);
}


void turnRight(float degrees) {
  float rotation = getNearestAngle(degrees) / 360.0;
  float distance = wheel_base * 3.1412 * rotation;
  move(distance, HIGH, HIGH);
}


void turnLeft(float degrees) {
  float rotation = getNearestAngle(degrees) / 360.0;
  float distance = wheel_base * 3.1412 * rotation;
  move(distance, LOW, LOW);
}


void move(float distance, int dir1, int dir2) {
  int steps = step(distance);
  float usDelay = (1 / speed) * 70;

  digitalWrite(M1dirpin, dir1);
  digitalWrite(M2dirpin, dir2);

  for (int i = 0; i < steps; i++) {
    digitalWrite(M1steppin, HIGH);
    digitalWrite(M2steppin, HIGH);
    delayMicroseconds(usDelay);
    digitalWrite(M1steppin, LOW);
    digitalWrite(M2steppin, LOW);
    delayMicroseconds(usDelay);
  }

}


void penUp() {
  delay(250);
  penServo.write(PEN_UP);
  delay(250);
}


void penDown() {
  delay(250);
  penServo.write(PEN_DOWN);
  delay(250);
}


void done() {
  digitalWrite(M1steppin, LOW);
  digitalWrite(M2steppin, LOW);
  pinMode(12, OUTPUT);
  pinMode(8, OUTPUT);
  digitalWrite(12, HIGH); // enable 2
  digitalWrite(8, HIGH); // enable  1
}


float getNearestAngle(float angle_) {
  // code contributed by Instructable user PMPU_student
  /*
    Lets rotate by 58 degrees.
    turnRight(58); // rotate only by 57.631138392857146 degrees(that is the
    value that is the closest to the desired one _and_ is lesser than it).
    That means you are doing a certain amount of motor steps. But if you do
    one more step during rotation you will rotate by 58.0078125 degrees,
    which is much more precise(but it is greater than desired value, thats
    why in original code it is not picked). The thing is that original code
    always decides to rotate by value that is lesser than desired value while
    it is sometimes worse than choosing the value that is bigger.
    My code chooses from both variants, minimizing the error.
  */
  float angle = 0;
  float angle_error;
  int step = 0;
  float previousAngle = 0;
  float step_length = 3.1412 * wheel_dia / steps_rev;
  //angle_ += angle_error;
  while (!(previousAngle <= angle_ && angle_ <= angle)) {
    step += 1;
    previousAngle = angle;
    angle = step * step_length * 360 / (wheel_base * 3.1412) + 0.01;
  }
  float dif1 = angle_ - angle;
  float dif2 = angle_ - previousAngle;
  if (abs(dif1) < abs(dif2)) {
    angle_error = dif1;
    Serial.println(angle_error);
    return angle;
  } else {
    angle_error = dif2;
    Serial.println(angle_error);
    return previousAngle;
  }
}


/*
  //////////////////////////////////////////////////////////////////
  //©2011 bildr
  //Released under the MIT License – Please reuse change and share
  //Using the easy stepper with your arduino
  //use rotate and/or rotateDeg to controll stepper motor
  //speed is any number from .01 -> 1 with 1 being fastest –
  //Slower Speed == Stronger movement
  //http://bildr.org/2011/06/easydriver/

  void rotate(int steps, float speed){
  //rotate a specific number of microsteps (8 microsteps per step) – (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  steps = abs(steps);

  digitalWrite(DIR_PIN, dir);

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay);
  }
  }


  void rotateDeg(float deg, float speed){
  //rotate a specific number of degrees (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (deg > 0)? HIGH:LOW;
  digitalWrite(DIR_PIN, dir);

  int steps = abs(deg)*(1/0.225);
  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){
    digitalWrite(STEP_PIN, HIGH);
    delayMicroseconds(usDelay);
    digitalWrite(STEP_PIN, LOW);
    delayMicroseconds(usDelay);
  }
  }

*/

void blinkEyes(int blinks) {
  for (int x = 0; x < blinks; x++) {
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(200);
  }
}

Меняем при необходимости значения для следующих переменных:

wheel_dia – диаметр колеса
wheel_base – расстояние между колёсами
steps_rev – шагов мотора на оботор
speed - скорость

Загружаем скетч в Arduino.

Подготовка ручки:

снимите колпачок с ручки
вставьте ручку в держатель с сервоприводом
убедитесь, что ручка не касается бумаги в этом положении
в держателе ручка должна свободно перемещаться

Всё готово, теперь пробуем нарисовать тестовый квадрат, включаем робота и когда он закончит рисовать первый квадрат, приподымаем его и выключаем. В скетче:

void loop()
{
int length = 100;
int angle = 90;

«length» задаёт сколько миллиметров робот должен проехать, а «angle» указывает на какой угол повернуть. Сначала замеряем длину стороны квадрата, она должна быть 100мм. Если измеренное расстояние больше 100мм, увеличьте значение переменной wheel_dia. Если измеренное расстояние меньше, уменьшите значение wheel_dia.

После калибровки расстояния перейдём к калибровке параметра wheel_base, который влияет на угол поворота. Поместите робота на чистый лист бумаги, включите его и дайте ему нарисовать все четыре квадрата:

Если квадрат при рисовании «проворачивается» слишком сильно по часовой стрелке, уменьшите значение wheel_base. Если квадрат при рисовании «проворачивается» слишком сильно против часовой стрелки, увеличьте значение wheel_base.

Из-за ошибок округления в коде, не идеальности колёс, недостаточной жесткости конструкции или неровности поверхности, идеально точно позиционировать робота не получится – ошибка позиционирования будет постоянно накапливаться. Поэтому не тратьте много времени на попытки идеально откалибровать, как на последнем рисунке это более-менее нормальный результат.

И в заключение вот ещё один пример скетча рисования:

/*
This sample code is for testing the 2 stepper motors 
The rotation velocity can be adjusted by the code switch 
Microcontroller: Arduino UNO  

FIT0278 Hybrid stepper 1.8 deg/step
  2B RED
  2A BLU
  1A BLK
  1B GRN
*/

#include <Servo.h>
#include <EEPROM.h>
typedef int var;        // map javascript datatype to integer

// setup servo
int servoPin = 3;
int PEN_DOWN = 170;     // angle of servo when pen is down
int PEN_UP = 90;        // angle of servo when pen is up
Servo penServo;

int M1dirpin = 4;
int M1steppin = 5;
int M2dirpin = 7;
int M2steppin = 6;

float wheel_dia=115;    //   mm (increase = decrease distance
float wheel_base=127.5;    //   mm (increase = spiral in) 
int steps_rev=1600;        //   
float speed = 0.05;          // 0.1 - 1

byte last_choice;  // remember laster pattern drawn (0 spiral, 1 snowflake)
int LED = 13;

void setup(){
  randomSeed(analogRead(A3));  // randomize on floating input
  pinMode(M1dirpin,OUTPUT);
  pinMode(M1steppin,OUTPUT);
  pinMode(M2dirpin,OUTPUT);
  pinMode(M2steppin,OUTPUT);

  pinMode(LED, OUTPUT);

  last_choice = EEPROM.read(1);
  if (last_choice > 1) {
    last_choice = 1; // may be very large first time
    EEPROM.write(1, last_choice);  
  }

  penServo.attach(servoPin);
  penUp();
  Serial.begin(9600);
  while (!Serial) {} // wait for serial port to connect. Needed for Leonardo only
  Serial.println("hello");
  delay(2000);
}


void loop() {
  int length = random(40, 60);
  
  int petals = random(3, 6); 
  int angle = random(50, 70);

  last_choice = !last_choice;
  EEPROM.write(1, last_choice);
  if(last_choice){// snowflake 
    blinkEyes(petals);
    length = 100;
    penDown();
    for (int petal = 0; petal < petals; petal++) {
      moveForward(length);
      turnRight(angle);
      moveForward(length);
      turnLeft(angle);
      moveBackward(length);
      turnRight(angle);
      moveBackward(length);
      turnRight(360/petals - angle);
    }
  }
  else{ // rose
    angle = 75;
    length = 200;
    // move away from center
    turnRight(45);
    moveBackward(length/2);
    turnLeft(45);
    
    // half size on first leg
    float size = length;
    moveForward(size * 0.25);
    penDown();
    moveForward(size * 0.75);
    turnRight(angle);
    size = size -5;
    
    // start the spiral
    while(size > 0){
      moveForward(size);
      turnRight(angle);
      size = size -5;
    }
  }
  
  penUp();
  moveForward(100); // move out of way 
  done();      // releases stepper motor
  while(1);    // wait for reset
}


// ----- HELPER FUNCTIONS -----------
int step(float distance){
  float steps = distance * steps_rev / (wheel_dia * 3.1412); //24.61
  return int(steps);  
}


void moveForward(float distance){
  move(distance, HIGH, LOW);
}


void moveBackward(float distance){
  move(distance, LOW, HIGH);
}


void turnRight(float degrees){
  float rotation = getNearestAngle(degrees) / 360.0;
  float distance = wheel_base * 3.1412 * rotation;
  move(distance, HIGH, HIGH);
}


void turnLeft(float degrees){
  float rotation = getNearestAngle(degrees) / 360.0;
  float distance = wheel_base * 3.1412 * rotation;
  move(distance, LOW, LOW);
}


void move(float distance, int dir1, int dir2){
  int steps = step(distance);
  float usDelay = (1/speed) * 70;

  digitalWrite(M1dirpin, dir1);
  digitalWrite(M2dirpin, dir2);
 
  for(int i=0; i < steps; i++){ 
    digitalWrite(M1steppin, HIGH);
    digitalWrite(M2steppin, HIGH); 
    delayMicroseconds(usDelay); 
    digitalWrite(M1steppin, LOW);
    digitalWrite(M2steppin, LOW);
    delayMicroseconds(usDelay); 
  } 

}


void penUp(){
  delay(250);
  penServo.write(PEN_UP);
  delay(250);
}


void penDown(){
  delay(250);  
  penServo.write(PEN_DOWN);
  delay(250);
}


void done(){
    digitalWrite(M1steppin, LOW);
    digitalWrite(M2steppin, LOW);
  pinMode(12, OUTPUT);
  pinMode(8, OUTPUT);
    digitalWrite(12, HIGH); // enable 2
    digitalWrite(8, HIGH); // enable  1
}


float getNearestAngle(float angle_){ 
  // code contributed by Instructable user PMPU_student
  /*
  Lets rotate by 58 degrees.
  turnRight(58); // rotate only by 57.631138392857146 degrees(that is the 
  value that is the closest to the desired one _and_ is lesser than it). 
  That means you are doing a certain amount of motor steps. But if you do 
  one more step during rotation you will rotate by 58.0078125 degrees, 
  which is much more precise(but it is greater than desired value, thats 
  why in original code it is not picked). The thing is that original code 
  always decides to rotate by value that is lesser than desired value while 
  it is sometimes worse than choosing the value that is bigger. 
  My code chooses from both variants, minimizing the error.
  */
  float angle = 0;
  float angle_error;
  int step = 0;
  float previousAngle = 0;
  float step_length = 3.1412 * wheel_dia / steps_rev;
  //angle_ += angle_error;
  while(!(previousAngle <= angle_ && angle_ <= angle)){
    step += 1;
    previousAngle = angle;
    angle = step * step_length * 360 / (wheel_base * 3.1412) + 0.01;
    }
  float dif1 = angle_- angle;
  float dif2 = angle_- previousAngle;
  if(abs(dif1) < abs(dif2)){
    angle_error = dif1;
    Serial.println(angle_error);
    return angle;
  }else{
    angle_error = dif2;
    Serial.println(angle_error);
    return previousAngle;
  }
}


/*
//////////////////////////////////////////////////////////////////
//©2011 bildr
//Released under the MIT License – Please reuse change and share
//Using the easy stepper with your arduino
//use rotate and/or rotateDeg to controll stepper motor
//speed is any number from .01 -> 1 with 1 being fastest –
//Slower Speed == Stronger movement
//http://bildr.org/2011/06/easydriver/

void rotate(int steps, float speed){
  //rotate a specific number of microsteps (8 microsteps per step) – (negitive for reverse movement)
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (steps > 0)? HIGH:LOW;
  steps = abs(steps);

  digitalWrite(DIR_PIN, dir);

  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){ 
    digitalWrite(STEP_PIN, HIGH); 
    delayMicroseconds(usDelay); 
    digitalWrite(STEP_PIN, LOW); 
    delayMicroseconds(usDelay); 
  } 
} 


void rotateDeg(float deg, float speed){ 
  //rotate a specific number of degrees (negitive for reverse movement) 
  //speed is any number from .01 -> 1 with 1 being fastest – Slower is stronger
  int dir = (deg > 0)? HIGH:LOW;
  digitalWrite(DIR_PIN, dir);

  int steps = abs(deg)*(1/0.225);
  float usDelay = (1/speed) * 70;

  for(int i=0; i < steps; i++){ 
    digitalWrite(STEP_PIN, HIGH); 
    delayMicroseconds(usDelay); 
    digitalWrite(STEP_PIN, LOW); 
    delayMicroseconds(usDelay); 
  } 
}

*/

void blinkEyes(int blinks){
  for(int x=0; x<blinks; x++){
    digitalWrite(LED, HIGH);
    delay(200);
    digitalWrite(LED, LOW);
    delay(200);
  } 
}

Автор: MakersBox (Ken)
Перевод и адаптация: RobotoTehnika.ru