Начал свой проект на 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);
}
//==============================================================================
