AutonomusParkingCar
5 years ago
1 changed files with 0 additions and 68 deletions
@ -1,68 +0,0 @@ |
|||||
#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); |
|
||||
} |
|
||||
} |
|
||||
|
|
Loading…
Reference in new issue