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.
69 lines
1.8 KiB
69 lines
1.8 KiB
5 years ago
|
#include <Wire.h>
|
||
|
|
#include <Adafruit_MotorShield.h>
|
||
|
|
#include "utility/Adafruit_MS_PWMServoDriver.h"
|
||
|
|
#define trigPin A1
|
||
|
|
#define echoPin A0
|
||
|
|
|
||
|
|
|
||
|
|
// 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
|
||
|
|
Serial.println("Adafruit Motorshield v2 - DC Motor test!");
|
||
|
|
pinMode(trigPin, OUTPUT);
|
||
|
|
pinMode(echoPin, INPUT);
|
||
|
|
|
||
|
|
AFMS.begin(); // create with the default frequency 1.6KHz
|
||
|
|
Serial.println("AFMS began."); //not reaching here
|
||
|
|
|
||
|
|
// Set the speed to start, from 0 (off) to 255 (max speed)
|
||
|
|
myMotor->setSpeed(100);
|
||
|
|
myMotor->run(FORWARD);
|
||
|
|
|
||
|
|
myMotor2->setSpeed(100);
|
||
|
|
myMotor2->run(BACKWARD);
|
||
|
|
|
||
|
|
myMotor3->setSpeed(100);
|
||
|
|
myMotor3->run(BACKWARD);
|
||
|
|
|
||
|
|
myMotor4->setSpeed(100);
|
||
|
|
myMotor4->run(FORWARD);
|
||
|
|
}
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
long duration, distance;
|
||
|
|
digitalWrite(trigPin, LOW); // Added this line
|
||
|
|
delayMicroseconds(2); // Added this line
|
||
|
|
digitalWrite(trigPin, HIGH);
|
||
|
|
delayMicroseconds(10); // Added this line
|
||
|
|
digitalWrite(trigPin, LOW);
|
||
|
|
duration = pulseIn(echoPin, HIGH);
|
||
|
|
distance = duration *0.034/2;
|
||
|
|
if (distance <= 5){
|
||
|
|
myMotor ->run(RELEASE);
|
||
|
|
myMotor2->run(RELEASE);
|
||
|
|
myMotor3->run(RELEASE);
|
||
|
|
myMotor4->run(RELEASE);
|
||
|
|
}
|
||
|
|
else {
|
||
|
|
Serial.print(distance);
|
||
|
|
Serial.println(" cm");
|
||
|
|
myMotor->run(FORWARD);
|
||
|
|
myMotor2->run(BACKWARD);
|
||
|
|
myMotor3->run(BACKWARD);
|
||
|
|
myMotor4->run(FORWARD);
|
||
|
|
myMotor ->setSpeed(100);
|
||
|
|
myMotor2->setSpeed(100);
|
||
|
|
myMotor3->setSpeed(100);
|
||
|
|
myMotor4->setSpeed(100);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|