CatHunter

CatHunter

Сообщение NIch » 08 сен 2013, 12:13

Всем доброго времени суток!

Начал свой проект на Arduino Uno, название незамысловатое CatHunter, задумка не давать моему ленивому коте просто так спать:-)...

Итак комплектующие:
1. Шасси: двухколёсная платформа Turtle
2. Ардуино Uno
3. Motor Shield
4. Troyka Shield
5. Инфракрасный дальномер Sharp (10-80 см)
6. 3 шт. - инфракрасный датчик препятствий
7. Микросервопривод FS90
8. Котэ и немного фантазии...

Ну, и результат 4-х дневных экспериментов:


Естественно это только начало работы, будут улучшение, появятся и новые записи...

Исходный код(естественно его бетта версия, т.е. проект далеко не закончен):
Код: Выделить всё
//==============================================================================
// Программа управления 2-х колесной тележкой "CatHunter"
//==============================================================================
// Моторы подключаются к клеммам M1+,M1-,M2+,M2- 
// Motor shield использует четыре контакта 6,5,7,4 для управления моторами
#define SPEED_LEFT     6
#define SPEED_RIGHT    5
#define DIR_LEFT      7
#define DIR_RIGHT      4
// Типы движения
#define STOP         0
#define FORWARD      1
#define BACKWARD      2
#define LEFT         3
#define RIGHT        4
// Маневр
#define FORWARD_LEFT   5
#define FORVARD_RIGHT  6
#define TURN_LEFT     7
#define TURN_RIGHT    8
//Текущее движение
int CurMove = FORWARD;
float ScaleSpeed = 1.5;
// Датчики
#define RADAR        A5
#define EYE_L        11
#define EYE_B        10
#define EYE_R        9
// Серво
#include <Servo.h>
#define SERVO        2
Servo servo;
int ServoPos;
int ServoDir = LEFT;
// Радар
int Radar;
int Eye_L;
int Eye_B;
int Eye_R;
//==============================================================================
// Движения тележки
void MoveCar(){
  switch(CurMove){
   case STOP:        {Stop();             break;}
   case FORWARD:      {Go(75, 75, FORWARD);  break;}
   case BACKWARD:     {Go(75, 75, BACKWARD); break;}
   case LEFT:        {Turn(50, LEFT);       break;} 
   case RIGHT:       {Turn(50, RIGHT);      break;}
   case FORWARD_LEFT:  {ForvardLeft();        break;}
   case FORVARD_RIGHT: {ForvardRight();       break;}
   case TURN_LEFT:    {TurnLeft();          break;}
   case TURN_RIGHT:   {TurnRight();         break;}
  }
}
void Go(int speed_L, int speed_R, int direction){
  analogWrite(SPEED_LEFT, speed_L  * ScaleSpeed);
  analogWrite(SPEED_RIGHT, speed_R * ScaleSpeed);
  digitalWrite(DIR_LEFT, direction ? LOW : HIGH);
  digitalWrite(DIR_RIGHT, direction ? LOW : HIGH);
}
void Turn(int speed, int direction){
  analogWrite(SPEED_LEFT, speed  * ScaleSpeed);
  analogWrite(SPEED_RIGHT, speed * ScaleSpeed);
  digitalWrite(DIR_LEFT, direction ? LOW : HIGH);
  digitalWrite(DIR_RIGHT, direction ? HIGH : LOW);
}
void Stop(){
  analogWrite(SPEED_LEFT,  0);
  analogWrite(SPEED_RIGHT, 0);
}
void ForvardLeft(){
  Go(75, 50, FORWARD);
}
void ForvardRight(){
  Go(50, 75, FORWARD);
}
void TurnLeft(){
  Turn(50, LEFT);
}
void TurnRight(){
  Turn(50, RIGHT);
}
//==============================================================================
// Движение радара
void MoveRadar(){   
  // Пишем в порт радара 
  servo.write(ServoPos);
 
  // Движение влево
  if (ServoDir == LEFT){
   //ServoPos += 25;
   ServoPos++;
   if (ServoPos >= 155){
     ServoDir = RIGHT;
   }
  }
  // Движение вправо
  else{
   //ServoPos -= 25;
   ServoPos--;
   if (ServoPos <= 35){
     ServoDir = LEFT;
   }
  }
 
  // Немного замедляем цикл
  delay(10); 
}
//==============================================================================
// Основные расчеты движения тележки
void CalcDir(){
  // Изначально предполагаем, что едем вперед
  CurMove = FORWARD; 
 
  // 4 датчика, каждый фиксирует наличие препятствия
  // Radar к тому же фиксирует расстояние до препятствия
   
  // Если датчик зафиксировал препятствие(для радара меньше допустимого)
  // Каждый друг друга исключает
  if     (Eye_B == 0){
   CurMove = FORWARD; 
  }
  else if (Eye_R == 0){
   CurMove = FORWARD_LEFT;
  }
  else if (Eye_L == 0){
   CurMove = FORVARD_RIGHT;
  }
  else if (Radar >= 300){
   CurMove = TURN_LEFT;
  }
}
//==============================================================================
void LogMove(){
  //Serial.println(Radar); 
  //Serial.println(Eye_L);
  //Serial.println(Eye_B);
  //Serial.println(Eye_R);
 
  switch(CurMove){
   case STOP:        {Serial.println("STOP");        break;}
   case FORWARD:      {Serial.println("FORWARD");      break;}
   case BACKWARD:     {Serial.println("BACKWARD");     break;}
   case LEFT:        {Serial.println("LEFT");        break;}
   case RIGHT:       {Serial.println("RIGHT");       break;}
   case FORWARD_LEFT:  {Serial.println("FORWARD_LEFT");  break;}
   case FORVARD_RIGHT: {Serial.println("FORVARD_RIGHT"); break;}
   case TURN_LEFT:    {Serial.println("TURN_LEFT");    break;}
   case TURN_RIGHT:   {Serial.println("TURN_RIGHT");   break;}
  } 
 
  Serial.println("----------"); 
}
//==============================================================================
void setup(){
  // Выходы двигателей
  for(int i = 4; i <= 7; i++)   
   pinMode(i, OUTPUT);
   
  // Входы сенсоров
  for(int i = 9; i <= 11; i++)   
   pinMode(i, INPUT);
   
  // Подключаемся к серво 
  servo.attach(SERVO);
  // Выставить серво на 0
  servo.write(95);
 
  // Настраиваем порт для отладки 
  Serial.begin(9600);   
 
  // Пауза перед запуском
  delay(3000);
}
//==============================================================================
void loop(){
  // Движение серво радара
  MoveRadar();
  // Движение тележки
  MoveCar();
 
  // Датчик радара
  Radar = analogRead(RADAR);   
  // Датчики препятствий
  Eye_L = digitalRead(EYE_L);
  Eye_B = digitalRead(EYE_B);
  Eye_R = digitalRead(EYE_R);
   
  // Вычисляем движение тележки в зависимости от датчиков
  CalcDir();
 
  // Логируем состояние
  //LogMove(); 
  //delay(250);
}
//==============================================================================
NIch
 
Сообщения: 1
Зарегистрирован: 08 сен 2013, 12:08
programming: Delphi, C++, JS

Re: CatHunter

Сообщение admin » 09 сен 2013, 07:14

Отличная работа! :bra_vo:
Аватара пользователя
admin
Администратор
 
Сообщения: 305
Зарегистрирован: 05 май 2011, 14:57
Откуда: Калининград


Вернуться в неРоботы

Кто сейчас на конференции

Сейчас этот форум просматривают: нет зарегистрированных пользователей и гости: 1

cron
© 2009-2017 |  О проекте  |  Политика Конфиденциальности  |