You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
323 lines
11 KiB
323 lines
11 KiB
#include <Wire.h>
|
|
#include <Adafruit_MotorShield.h>
|
|
#include "utility/Adafruit_MS_PWMServoDriver.h"
|
|
|
|
//define logic control output pin
|
|
#define trigPin1 13
|
|
#define echoPin1 12
|
|
|
|
#define trigPin2 A3
|
|
#define echoPin2 A2
|
|
|
|
#define trigPin3 7
|
|
#define echoPin3 6
|
|
|
|
#define trigPin4 9
|
|
#define echoPin4 8
|
|
|
|
#define trigPin5 A1
|
|
#define echoPin5 A0
|
|
|
|
#define trigPin6 11
|
|
#define echoPin6 10
|
|
|
|
// Create the motor shield object with the default I2C address
|
|
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
|
|
|
|
// Select which 'port' M1, M2, M3 or M4. In this case, M1
|
|
Adafruit_DCMotor *myMotor = AFMS.getMotor(1);
|
|
Adafruit_DCMotor *myMotor2 = AFMS.getMotor(2);
|
|
Adafruit_DCMotor *myMotor3 = AFMS.getMotor(3);
|
|
Adafruit_DCMotor *myMotor4 = AFMS.getMotor(4);
|
|
|
|
void setup() {
|
|
Serial.begin(9600); // set up Serial library at 9600 bps
|
|
|
|
// Configure the pin modes for each drive motor
|
|
pinMode(trigPin1, OUTPUT);
|
|
pinMode(echoPin1, INPUT);
|
|
|
|
pinMode(trigPin2, OUTPUT);
|
|
pinMode(echoPin2, INPUT);
|
|
|
|
pinMode(trigPin3, OUTPUT);
|
|
pinMode(echoPin3, INPUT);
|
|
|
|
pinMode(trigPin4, OUTPUT);
|
|
pinMode(echoPin4, INPUT);
|
|
|
|
pinMode(trigPin5, OUTPUT);
|
|
pinMode(echoPin5, INPUT);
|
|
|
|
pinMode(trigPin6, OUTPUT);
|
|
pinMode(echoPin6, INPUT);
|
|
|
|
AFMS.begin(); // create with the default frequency 1.6KHz
|
|
|
|
// Set the speed to start, from 0 (off) to 255 (max speed)
|
|
myMotor->setSpeed(100);
|
|
myMotor->run(BACKWARD);
|
|
|
|
myMotor2->setSpeed(100);
|
|
myMotor2->run(FORWARD);
|
|
|
|
myMotor3->setSpeed(100);
|
|
myMotor3->run(FORWARD);
|
|
|
|
myMotor4->setSpeed(100);
|
|
myMotor4->run(BACKWARD);
|
|
}
|
|
|
|
void loop() {
|
|
long duration1, distance1;
|
|
long duration2, distance2;
|
|
long duration3, distance3;
|
|
long duration4, distance4;
|
|
long duration5, distance5;
|
|
long duration6, distance6;
|
|
long a, b, c, d, e;
|
|
|
|
digitalWrite(trigPin1, LOW); // Sets the trigPin1 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin1, HIGH); // Sets the trigPin1 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin1, LOW);
|
|
duration1 = pulseIn(echoPin1, HIGH); // Reads the echoPin1, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin2, LOW); // Sets the trigPin2 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin2, HIGH); // Sets the trigPin2 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin2, LOW);
|
|
duration2 = pulseIn(echoPin2, HIGH); // Reads the echoPin2, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin3, LOW); // Sets the trigPin3 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin3, HIGH); // Sets the trigPin3 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin3, LOW);
|
|
duration3 = pulseIn(echoPin3, HIGH); // Reads the echoPin3, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin4, LOW); // Sets the trigPin4 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin4, HIGH); // Sets the trigPin4 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin4, LOW);
|
|
duration4 = pulseIn(echoPin4, HIGH); // Reads the echoPin4, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin5, LOW); // Sets the trigPin5 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin5, HIGH); // Sets the trigPin5 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin5, LOW);
|
|
duration5 = pulseIn(echoPin5, HIGH); // Sets the trigPin5 on HIGH state for 10 micro seconds
|
|
|
|
digitalWrite(trigPin6, LOW); // Sets the trigPin6 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin6, HIGH); // Sets the trigPin6 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin6, LOW);
|
|
duration6 = pulseIn(echoPin6, HIGH); // Sets the trigPin6 on HIGH state for 10 micro seconds
|
|
|
|
// Calculating the distance
|
|
distance1 = (duration1 /2) / 29.1;
|
|
distance2 = (duration2 /2) / 29.1;
|
|
distance3 = (duration3 /2) / 29.1;
|
|
distance4 = (duration4 /2) / 29.1;
|
|
distance5 = (duration5 /2) / 29.1;
|
|
distance6 = (duration6 /2) / 29.1;
|
|
|
|
// Prints the distance on the Serial Monitor
|
|
Serial.print(distance1);
|
|
Serial.println(" cm");
|
|
Serial.print(distance2);
|
|
Serial.println(" cm");
|
|
Serial.print(distance3);
|
|
Serial.println(" cm");
|
|
Serial.print(distance4);
|
|
Serial.println(" cm");
|
|
Serial.print(distance5);
|
|
Serial.println(" cm");
|
|
Serial.print(distance6);
|
|
Serial.println(" cm");
|
|
Serial.println("\n");
|
|
|
|
a = stop0(distance1, distance2, distance5, distance6);
|
|
b = move1(distance1, distance2, distance5, distance6);
|
|
c = move2(distance1, distance2, distance5, distance6);
|
|
d = turn1(distance1, distance2, distance3, distance4, distance5);
|
|
e = turn2(distance1, distance2, distance3, distance4, distance5);
|
|
}
|
|
|
|
long stop0 (long dis1, long dis2, long dis5, long dis6) //define stop function
|
|
{
|
|
if (dis1 <= 8 && dis2 <= 8 && dis5 <=8 && dis6 <= 8)
|
|
{
|
|
myMotor ->run(RELEASE);
|
|
myMotor2->run(RELEASE);
|
|
myMotor3->run(RELEASE);
|
|
myMotor4->run(RELEASE);
|
|
Serial.println("stop0");
|
|
}
|
|
}
|
|
long move1 (long dis1, long dis2, long dis5, long dis6) //define forward function
|
|
{
|
|
if (dis1 < 8 && dis2 > 4 && dis5 > 4 && dis6 < 8) {
|
|
Serial.println("move1");
|
|
myMotor ->run(FORWARD);
|
|
myMotor2->run(BACKWARD);
|
|
myMotor3->run(BACKWARD);
|
|
myMotor4->run(FORWARD);
|
|
myMotor ->setSpeed(100);
|
|
myMotor2->setSpeed(100);
|
|
myMotor3->setSpeed(100);
|
|
myMotor4->setSpeed(100);
|
|
delay(100);
|
|
}
|
|
}
|
|
|
|
long move2 (long dis1, long dis2, long dis5, long dis6) //define backward function
|
|
{
|
|
if (dis2 < 4 && dis1 > 8 && dis5 < 4 && dis6 > 8)
|
|
{
|
|
Serial.println("move2");
|
|
myMotor ->run(BACKWARD);
|
|
myMotor2->run(FORWARD);
|
|
myMotor3->run(FORWARD);
|
|
myMotor4->run(BACKWARD);
|
|
myMotor ->setSpeed(100);
|
|
myMotor2->setSpeed(100);
|
|
myMotor3->setSpeed(100);
|
|
myMotor4->setSpeed(100);
|
|
delay(100);
|
|
}
|
|
}
|
|
|
|
long turn1 (long dis1, long dis2, long dis3, long dis4, long dis5) //define right turn function
|
|
{
|
|
long dur1, dur2, dur3, dur4, dur5, dur6;
|
|
while (dis2 > 7 && dis3 < 9 && dis4 > 2 && dis5 > 9){
|
|
Serial.println("turn1");
|
|
myMotor ->run(FORWARD);
|
|
myMotor2->run(BACKWARD);
|
|
myMotor3->run(BACKWARD);
|
|
myMotor4->run(FORWARD);
|
|
myMotor ->setSpeed(200);
|
|
myMotor2->setSpeed(200);
|
|
myMotor3->setSpeed(20);
|
|
myMotor4->setSpeed(20);
|
|
|
|
digitalWrite(trigPin1, LOW); // Sets the trigPin1 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin1, HIGH); // Sets the trigPin1 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin1, LOW);
|
|
dur1 = pulseIn(echoPin1, HIGH); // Reads the echoPin1, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin2, LOW); // Sets the trigPin2 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin2, HIGH); // Sets the trigPin2 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin2, LOW);
|
|
dur2 = pulseIn(echoPin2, HIGH); // Reads the echoPin2, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin3, LOW); // Sets the trigPin3 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin3, HIGH); // Sets the trigPin3 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin3, LOW);
|
|
dur3 = pulseIn(echoPin3, HIGH); // Reads the echoPin3, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin4, LOW); // Sets the trigPin4 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin4, HIGH); // Sets the trigPin4 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin4, LOW);
|
|
dur4 = pulseIn(echoPin4, HIGH); // Reads the echoPin4, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin5, LOW); // Sets the trigPin5 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin5, HIGH); // Sets the trigPin5 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin5, LOW);
|
|
dur5 = pulseIn(echoPin5, HIGH); // Sets the trigPin5 on HIGH state for 10 micro seconds
|
|
|
|
// Calculating the distance
|
|
dis1 = (dur1 /2) / 29.1;
|
|
dis2 = (dur2 /2) / 29.1;
|
|
dis3 = (dur3 /2) / 29.1;
|
|
dis4 = (dur4 /2) / 29.1;
|
|
dis5 = (dur5 /2) / 29.1;
|
|
|
|
delay(120);
|
|
|
|
myMotor ->run(RELEASE);
|
|
myMotor2->run(RELEASE);
|
|
myMotor3->run(RELEASE);
|
|
myMotor4->run(RELEASE);
|
|
}
|
|
}
|
|
|
|
long turn2 (long dis1, long dis2, long dis3, long dis4, long dis5) //define left function
|
|
{
|
|
long dur1, dur2, dur3, dur4, dur5;
|
|
while (dis2 < 6 && dis3 < 8 && dis4 > 2){
|
|
Serial.println("turn2");
|
|
myMotor ->run(FORWARD);
|
|
myMotor2->run(BACKWARD);
|
|
myMotor3->run(BACKWARD);
|
|
myMotor4->run(FORWARD);
|
|
myMotor ->setSpeed(30);
|
|
myMotor2->setSpeed(30);
|
|
myMotor3->setSpeed(200);
|
|
myMotor4->setSpeed(200);
|
|
|
|
digitalWrite(trigPin1, LOW); // Sets the trigPin1 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin1, HIGH); // Sets the trigPin1 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin1, LOW);
|
|
dur1 = pulseIn(echoPin1, HIGH); // Reads the echoPin1, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin2, LOW); // Sets the trigPin2 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin2, HIGH); // Sets the trigPin2 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin2, LOW);
|
|
dur2 = pulseIn(echoPin2, HIGH); // Reads the echoPin2, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin3, LOW); // Sets the trigPin3 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin3, HIGH); // Sets the trigPin3 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin3, LOW);
|
|
dur3 = pulseIn(echoPin3, HIGH); // Reads the echoPin3, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin4, LOW); // Sets the trigPin4 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin4, HIGH); // Sets the trigPin4 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin4, LOW);
|
|
dur4 = pulseIn(echoPin4, HIGH); // Reads the echoPin4, returns the sound wave travel time in microseconds
|
|
|
|
digitalWrite(trigPin5, LOW); // Sets the trigPin5 on LOW state for 2 micro seconds
|
|
delayMicroseconds(2);
|
|
digitalWrite(trigPin5, HIGH); // Sets the trigPin5 on HIGH state for 10 micro seconds
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin5, LOW);
|
|
dur5 = pulseIn(echoPin5, HIGH); // Sets the trigPin5 on HIGH state for 10 micro seconds
|
|
|
|
// Calculating the distance
|
|
dis1 = (dur1 /2) / 29.1;
|
|
dis2 = (dur2 /2) / 29.1;
|
|
dis3 = (dur3 /2) / 29.1;
|
|
dis4 = (dur4 /2) / 29.1;
|
|
|
|
delay(120);
|
|
|
|
myMotor ->run(RELEASE);
|
|
myMotor2->run(RELEASE);
|
|
myMotor3->run(RELEASE);
|
|
myMotor4->run(RELEASE);
|
|
}
|
|
}
|
|
|