138 lines
4.1 KiB
C++
138 lines
4.1 KiB
C++
#include <AFMotor.h>
|
|
//SMARS Demo 1 with Line sensor
|
|
//This sketch makes the robot move inside a delimitated Area (you can make a perimeter with insulating tape)
|
|
//you'll need an Adafruit Motor shield V1 https://goo.gl/7MvZeo and a IR sensor https://goo.gl/vPWfzx
|
|
AF_DCMotor rightMotor(2);
|
|
AF_DCMotor leftMotor(1);
|
|
// declare variables
|
|
int lineNumber;
|
|
int trigPin = 10; // Trigger
|
|
int echoPin = 11; // Echo
|
|
long duration, cm, inches;
|
|
int speed_L = 200;
|
|
int speed_R = 200;
|
|
String motorSet = "";
|
|
#define MOTORS_CALIBRATION_OFFSET 0 // this sets offset to allow for differences between the two DC motors
|
|
|
|
#define MAX_SPEED 200 // sets speed of DC traction motors to 120/256 or about 47% of full speed - to reduce power draining.
|
|
int speedSet = 0;
|
|
|
|
|
|
void setup() {
|
|
Serial.begin(9600);
|
|
//Define inputs and outputs
|
|
pinMode(trigPin, OUTPUT);
|
|
pinMode(echoPin, INPUT);
|
|
// defines Right motor connector
|
|
// defines Left motor connector
|
|
//defines the variable where it will store the
|
|
// sets up Serial library at 9600 bps
|
|
//changes the following values to make the robot possible drive as straight as
|
|
|
|
|
|
leftMotor.setSpeed(speed_L);
|
|
rightMotor.setSpeed(speed_R); // sets L motor speed
|
|
|
|
// sets R motor speed
|
|
rightMotor.run(RELEASE);
|
|
leftMotor.run(RELEASE); //turns L motor on
|
|
//turns R motor on
|
|
}
|
|
void loop() {
|
|
|
|
// the following operations will make the robot goes backward for 2 seconds and turns left for 1.5 seconds
|
|
// rightMotor.run(RELEASE);
|
|
// leftMotor.run(RELEASE);
|
|
// recule();
|
|
// delay(2000);
|
|
delay(150);
|
|
int dist = distance();
|
|
dist = min(200, dist);
|
|
if (dist <= 20) {
|
|
while (dist <= 30) {
|
|
moveBackward();
|
|
dist = distance();
|
|
dist = min(200, dist);
|
|
}
|
|
turnRight();
|
|
}
|
|
else {
|
|
moveForward();
|
|
}
|
|
|
|
|
|
}
|
|
void turnRight() {
|
|
motorSet = "RIGHT";
|
|
leftMotor.run(FORWARD); // turn motor 1 forward
|
|
rightMotor.run(BACKWARD); // turn motor 2 backward
|
|
delay(400); // run motors this way for 400ms
|
|
motorSet = "FORWARD";
|
|
leftMotor.run(FORWARD); // set both motors back to forward
|
|
rightMotor.run(FORWARD);
|
|
}
|
|
//void avance()
|
|
//{
|
|
// // turning left
|
|
// rightMotor.run(FORWARD);
|
|
// leftMotor.run(FORWARD);
|
|
//}
|
|
//void recule()
|
|
//{
|
|
// //go backward
|
|
// rightMotor.run(BACKWARD);
|
|
// leftMotor.run(BACKWARD);
|
|
//}
|
|
|
|
int distance() {
|
|
// The sensor is triggered by a HIGH pulse of 10 or more microseconds.
|
|
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
|
|
digitalWrite(trigPin, LOW);
|
|
delayMicroseconds(5);
|
|
digitalWrite(trigPin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(trigPin, LOW);
|
|
|
|
// Read the signal from the sensor: a HIGH pulse whose
|
|
// duration is the time (in microseconds) from the sending
|
|
// of the ping to the reception of its echo off of an object.
|
|
pinMode(echoPin, INPUT);
|
|
duration = pulseIn(echoPin, HIGH);
|
|
|
|
// Convert the time into a distance
|
|
cm = (duration/2) / 29.1; // Divide by 29.1 or multiply by 0.0343
|
|
inches = (duration/2) / 74; // Divide by 74 or multiply by 0.0135
|
|
|
|
Serial.print(inches);
|
|
Serial.print("in, ");
|
|
Serial.print(cm);
|
|
Serial.print("cm");
|
|
Serial.println();
|
|
|
|
return cm;
|
|
}
|
|
|
|
void moveForward() {
|
|
motorSet = "FORWARD";
|
|
leftMotor.run(FORWARD); // turn it on going forward
|
|
rightMotor.run(FORWARD); // turn it on going forward
|
|
for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
|
|
{
|
|
leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET);
|
|
rightMotor.setSpeed(speedSet);
|
|
delay(5);
|
|
}
|
|
}
|
|
//-------------------------------------------------------------------------------------------------------------------------------------
|
|
void moveBackward() {
|
|
motorSet = "BACKWARD";
|
|
leftMotor.run(BACKWARD); // turn it on going forward
|
|
rightMotor.run(BACKWARD); // turn it on going forward
|
|
for (speedSet = 0; speedSet < MAX_SPEED; speedSet += 2) // slowly bring the speed up to avoid loading down the batteries too quickly
|
|
{
|
|
leftMotor.setSpeed(speedSet + MOTORS_CALIBRATION_OFFSET);
|
|
rightMotor.setSpeed(speedSet);
|
|
delay(5);
|
|
}
|
|
}
|