#include <AFMotor.h>
#include <NewPing.h>
#include <Servo.h>
#define TRIG_PIN A14
#define ECHO_PIN A15
#define MAX_DISTANCE 200
#define MAX_SPEED 235 // sets speed of DC motors
#define MAX_SPEED_OFFSET 20
AF_DCMotor motor1(1);
AF_DCMotor motor2(4);
Servo myservo;
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
boolean goesForward=false;
int distance = 100;
int speedSet = 0;
char command;
const int onfar = 13;
int dugme=1;
void setup()
{
pinMode(onfar, OUTPUT);
myservo.attach(9);
myservo.write(115);
int readPing();
delay(2000);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
distance = readPing();
delay(100);
Serial.begin(9600); //Set the baud rate to your Bluetooth module.
}
int readPing() {
delay(70);
int cm = sonar.ping_cm();
if(cm==0)
{
cm = 250;
}
return cm;
}
void forward()
{
motor1.setSpeed(255); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(255); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
}
void back()
{
motor1.setSpeed(255);
motor1.run(BACKWARD); //rotate the motor counterclockwise
motor2.setSpeed(255);
motor2.run(BACKWARD); //rotate the motor counterclockwise
}
void left()
{
motor1.setSpeed(255); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(0);
motor2.run(RELEASE); //turn motor2 off
}
void right()
{
motor1.setSpeed(0);
motor1.run(RELEASE); //turn motor1 off
motor2.setSpeed(255); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
}
void onsol()
{
motor1.setSpeed(255); //Define maximum velocity
motor1.run(FORWARD); //rotate the motor clockwise
motor2.setSpeed(125); //Define maximum velocity
motor2.run(FORWARD); //rotate the motor clockwise
}
void onsag()
{
motor1.setSpeed(125);
motor1.run(FORWARD); //rotate the motor counterclockwise
motor2.setSpeed(255);
motor2.run(FORWARD); //rotate the motor counterclockwise
}
void arkasol()
{
motor1.setSpeed(125); //Define maximum velocity
motor1.run(BACKWARD); //rotate the motor clockwise
motor2.setSpeed(255);
motor2.run(BACKWARD); //turn motor2 off
}
void arkasag()
{
motor1.setSpeed(255);
motor1.run(BACKWARD); //turn motor1 off
motor2.setSpeed(125); //Define maximum velocity
motor2.run(BACKWARD); //rotate the motor clockwise
}
void Stop()
{
motor1.setSpeed(0);
motor2.run(RELEASE); //turn motor1 off
motor2.setSpeed(0);
motor2.run(RELEASE); //turn motor2 off
}
void otokapa()
{
dugme=0;
motor1.setSpeed(0);
motor2.run(RELEASE); //turn motor1 off
motor2.setSpeed(0);
motor2.run(RELEASE); //turn motor2 off
}
void onac()
{
myservo.write(90);
}
void onkapa()
{
myservo.write(0);
}
int lookRight()
{
myservo.write(50);
delay(500);
int distance = readPing();
delay(100);
myservo.write(115);
return distance;
}
int lookLeft()
{
myservo.write(170);
delay(500);
int distance = readPing();
delay(100);
myservo.write(115);
return distance;
delay(100);
}
void moveStop() {
motor1.run(RELEASE);
motor2.run(RELEASE);
}
void moveForward() {
if(!goesForward)
{
goesForward=true;
motor1.run(FORWARD);
motor2.run(FORWARD);
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet);
motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(5);
}
}
}
void moveBackward() {
goesForward=false;
motor1.run(BACKWARD);
motor2.run(BACKWARD);
for (speedSet = 0; speedSet < MAX_SPEED; speedSet +=2) // slowly bring the speed up to avoid loading down the batteries too quickly
{
motor1.setSpeed(speedSet);
motor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
delay(5);
}
}
void turnRight() {
motor2.run(FORWARD);
motor1.run(BACKWARD);
delay(300);
motor2.run(FORWARD);
motor1.run(FORWARD);
}
void turnLeft() {
motor2.run(BACKWARD);
motor1.run(FORWARD);
delay(300);
motor2.run(FORWARD);
motor1.run(FORWARD);
}
void otoac()
{
long duration, distance;
int distanceR = 0;
int distanceL = 0;
delay(40);
if(distance<=24)
{
moveStop();
delay(100);
moveBackward();
delay(300);
moveStop();
delay(200);
distanceR = lookRight();
delay(200);
distanceL = lookLeft();
delay(200);
if(distanceR>=distanceL)
{
turnRight();
moveStop();
}else
{
turnLeft();
moveStop();
}
}else
{
moveForward();
}
distance = readPing();
}
void loop(){
if(Serial.available() > 0){
command = Serial.read();
Stop();
switch(command){
case 'F':
forward();
break;
case 'B':
back();
break;
case 'L':
left();
break;
case 'R':
right();
break;
case 'G':
onsol();
break;
case 'I':
onsag();
break;
case 'H':
arkasag();
break;
case 'J':
arkasol();
break;
case 'W':
onac();
break;
case 'w':
onkapa();
break;
case 'X':
otoac();
break;
case 'x':
otokapa();
break;
}
}
}
Comments
Please log in or sign up to comment.