Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- // include libraries will be used
- #include <NewPing.h>
- #include <QTRSensors.h>
- #include <Wire.h>
- #include <Adafruit_MotorShield.h>
- // defining sensors, which is part of QTRSensor library
- #define NUM_SENSORS 8 // number of sensors used
- #define NUM_SAMPLES_PER_SENSOR 4 // average 4 analog samples per sensor reading
- #define EMITTER_PIN 2 // emitter is controlled by digital pin 2
- #define Gnd_PIN 34 /////////// Ground PIN
- #define Echo_PIN 31 /////////// Signal Out PIN
- #define Trig_PIN 33 /////////// Return Signal
- #define Vcc_PIN 40 /////////// 5vdc supply
- #define Max_Dist1 3000 /////////// Setting Max Distance uS can sense for
- /////////// Sensor 2 - RH Side ///////////
- #define Gnd2_PIN 22 /////////// Ground PIN
- #define Echo2_PIN 24 /////////// Signal Out PIN
- #define Trig2_PIN 26 /////////// Return Signal
- #define Vcc2_PIN 28 /////////// 5vdc supply
- #define Max_Dist2 3000
- /*
- define global variables, which will be used later
- state to remember state, what it was doing at that time and when it was turning left right and how hard.
- <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<------------------------>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>@@@@@@@@@@@@@@@@@@@@@@@@@@
- 0 - 90Degree Left, 1 - Hard left, 2 - Med left, 3 - Low Left, 4 - MiddleL, 5 - MiddleR, 6 - Low right, 7 - Med right, 8 - Hard right, 9 - 90Degree right, 10 - No black line found
- <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<------------------------>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>@@@@@@@@@@@@@@@@@@@@@@@@@@
- set to be default state 4 middle for now.
- dark is defined when sensor reads 800
- waiting is part of delay variable, which could be changed easily.
- sumSensor is variable, which stores total sum of sensorValues for later to determine, how many sensors are trigerred
- wasStateTen, tooManySensors, reset, noLineTimer is variables to remember, default 0
- */
- // all 8 QTR sensors are connected to analog inputs 15 through 8, respectively
- QTRSensorsAnalog qtra((unsigned char[]) { A15, A14, A13, A12, A11, A10, A9, A8},
- NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
- unsigned int sensorValues[NUM_SENSORS];
- //Motorshield library setup, first motor is set to M1 and other on M4
- Adafruit_MotorShield AFMS = Adafruit_MotorShield();
- Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
- Adafruit_DCMotor *myMotor3 = AFMS.getMotor(4);
- /////////// Setting line sensors dark position to be 800
- int Program = 0; /////////// Initial Setting of the Program Data Register
- int Follower = 0; /////////// Initial Setting of the Line Follower Program
- int Distance1; /////////// Ultrasonic sensor 1
- int Distance2; /////////// Ultrasonic sensor 2
- int dark = 800;
- int waiting = 1;
- NewPing sonar1(Trig_PIN, Echo_PIN, Max_Dist1); /////////// NewPing setup of pins and maximum distance.
- NewPing sonar2(Trig2_PIN, Echo2_PIN, Max_Dist2); //////
- void setup() {
- Serial.begin(9600); // set up Serial library at 9600 bps
- AFMS.begin(); // create with the default frequency 1.6KHz
- // RELEASE function of library allows to stop motors if they are for some reason running
- myMotor1->run(RELEASE);
- myMotor3->run(RELEASE);
- /////////// Setting Pins to I/Os - Sensor 1 ///////////
- pinMode(Gnd_PIN, OUTPUT); //Setting the ground pin to an output
- digitalWrite(Gnd_PIN, LOW); //Setting the ground pin low, 0v
- pinMode(Vcc_PIN, OUTPUT); //Setting the supply voltage to an output
- digitalWrite(Vcc_PIN, HIGH); //Setting the supply voltage to 5v
- pinMode(Echo_PIN, INPUT); //Setting ECHO pin to an input
- pinMode(Trig_PIN, OUTPUT); //Setting TRIGGER pin to an output
- /////////// Setting Pins to I/Os - Sensor 2 ///////////
- pinMode(Gnd2_PIN, OUTPUT); //Setting the ground pin to an output
- digitalWrite(Gnd2_PIN, LOW); //Setting the ground pin low, 0v
- pinMode(Vcc2_PIN, OUTPUT); //Setting the supply voltage to an output
- digitalWrite(Vcc2_PIN, HIGH); //Setting the supply voltage to 5v
- pinMode(Echo2_PIN, INPUT); //Setting ECHO pin to an input
- pinMode(Trig2_PIN, OUTPUT); //Setting TRIGGER pin to an output
- pinMode(13, OUTPUT); //set arduino LED to an output
- digitalWrite(13, HIGH); // turn on Arduino's LED to indicate we are in calibration mode
- Serial.println("Calibrating");
- // make the calibration take about 10 seconds
- for (int i = 0; i < 400; i++)
- {
- qtra.calibrate(); // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
- }
- digitalWrite(13, LOW); // turn off Arduino's LED to indicate we are through with calibration
- // print the calibration minimum values measured when emitters were on
- for (int i = 0; i < NUM_SENSORS; i++)
- {
- Serial.print(qtra.calibratedMinimumOn[i]);
- Serial.print(' ');
- }
- Serial.println();
- // print the calibration maximum values measured when emitters were on
- for (int i = 0; i < NUM_SENSORS; i++)
- {
- Serial.print(qtra.calibratedMaximumOn[i]);
- Serial.print(' ');
- }
- Serial.println();
- }
- void loop() {
- /////////// Ultrasonic Sensor Serial Coms to display value //////////
- unsigned int position = qtra.readLine(sensorValues);
- for (unsigned char i = 0; i < NUM_SENSORS; i++)
- {
- Serial.print(sensorValues[i]);
- Serial.print('\t');
- }
- unsigned int uS1 = sonar1.ping(); // Send ping, get ping time in microseconds (uS).
- Distance1 = (uS1 / US_ROUNDTRIP_CM);
- delay(50);
- unsigned int uS2 = sonar2.ping(); // Send ping, get ping time in microseconds (uS).
- Distance2 = (uS2 / US_ROUNDTRIP_CM);
- /////////// Serial Print Ultrasonic Sensor 1 ///////////
- Serial.print("Ping1: ");
- Serial.print(Distance1); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
- Serial.print("cm "); // Serial Display Units in Centimeters
- /////////// Serial Print Ultrasonic Sensor 2 ///////////
- Serial.print("Ping2: ");
- Serial.print(Distance2); // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
- Serial.println("cm"); // Serial Display Units in Centimeters
- ///////////////////////SIMPLE LINE FOLLOWER/////////////////////////////
- //If robot have not triggered many sensors and our current state is not 10(no black line)
- if(Program == 0){
- int wasUsed = 0; //Sensor readings could read dark for more than 1 sensor, this will ensure condition is only triggered once
- //left hard
- if(sensorValues[7] > dark && wasUsed == 0){
- myMotor1->run(BACKWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(30);
- myMotor3->setSpeed(110);
- Serial.println("left hard");
- wasUsed = 1;
- delay(waiting);
- }
- //right hard
- if(sensorValues[0] > dark && wasUsed == 0){
- myMotor1->run(FORWARD);
- myMotor3->run(BACKWARD);
- myMotor1->setSpeed(110);
- myMotor3->setSpeed(30); // was 25
- Serial.println("right hard");
- wasUsed = 1;
- delay(waiting);
- }
- //left medium
- if(sensorValues[6] > dark && wasUsed == 0){
- myMotor1->run(BACKWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(30);
- myMotor3->setSpeed(100);
- Serial.println("left medium");
- wasUsed = 1;
- delay(waiting);
- }
- //right medium
- if(sensorValues[1] > dark && wasUsed == 0){
- myMotor1->run(FORWARD);
- myMotor3->run(BACKWARD);
- myMotor1->setSpeed(100);
- myMotor3->setSpeed(30); // was 25
- Serial.println("right medium");
- wasUsed = 1;
- delay(waiting);
- }
- //left low
- if(sensorValues[5] > dark && wasUsed == 0){
- myMotor1->run(FORWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(50);
- myMotor3->setSpeed(75); // was 25
- Serial.println("left low");
- wasUsed = 1;
- delay(waiting);
- }
- //right low
- if(sensorValues[2] > dark && wasUsed == 0){
- myMotor1->run(FORWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(75);
- myMotor3->setSpeed(50); // was 25
- Serial.println("right low");
- wasUsed = 1;
- delay(waiting);
- }
- //forward
- if((sensorValues[4] > dark || sensorValues[3] > dark) && (wasUsed == 0)){
- myMotor1->run(FORWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(70);
- myMotor3->setSpeed(75);
- Serial.println("forward");
- wasUsed = 1;
- delay(waiting);
- }
- if ((Distance1 <= 15) && (Distance1 != 0)){
- Program = 1; /////////// Detect obstacle at <=10cm - Set 1 in Data Register
- myMotor1->run(RELEASE);
- myMotor3->run(RELEASE);
- Serial.println("Program set to 1");
- delay(1000);
- }
- }// simple line follower ends here
- ///////////////////////SIMPLE LINE FOLLOWER///////////////
- /////////// If the Ultrasonic Sensor 1 detects an object less than 10cm, a 1 is set in Program Data Register to initiate Obstacle Avoidance ///////////
- if(Program == 1){
- Serial.println("Im in Program 1");
- if((Distance2 <= 15) && (Distance2 != 0)){ /////////// Once uS2 detects object, +1 into Program register to move program on
- Program = 2;
- }
- myMotor1->run(BACKWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(50);
- myMotor3->setSpeed(50);
- Serial.println("Turn left");
- }
- ///////////////////////////////////
- if(Program == 2){
- Serial.println("Im in program 2");
- if((sensorValues[0] > dark) || (sensorValues[1] > dark) || (sensorValues[2] > dark) || (sensorValues[3] > dark) || (sensorValues[4] > dark) || (sensorValues[5] > dark)
- || (sensorValues[6] > dark) || (sensorValues[7] > dark)){
- Program = 0;
- myMotor1->run(RELEASE);
- myMotor3->run(RELEASE);
- Serial.println("Set program to Line Follower");
- delay (1000);
- }
- myMotor1->run(FORWARD);
- myMotor3->run(FORWARD);
- myMotor1->setSpeed(60);
- myMotor3->setSpeed(40);
- Serial.println("Driving in arc");
- }
- } //LOOP
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement