Проект 8 сумобот для соревнований в МГУПИ 24.11.2012

Ссылки на информацию о комплектующих робота проекта 8:

http://www.meanpc.com/2012/01/how-to-use-tb6612fng-motor-driver-with.html статья по драйверу двигателя pololu Tb6612fng

http://www.pololu.com/catalog/product/136 датчик дистанции шарп GP2Y0A21 информация http://www.pololu.com/file/0J85/gp2y0a21yk0f.pdf брошюра http://robot-develop.org/archives/1043 использование инфракрасного датчика с ардуино http:

//www.domko.ru/pdf/pdf/d/dfr0010.pdf плата dfrduino nano

http://www.dfrobot.com/wiki/index.php?title=Arduino_Nano_328_(SKU:_DFR0010) Вики по dfrduino http://robocraft.ru/blog/arduino/245.html использование серво на ардуино 10 и 11 пины не использовать для pwm http://roboforum.ru/forum68/topic9969.html - форум, по тематике где  и как лучше использовать ИК датчики дистанции и УЗ дальномеры

Расчет необходимого количества пинов ардуино

7 цифровых для управления драйвером двигателей из них 2 pwm.

2 цифровых для сервоприводов

4 аналоговых для qtr1a – датчиков черного и белого.

4 аналоговых для датчиков дистанции шарп

2012-10-29 прежде всего необходимо проверить будет ли работать отвал (ы) под управлением сервоприводов, как предполагается на схеме ( corel draw). Затем собрать на макетке.

2012-11-13 собран робот, начаты испытания. первые результаты проверок надежности – необходимо переклеить сервоприводы на клей вместо термопистолета – не держит. Проблема с правым колесом – не выдержала гайка внутри колеса. также посадил на клей.

2012-11-14 проект собран – начались ходовые и прочностные испытания

2012-11-15 проект прошел цикл ходовых испытаний с роботом «пятерочка» из за разницы в весе не в его пользу, не всегда получалось завершать бои победами. колеса, прошедшие небольшую модернизацию, работают. Тем не менее сегодня изготовлено колесо по новой технологии, когда в качестве элемента крепежа использован зажим для проводов. основной задачей является догрузка робота до разрешенной массы 500 грамм и проведение испытаний в таком режиме. Предположительно будут  проблемы в отвалах на местах стыка с корпусом и на месте клеевого крепления тяги. пути решения – пайка и усиление прочности шарниров  пропайкой также.

2012-11-16 продолжилось нагрузочное тестирование и отладка программного кода. В частности была выявлена ошибка кода, связанная с не чтением данных заднего левого датчика черного и белого. Вместо него опрашивался его правый сосед дважды. Сделано новое колесо с улучшенной посадкой на ось мотора. Проведено тестирование ИК датчиков. Найден интересный факт – при фронтальном расположении датчика перед зеркалом он показывал расстояние свыше 30 метров. Фольга имеет такой-же коэффициент отражения, как и рука, например… задача утяжеления осталась….

2012-11-18 Работы по тестированию и доделке практически закончены. работает стабильная версия 1.3. проводятся работы по интеграции TSOP1736 – ик датчика для включения робота дистанционно. пока есть проблемы с распознаванием команды от пульта… похоже на какие-то конфликты софта. Доделана зарядка LiPo батареи для моторов без разбора робота. Кроме того изготовлены 2 комплекта шин ( более мягкие и жесткие из различного силикона (15 и 40).

2012-11-19 не заработал ик датчик tsop1732 – для работы библиотеки irsensor.h надо pwm на 11 пине для таймера. А он занят pwm ом для драйвера моторов. Пришлось исключить напрочь… Появилась новая проблема – зависание двигателей после некоторого времени работы в разных режимах. Заметил, что основное зависание – после включения максимального ускорения вперед или назад, также после качаниялопасти отвала (хотя это может быть ускорение, конечно, как как отвалы спереди и сзади). Робот после такого зависания, даже после полного отключения питания не включает двигаели – так что может быть проблема с драйвером двигателей. Проверил емкость питающей двигатели батареи, перезарядил ее – проблемы не исчезли. Пути изучения проблемы в режиме «мозгового штурма»: 1. закомментировать вывод в порт данных (маловероятно) 2. снизить мощность PWM c 255 максимальных – для проверки причины… 3. проверить вывод 4 ( который трчит между моторами – может вормируется какой-либо потенциал от вращения двигателей на нем и отключает плату (маловероятно) 4. проверить в каком режиме работает Arduino при зависании драйвера двигателей и какой потенциал куда подается… 5. заменить драйвер двигателей на новый… 6. заменить ардуино нано на новую… вероятной причиной проблем является подача на 11 шим сигнала который генерировала библиотека IRsensor.h  – может какие-то проблемы начались у драйвера… Также пайка для подключения датчика IR – пересобрать плату заново…

2012-11-21 01-00 ночь. проблемы с зависанием вылечились установкой значения PWM на 230. не 255. добавил вторую кнопку включения робота на 4 пин. первая кнопка включает питание. вторая запускает робота на исполнение программы. вместо TSOP1736 и пульта… что доделать – волнует крепление серво и тяг серво к отвалам – на моменте всего лишь – хотя пока претензий нет… также желательно еще по два шарпа в каждом направлении – для стереозрения, так сказать… но это в следующем проекте, вероятно…

2012-11-21 вечер. Оптимизирован код программы управления и доделан верхний груз. Общий вес робота 497 грамм.

Схема размещения компонентов

схема печатной платы

скетч программы управления для Ардуино:

/*Проект сумо-робот 8_1_3_1 SURKOV TEAM 2012
сумо робот ищет противника поворачиваясь направо, при обнаружении атакует прямо при длостижении белой краевой полосы отъезжает назад.
Последняя рабочая версия, модернизирована кнопкой включения(чтобы не было движения отвала (сервы).
*/

#include <Servo.h> // библиотека работы с сервомоторам

// переменные для моторов
#define PWMA 11
#define AIN1 9
#define AIN2 10
#define BIN1 7
#define BIN2 6
#define PWMB 5
#define STBY 8
#define motor_A 0
#define motor_B 1
#define MFORWARD 1
#define MREVERSE 0
#define MRIGHT 1
#define MLEFT 0

//QTR1A
#define NUM_SENSORS             4  // число используемых сенсоров черного и белого (по краям)
#define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading

// QTR1A - сенсоры подключены к аналоговым выводам 7,6,5,4 ардуино нано
QTRSensorsAnalog qtra((unsigned char[]) {7, 6, 5, 4}, NUM_SENSORS, NUM_SAMPLES_PER_SENSOR);
unsigned int sensorValues[NUM_SENSORS];

// сервомоторы
Servo srvo_front;  // создаем объект серво для переднего сервомотора
int pos_srvo_front = 19;    // переменная для хранения позиции переднего сервомотора
Servo srvo_back;  // создаем объект серво для переднего сервомотора
int pos_srvo_back = 15;    // переменная для хранения позиции переднего сервомотора

//моторы pololu через драйвер TB6612FNG
//const int pwmA = 11;
//const int Ain2 = 10;
//const int Ain1 = 9;
//const int stdby = 8;
//const int Bin1 = 7;
//const int Bin2 = 6;
//const int pwmB = 5;
const int valDelay = 50; //задержка в милисекундах для моторов (частота цикла loop без событий)
                         //не желательно делать меньше 10 (оптимально 30-70) для невылетов за пределы поля и пр.

// сервомоторы опускания отвалов
const int front_srvo = 13;
const int back_srvo = 12;

// датчики ИК - SHARP_ GP2Y0A21YK0F подключены к аналоговым портам
const int front_sharp = 0;
const int back_sharp = 1; 

// другие переменные
const int distance_long = 45; // максимальная дистанция для поиска цели для этого датчика шарп и алгоритма расчета дистанции
const int distance_short = 6; // минимальная дистанция для цели (дистанция остановки перед целью)

const int wb_border_int = 600; //граница различения белого и черного - черный выше, белый ниже этой границы (от 0 до 1023)

float cm_front;                // переменная для хранения дистанции в сантиметрах c переднего датчика
float cm_back;                // переменная для хранения дистанции в сантиметрах c заднего датчика

int switch_var = 0; //переменная для учета оборотов на на 360 градусов (100 примерно 360 градусов)
int target_var = 0; // переменная для более точного отслеживания цели

const int buttonPin = 4;     // номер входа, подключенный к кнопке
int buttonFlag = 0; // переменная хранящая значение нажатия кнопки - 0 - не нажата - 1 - нажата
int buttonState = 0; // переменная для хранения значения есть ли напряжение на 4 пине

void setup()
{
  //Serial.begin(9600);       //запускаем монитор порта для контроля вывода результатов
  pinMode(PWMA,OUTPUT);
  pinMode(AIN1,OUTPUT);
  pinMode(AIN2,OUTPUT);
  pinMode(PWMB,OUTPUT);
  pinMode(BIN1,OUTPUT);
  pinMode(BIN2,OUTPUT);
  pinMode(STBY,OUTPUT);
  motor_standby(false);

  pinMode(front_srvo, OUTPUT); //сервоприводы
  pinMode(back_srvo, OUTPUT);
  pinMode(front_sharp, INPUT); // датчики дистанции
  pinMode(back_sharp, INPUT);

  // инициализируем пин, подключенный к кнопке, как вход
  pinMode(buttonPin, INPUT); 

} 

void loop()
{ 

  // считываем значения с входа кнопки
  buttonState = digitalRead(buttonPin);

  /*
  Serial.print("buttonState=");
  Serial.print(buttonState);
  Serial.print(" ButtonFlag=");
  Serial.println(buttonFlag);
  delay(1000);
  */

  // проверяем нажата ли кнопка и есть ли сигнал высокого напряжения на 4 пине
  // если нажата, то buttonState будет HIGH:
  if ((buttonState == HIGH) && (buttonFlag == 0)) {  // и кнопка до этого была не нажата
    // выполняем программу танцев в ожидании 5 секунд
    delay(1900); // задержка на 1,9 секунды
  srvo_front.attach(13);        // 13 пин управляет передним сервомотором
  srvo_front.write(pos_srvo_front);
  srvo_back.attach(12);        // 12 пин управляет передним сервомотором
  srvo_back.write(pos_srvo_back);
  delay(2500); //задержка на 2.5 секунды по правилам соревнований
  srvo_front.write(pos_srvo_front-10);
  srvo_back.write(pos_srvo_back+15);
  delay(100);
  srvo_front.write(pos_srvo_front);
  srvo_back.write(pos_srvo_back);
  delay(100);
  srvo_front.write(pos_srvo_front+10);
  srvo_back.write(pos_srvo_back-15);
   delay(100);
  srvo_front.write(pos_srvo_front);
  srvo_back.write(pos_srvo_back);
   delay(100);
  srvo_front.write(pos_srvo_front-10);
  srvo_back.write(pos_srvo_back+15);
  delay(100);
  srvo_front.write(pos_srvo_front);
  srvo_back.write(pos_srvo_back);
  delay(100);

  buttonFlag = 1;} // кнопка нажата 

  if ((buttonState == LOW) && (buttonFlag == 1)){ //отжали кнопку и уровень сигнала упал а робот двигался
    // определяем новое значение признака нажатия кнопки
    buttonFlag = 0;// кнопка не нажата
    motor_brake(); // моторы останавливаем
    srvo_front.write(160); // возвращаем отвалы вверх и отключаем сервы
    srvo_back.write(160);
    delay(1000);
    srvo_front.detach();
    srvo_back.detach();
  } 

  // собственно программа

  if (buttonFlag == 1) {// выполняем все действия при условии, что кнопка запуска нажата

  qtra.read(sensorValues);              //читаем данные датчиков черного_белого и помещаем их в массив по количеству датчиков

  /*
  Serial.print(sensorValues[0]); //задний правый
  Serial.print("  ");
  Serial.print(sensorValues[1]); // задний левый
  Serial.print("  ");
  Serial.print(sensorValues[2]); // передний левый
  Serial.print("  ");
  Serial.println(sensorValues[3]); // передний правый
  */

  // проверяем, что робот находится на черном поле
  if((sensorValues [3] > wb_border_int) && (sensorValues [2] > wb_border_int)){ // передний правый и левый сенсор на черном поле

   if((sensorValues [0] > wb_border_int) && (sensorValues [1] > wb_border_int)){ // задний правый и левый сенсор на черном поле

    //cm_front = 102400/(analogRead(front_sharp)*25-1000);     // измеряем расстояние до объекта спереди
    //cm_back = 102400/(analogRead(back_sharp)*25-1000);     // измеряем расстояние до объекта сзади

    //cm_front = 12343.85*pow(analogRead(front_sharp),-1.15);     // измеряем расстояние до объекта спереди
    //cm_back = 12343.85*pow(analogRead(back_sharp),-1.15);     // измеряем расстояние до объекта сзади

    cm_front = avr_IR_cm(front_sharp);
    cm_back = avr_IR_cm(back_sharp);
  /*
  Serial.print(" dist_front ");
  Serial.print(cm_front);
  Serial.print(" dist_back ");
  Serial.println(cm_back);
  */

    if ((cm_front > distance_long) && (cm_back > distance_long)) { // противник не найден на заданном расстоянии
     if (switch_var <=13){// вправо (40 - испытанное значение)
     motor_turn(MRIGHT, 70, 70); //50 - опробованная скорость
     delay(valDelay);
     target_var = 0;}
     if ((switch_var > 13) && (switch_var <=26)){// влево
     motor_turn(MLEFT, 70, 70);
     delay(valDelay);
     target_var = 0;}
     switch_var = switch_var+1;

     if (switch_var > 26) { switch_var = 0;}
    } //конец условия if ((cm_front > distance_long) && (cm_back > distance_long)
    else {
    /*
    Serial.print(" dist_front ");
    Serial.print(cm_front);
    Serial.print(" dist_back ");
    Serial.println(cm_back);
    */
    }

    if (cm_front <= cm_back){ // определяем направление движения - вперед

  // едем прямо к препятствию, которое расположено ближе distance_long
  // вперед
   if ((cm_front <= distance_long) && (cm_front > distance_short)){
     target_var = 1; // цель обнаружена
   if ((cm_front <= 15) && (cm_front > distance_short)){ //толкаем соперника - включаем форсаж
   motor_drive(MFORWARD, 230);   //Move FORWARD at full speed
   delay(valDelay);}
   else { // просто преследуем соперника
   motor_drive(MFORWARD, 200);   //Move FORWARD
   delay(valDelay);}
   }
   } // конец условия if (cm_front <= cm_back)
   else { // если объект обнаружен сзади

   // едем назад к препятствию, которое расположено ближе distance_long
   if ((cm_back <= distance_long) && (cm_back > distance_short)){
      target_var = 1; // цель обнаружена
   if ((cm_back <= 15) && (cm_back > distance_short)){ //толкаем соперника - включаем форсаж
   motor_drive(MREVERSE, 230);   //на всех парах
   delay(valDelay);}
   else { // просто преследуем соперника
   motor_drive(MREVERSE, 200);   //чуть помедленнее кони
   delay(valDelay);}
   }
   } // конец условия else if (cm_front >= cm_back) 

   } // конец условия if((sensorValues [0] > wb_border_int) && (sensorValues [0] > wb_border_int))
   else { // если задние датчики выкатились на белое поле
   motor_drive(MFORWARD,150);
   delay(valDelay*3);
   }
   } // конец условия  if((sensorValues [3] > wb_border_int) && (sensorValues [2] > wb_border_int)) 

   else { // если передние датчики выкатились на белое поле

   motor_drive(MREVERSE,150);
   delay(valDelay*3);
   }// конец условия  else if((sensorValues [3] > wb_border_int) && (sensorValues [2] > wb_border_int)) 

  } //  конец buttonFlag = 1
 } // конец loop()

float avr_IR_cm(int ir_port){
 long total = 0;
 for (int i=0; i<=2; i++){ total  = total + analogRead(ir_port);}

 return  12343.85*pow(total/3,-1.15);
}  

//Turns off the outputs of the Motor Driver when true
void motor_standby(char standby)
{
  if (standby == true) digitalWrite(STBY,LOW);
  else digitalWrite(STBY,HIGH);
}

//Stops the motors from spinning and locks the wheels
void motor_brake()
{
  digitalWrite(AIN1,1);
  digitalWrite(AIN2,1);
  digitalWrite(PWMA,LOW);
  digitalWrite(BIN1,1);
  digitalWrite(BIN2,1);
  digitalWrite(PWMB,LOW);
} 

//Controls the direction the motors turn, speed from 0(off) to 255(full speed)
void motor_drive(char direction, unsigned char speed)
{
  if (direction == MFORWARD)
  {
    motor_control(motor_A, MFORWARD, speed);
    motor_control(motor_B, MFORWARD, speed);
  }
  else
  {
    motor_control(motor_A, MREVERSE, speed);
    motor_control(motor_B, MREVERSE, speed);
  }
}

//You can control the turn radius by specifying the speed of each motor
//Set both to 255 for it to spin in place
void motor_turn(char direction, unsigned char speed_A, unsigned char speed_B )
{
  if (direction == MRIGHT)
  {
    motor_control(motor_A, MREVERSE, speed_A);
    motor_control(motor_B, MFORWARD, speed_B);
  }
  else
  {
    motor_control(motor_A, MFORWARD, speed_A);
    motor_control(motor_B, MREVERSE, speed_B);
  }
}

void motor_control(char motor, char direction, unsigned char speed)
{
  if (motor == motor_A)
  {
    if (direction == MFORWARD)
    {
      digitalWrite(AIN1,LOW);
      digitalWrite(AIN2,HIGH);
    }
    else
    {
      digitalWrite(AIN1,HIGH);
      digitalWrite(AIN2,LOW);
    }
    analogWrite(PWMA,speed);
  }
  else
  {
    if (direction == MFORWARD)  //Notice how the direction is reversed for motor_B
    {                          //This is because they are placed on opposite sides so
      digitalWrite(BIN1,HIGH);  //to go FORWARD, motor_A spins CW and motor_B spins CCW
      digitalWrite(BIN2,LOW);
    }
    else
    {
      digitalWrite(BIN1,LOW);
      digitalWrite(BIN2,HIGH);
    }
    analogWrite(PWMB,speed);
  }
}