/**

 Bruno Santos, 2013

 feiticeir0@whatgeek.com.pt

 Small code to test DC motors - 2x with a L298 Dual H-Bridge Motor Driver

Kimmo ja Pauli muokkasivat 2019

Free to share

**/

#define TRIGGER_PIN  10 // Arduino pin tied to trigger pin on the ultrasonic sensor.

#define ECHO_PIN 11  // Arduino pin tied to echo pin on the ultrasonic sensor.

#define MAX_DISTANCE 300 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.

#include <NewPing.h>

#include <Servo.h>

Servo myservo;

int pos = 0;

NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.

int distance = 2;

//Testing the DC Motors

//Define Pins

//Motor A

int enableA = 3;

int pinA1 = 4;

int pinA2 = 5;

//Motor B

int enableB = 8;

int pinB1 = 6;

int pinB2 = 7;

void setup() {

 Serial.begin (9600);

 //configure pin modes

 pinMode (enableA, OUTPUT);

 pinMode (pinA1, OUTPUT);

 pinMode (pinA2, OUTPUT);

 pinMode (enableB, OUTPUT);

 pinMode (pinB1, OUTPUT);

 pinMode (pinB2, OUTPUT);

 myservo.attach(9);

}

void forward2() {

 //enabling motor A

 Serial.println ("Enabling Motor A");

 digitalWrite (enableA, HIGH);

 digitalWrite (enableB, HIGH);

 //do something

 //forward

 Serial.println ("Forward");

 digitalWrite (pinA2, HIGH);

 digitalWrite (pinA1, LOW);

 digitalWrite (pinB1, HIGH);

 digitalWrite (pinB2, LOW);

 delay (2000);

 digitalWrite (pinA2, LOW);

 digitalWrite (pinA1, LOW);

 digitalWrite (pinB1, LOW);

 digitalWrite (pinB2, LOW);

 delay (1000);

 digitalWrite (pinA1, HIGH);

 digitalWrite (pinA2, LOW);

 digitalWrite (pinB1, HIGH);

 digitalWrite (pinB2, LOW);

 delay (1000);

 digitalWrite (pinA2, LOW);

 digitalWrite (pinA1, LOW);

 digitalWrite (pinB1, LOW);

 digitalWrite (pinB2, LOW);

delay(1000);

 

 digitalWrite (pinA2, HIGH);

 digitalWrite (pinA1, LOW);

 digitalWrite (pinB1, HIGH);

 digitalWrite (pinB2, LOW);

 delay (2000);

digitalWrite (enableB, LOW);

digitalWrite (enableA, LOW);

}

void forward() {

 //enabling motor A

 Serial.println ("Enabling Motor A+B");

 digitalWrite (enableA, HIGH);

 digitalWrite (enableB, HIGH);

 //do something

 //forward

 Serial.println ("Forward");

 digitalWrite (pinA2, HIGH);

 digitalWrite (pinA1, LOW);

 digitalWrite (pinB1, HIGH);

 digitalWrite (pinB2, LOW);

}

void stop() {

 Serial.println ("Stop");

 digitalWrite (pinA2, LOW);

 digitalWrite (pinA1, LOW);

 digitalWrite (pinB1, LOW);

 digitalWrite (pinB2, LOW);

 delay (200);

 digitalWrite (enableB, LOW);

 digitalWrite (enableA, LOW);

}

void vaihda_suunta() {

 int oikea=0;

 int vasen=0;

 pos=10;

 myservo.write(pos);   // tell servo to go to position in variable 'pos'

 delay(500);

 vasen = sonar.ping_cm();

 delay(15);

 pos = 170;

 myservo.write(pos);   // tell servo to go to position in variable 'pos'

 delay(500);

 oikea = sonar.ping_cm();

 delay(15);

 pos = 90;

 myservo.write(pos);   // tell servo to go to position in variable 'pos'

 delay(500);

 if (oikea == 0) oikea = 500;

 if (vasen == 0) vasen = 500;

 if (oikea < vasen) {

Serial.println ("Käännyn Oikealle!");

Serial.println (oikea);

delay(1000);

 }

 else {

 Serial.println ("Käännyn Vasemmalle");

 Serial.println (vasen);

delay(1000);

 }

}

void loop() {

 distance = sonar.ping_cm();

 Serial.println(distance);

 if ((distance < 30) && (distance > 2) ){

  stop();

  vaihda_suunta();

} else {

  //forward();

}

 delay (50);

}