|
|
@ -7,9 +7,18 @@ |
|
|
|
|
|
|
|
/*
|
|
|
|
* Include all libraries. |
|
|
|
* |
|
|
|
*/ |
|
|
|
//Include the Arduino Stepper Library
|
|
|
|
#include <Stepper.h> |
|
|
|
|
|
|
|
/*
|
|
|
|
* Include the Arduino Servo Library |
|
|
|
*/ |
|
|
|
#include <Servo.h> |
|
|
|
|
|
|
|
/*
|
|
|
|
* Include the Ping Ulptrasonic Library |
|
|
|
*/ |
|
|
|
#include <NewPing.h> |
|
|
|
|
|
|
|
/*
|
|
|
|
* ------------------------------------------------------- |
|
|
@ -18,18 +27,18 @@ |
|
|
|
/*
|
|
|
|
* All Definitions. |
|
|
|
*/ |
|
|
|
// Number of steps per internal motor revolution
|
|
|
|
#define STEPS_PER_REV 32 |
|
|
|
|
|
|
|
#define GEAR_RED 64 |
|
|
|
#define ULTRASONIC_TIMEOUT 100000 |
|
|
|
#define SERVO_SPEED 6 |
|
|
|
#define MAX_DISTANCE 200 |
|
|
|
/*
|
|
|
|
* ------------------------------------------------------- |
|
|
|
*/ |
|
|
|
|
|
|
|
// defines pins numbers
|
|
|
|
/*
|
|
|
|
* Define Ultrasonic Pins |
|
|
|
*/ |
|
|
|
const int trigPin = 12; |
|
|
|
const int echoPin = 9; |
|
|
|
//const int response = 13;
|
|
|
|
const int echoPin = 11; |
|
|
|
/*
|
|
|
|
* For Motor Driver Pis. |
|
|
|
*/ |
|
|
@ -41,37 +50,51 @@ const int rightSpeed = 5; |
|
|
|
const int leftSpeed = 3; |
|
|
|
|
|
|
|
/*
|
|
|
|
* For Stepper Motor Pins |
|
|
|
* For Servo Motor Pins |
|
|
|
*/ |
|
|
|
const int in1_1 = 6; |
|
|
|
const int in1_2 = 10; |
|
|
|
const int in1_3 = 11; |
|
|
|
const int in1_4 = 13; |
|
|
|
|
|
|
|
const int stepper_motor_speed = 700; |
|
|
|
const int servoPin = 6; |
|
|
|
|
|
|
|
// Number of steps per geared output rotation
|
|
|
|
const float STEPS_PER_OUT_REV = STEPS_PER_REV * GEAR_RED; |
|
|
|
const int StepsRequired = STEPS_PER_OUT_REV / 4; |
|
|
|
/*
|
|
|
|
* Define Servo Data Struct |
|
|
|
*/ |
|
|
|
Servo myServo; |
|
|
|
|
|
|
|
// define Stepper
|
|
|
|
Stepper steppermotor(STEPS_PER_REV, in1_1, in1_2, in1_3, in1_4); |
|
|
|
/*
|
|
|
|
* Define Ultrasonic Struct |
|
|
|
*/ |
|
|
|
NewPing sonar1 (trigPin, echoPin, MAX_DISTANCE); |
|
|
|
|
|
|
|
// defines variables
|
|
|
|
/*
|
|
|
|
* Define all useful variables |
|
|
|
*/ |
|
|
|
long duration; |
|
|
|
int distance, distanceCm, pos; |
|
|
|
float distance, distanceCm; |
|
|
|
int pos, dir; |
|
|
|
|
|
|
|
/*
|
|
|
|
* |
|
|
|
*/ |
|
|
|
void clearUltrasonic(){ |
|
|
|
// Clears the trigPin
|
|
|
|
analogWrite(trigPin, LOW); |
|
|
|
delayMicroseconds(2); |
|
|
|
* Turn Servo using custom Speed |
|
|
|
*/ |
|
|
|
void turnServo(int from, int deg){ |
|
|
|
int d; |
|
|
|
if (from > deg){ |
|
|
|
for(d=from; d>=deg; d--){ |
|
|
|
myServo.write(d); |
|
|
|
delay(SERVO_SPEED); |
|
|
|
} |
|
|
|
} else{ |
|
|
|
for(d=from; d<=deg; d++){ |
|
|
|
myServo.write(d); |
|
|
|
delay(SERVO_SPEED); |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Setup Wiring Components |
|
|
|
*/ |
|
|
|
void setup() { |
|
|
|
pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
|
|
|
|
pinMode(echoPin, INPUT); // Sets the echoPin as an Input
|
|
|
|
//pinMode(trigPin, OUTPUT); // Sets the trigPin as an Output
|
|
|
|
//pinMode(echoPin, INPUT); // Sets the echoPin as an Input
|
|
|
|
|
|
|
|
//pinMode(response, OUTPUT); // Sets to led aoutput.
|
|
|
|
|
|
|
@ -83,37 +106,49 @@ void setup() { |
|
|
|
pinMode(rightSpeed, OUTPUT); |
|
|
|
pinMode(leftSpeed, OUTPUT); |
|
|
|
|
|
|
|
//Initialize Servo Motor
|
|
|
|
myServo.attach(servoPin); |
|
|
|
pos = 1; |
|
|
|
dir = 1; |
|
|
|
|
|
|
|
//Initialize car direction and make walk
|
|
|
|
absolute(); |
|
|
|
forward(); |
|
|
|
|
|
|
|
Serial.begin(9600); // Starts the serial communication
|
|
|
|
clearUltrasonic(); |
|
|
|
|
|
|
|
// Clears the trigPin
|
|
|
|
analogWrite(trigPin, LOW); |
|
|
|
delayMicroseconds(2); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Stepper Function |
|
|
|
* Stepper Function for Servo. |
|
|
|
* 1st Step: straight |
|
|
|
* 2nd Step: left |
|
|
|
* 3rd Step: straight |
|
|
|
* 4th Step: right |
|
|
|
*/ |
|
|
|
void next_step() |
|
|
|
{ |
|
|
|
if(pos == 1) |
|
|
|
{ |
|
|
|
steppermotor.setSpeed(stepper_motor_speed); |
|
|
|
steppermotor.step(StepsRequired); |
|
|
|
{ //Servo move to right
|
|
|
|
turnServo(90, 30); |
|
|
|
pos = 2; |
|
|
|
} |
|
|
|
else if(pos ==2) |
|
|
|
{ |
|
|
|
steppermotor.setSpeed(stepper_motor_speed); |
|
|
|
steppermotor.step(-1*StepsRequired); |
|
|
|
{ //Servo move to center
|
|
|
|
turnServo(30, 90); |
|
|
|
pos = 3; |
|
|
|
} |
|
|
|
else if(pos == 3) |
|
|
|
{ |
|
|
|
steppermotor.setSpeed(stepper_motor_speed); |
|
|
|
steppermotor.step(-1*StepsRequired); |
|
|
|
{ //Servo move to left
|
|
|
|
turnServo(90, 150); |
|
|
|
pos = 4; |
|
|
|
} |
|
|
|
else if(pos == 4) |
|
|
|
{ |
|
|
|
steppermotor.setSpeed(stepper_motor_speed); |
|
|
|
steppermotor.step(StepsRequired); |
|
|
|
{ //Servo move to center
|
|
|
|
turnServo(150, 90); |
|
|
|
pos = 1; |
|
|
|
} |
|
|
|
} |
|
|
@ -122,51 +157,42 @@ void next_step() |
|
|
|
* Distance meter via Ultrasonic |
|
|
|
*/ |
|
|
|
|
|
|
|
int getDistance(int echoPin, int trigPin){ |
|
|
|
clearUltrasonic(); |
|
|
|
// Sets the trigPin on HIGH state for 10 micro seconds
|
|
|
|
digitalWrite(trigPin, HIGH); |
|
|
|
delayMicroseconds(10); |
|
|
|
digitalWrite(trigPin, LOW); |
|
|
|
|
|
|
|
// Reads the echbackwardoPin, returns the sound wave travel time in microseconds
|
|
|
|
duration = pulseIn(echoPin, HIGH); |
|
|
|
distanceCm = duration * 0.0340 / 2; |
|
|
|
float getDistance(int echoPin, int trigPin){ |
|
|
|
do{ |
|
|
|
int uS1 = sonar1.ping(); |
|
|
|
distanceCm = uS1 / US_ROUNDTRIP_CM; |
|
|
|
} while(distanceCm == 0); |
|
|
|
|
|
|
|
return distanceCm; |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Stepper Function |
|
|
|
* Left turn walk |
|
|
|
*/ |
|
|
|
void step(/*boolean dir,*/int steps){ |
|
|
|
// digitalWrite(dirPin1,dir);
|
|
|
|
// digitalWrite(dirPin2,dir);
|
|
|
|
delay(50); |
|
|
|
for(int i=0;i<steps;i++){ |
|
|
|
forward(); |
|
|
|
delayMicroseconds(100); |
|
|
|
motor_stop(); |
|
|
|
delayMicroseconds(100); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
void turnLeft(){ |
|
|
|
analogWrite(leftSpeed, 255); |
|
|
|
analogWrite(rightSpeed, 0); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Right turn walk |
|
|
|
*/ |
|
|
|
void turnRight(){ |
|
|
|
analogWrite(leftSpeed, 0); |
|
|
|
analogWrite(rightSpeed, 255); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Straight walk |
|
|
|
*/ |
|
|
|
void absolute(){ |
|
|
|
analogWrite(leftSpeed, 255); |
|
|
|
analogWrite(rightSpeed, 255); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Backward walk |
|
|
|
*/ |
|
|
|
void backward(){ |
|
|
|
// Set Motor A backward
|
|
|
|
digitalWrite(in1, HIGH); |
|
|
@ -176,6 +202,9 @@ void backward(){ |
|
|
|
digitalWrite(in4, LOW); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Forward walk |
|
|
|
*/ |
|
|
|
void forward(){ |
|
|
|
// Set Motor A forward
|
|
|
|
digitalWrite(in1, LOW); |
|
|
@ -185,6 +214,9 @@ void forward(){ |
|
|
|
digitalWrite(in4, HIGH); |
|
|
|
} |
|
|
|
|
|
|
|
/*
|
|
|
|
* Stop motors |
|
|
|
*/ |
|
|
|
void motor_stop(){ |
|
|
|
digitalWrite(in1, LOW); |
|
|
|
digitalWrite(in2, LOW); |
|
|
@ -194,13 +226,33 @@ void motor_stop(){ |
|
|
|
|
|
|
|
void turn(){ |
|
|
|
if (pos == 1){ |
|
|
|
turnLeft(); |
|
|
|
} else if (pos == 2){ |
|
|
|
absolute(); |
|
|
|
} else if (pos == 2){ |
|
|
|
turnLeft(); |
|
|
|
} else if (pos == 3){ |
|
|
|
turnRight(); |
|
|
|
} else if (pos == 4){ |
|
|
|
absolute(); |
|
|
|
} else if (pos == 4){ |
|
|
|
turnRight(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
void moveCar(){ |
|
|
|
if (pos == 3){ |
|
|
|
if (dir == 1){ |
|
|
|
turnRight(); |
|
|
|
backward(); |
|
|
|
|
|
|
|
dir = 2; |
|
|
|
} else{ |
|
|
|
turnLeft(); |
|
|
|
backward(); |
|
|
|
|
|
|
|
dir = 1; |
|
|
|
} |
|
|
|
delay(1500); |
|
|
|
forward(); |
|
|
|
} else{ |
|
|
|
forward(); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
@ -209,14 +261,17 @@ void loop() { |
|
|
|
distance= getDistance(echoPin, trigPin); |
|
|
|
|
|
|
|
// Prints the distance on the Serial Monitor
|
|
|
|
//Serial.print("Distance: ");
|
|
|
|
Serial.print("Distance: "); |
|
|
|
Serial.println(distance); |
|
|
|
forward(); |
|
|
|
if (distance < 9){ |
|
|
|
if (distance < 6){ |
|
|
|
motor_stop(); |
|
|
|
// Set Ultrasonic to next step
|
|
|
|
next_step(); |
|
|
|
// Change direction to motors
|
|
|
|
turn(); |
|
|
|
delay(1500); |
|
|
|
forward(); |
|
|
|
// Move car
|
|
|
|
moveCar(); |
|
|
|
delay(1000); |
|
|
|
} |
|
|
|
delay(500); |
|
|
|
delay(100); |
|
|
|
} |