|
Pozdrav, potrebna mi je pomoc oko pisanja algoritma za vec napisani kod, pa ako je neko dobre volje da mi izadje u sustret bio bih mu veoma zahvalan. Unapre hvala! Evo i koda
#include <AFMotor.h>
#include <NewPing.h>
#include <Servo.h>
#include <DistanceGP2Y0A21YK.h>
#define TRIG_PIN A4
#define ECHO_PIN A5
#define MAX_DISTANCE 200
#define MAX_SPEED 190 // sets speed of DC motors
#define MAX_SPEED_OFFSET 20
NewPing sonar(TRIG_PIN, ECHO_PIN, MAX_DISTANCE);
AF_DCMotor motor1(1, MOTOR12_1KHZ);
AF_DCMotor motor2(3, MOTOR12_1KHZ);
Servo myservo;
boolean goesForward=false;
int distance = 100;
int speedSet = 0;
DistanceGP2Y0A21YK Dist;
void setup() {
myservo.attach(9);
myservo.write(115);
Serial.begin(9600); //Enable the serial comunication
Dist.begin(A5);
distance = Dist.getDistanceCentimeter();
delay(2000);
//distance = readPing();
distance = Dist.getDistanceCentimeter();
delay(100);
//distance = readPing();
distance = Dist.getDistanceCentimeter();
delay(100);
//distance = readPing();
distance = Dist.getDistanceCentimeter();
delay(100);
//distance = readPing();
distance = Dist.getDistanceCentimeter();
delay(100);
}
void loop() {
int distanceR = 0;
int distanceL = 0;
delay(40);
//distance = 25;
if(distance<=8)
{
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 = distance = Dist.getDistanceCentimeter();
}
int lookRight()
{
myservo.write(50);
delay(500);
//int distance = readPing();
distance = Dist.getDistanceCentimeter();
delay(100);
myservo.write(115);
return distance;
}
int lookLeft()
{
myservo.write(170);
delay(500);
//int distance = readPing();
distance = Dist.getDistanceCentimeter();
delay(100);
myservo.write(115);
return distance;
delay(100);
}
int readPing() {
delay(70);
int cm = sonar.ping_cm();
if(cm==0)
{
cm = 250;
}
return cm;
}
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() {
motor1.run(FORWARD);
motor2.run(BACKWARD);
delay(300);
motor1.run(FORWARD);
motor2.run(FORWARD);
}
void turnLeft() {
motor1.run(BACKWARD);
motor2.run(FORWARD);
delay(300);
motor1.run(FORWARD);
motor2.run(FORWARD);
}
|
