SHARE
TWEET

Untitled

a guest Dec 13th, 2019 86 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. // include libraries will be used
  2. #include <NewPing.h>
  3. #include <QTRSensors.h>
  4. #include <Wire.h>
  5. #include <Adafruit_MotorShield.h>
  6.  
  7. // defining sensors, which is part of QTRSensor library
  8. #define NUM_SENSORS             8  // number of sensors used
  9. #define NUM_SAMPLES_PER_SENSOR  4  // average 4 analog samples per sensor reading
  10. #define EMITTER_PIN             2  // emitter is controlled by digital pin 2
  11.  
  12.  
  13. #define Gnd_PIN 34                       /////////// Ground PIN
  14. #define Echo_PIN 31                      /////////// Signal Out PIN
  15. #define Trig_PIN 33                      /////////// Return Signal
  16. #define Vcc_PIN 40                       /////////// 5vdc supply
  17. #define Max_Dist1 3000                    /////////// Setting Max Distance uS can sense for
  18.  
  19. /////////// Sensor 2 - RH Side ///////////
  20. #define Gnd2_PIN 22                      /////////// Ground PIN
  21. #define Echo2_PIN 24                     /////////// Signal Out PIN
  22. #define Trig2_PIN 26                     /////////// Return Signal
  23. #define Vcc2_PIN 28                      /////////// 5vdc supply
  24. #define Max_Dist2 3000  
  25. /*
  26. define global variables, which will be used later
  27. state to remember state, what it was doing at that time and when it was turning left right and how hard.
  28. <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<------------------------>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>@@@@@@@@@@@@@@@@@@@@@@@@@@
  29. 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
  30. <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<------------------------>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>@@@@@@@@@@@@@@@@@@@@@@@@@@
  31. set to be default state 4 middle for now.
  32. dark is defined when sensor reads 800
  33. waiting is part of delay variable, which could be changed easily.
  34. sumSensor is variable, which stores total sum of sensorValues for later to determine, how many sensors are trigerred
  35. wasStateTen, tooManySensors, reset, noLineTimer is variables to remember, default 0
  36. */
  37.  
  38. // all 8 QTR sensors are connected to analog inputs 15 through 8, respectively
  39. QTRSensorsAnalog qtra((unsigned char[]) { A15, A14, A13, A12, A11, A10, A9, A8},
  40. NUM_SENSORS, NUM_SAMPLES_PER_SENSOR, EMITTER_PIN);
  41. unsigned int sensorValues[NUM_SENSORS];
  42.  
  43. //Motorshield library setup, first motor is set to M1 and other on M4
  44. Adafruit_MotorShield AFMS = Adafruit_MotorShield();
  45. Adafruit_DCMotor *myMotor1 = AFMS.getMotor(1);
  46. Adafruit_DCMotor *myMotor3 = AFMS.getMotor(4);
  47.                         /////////// Setting line sensors dark position to be 800
  48. int Program = 0;                        /////////// Initial Setting of the Program Data Register
  49. int Follower = 0;                       /////////// Initial Setting of the Line Follower Program
  50. int Distance1;                          /////////// Ultrasonic sensor 1
  51. int Distance2;                          /////////// Ultrasonic sensor 2
  52. int dark = 800;
  53. int waiting = 1;
  54.  
  55. NewPing sonar1(Trig_PIN, Echo_PIN, Max_Dist1);    /////////// NewPing setup of pins and maximum distance.
  56. NewPing sonar2(Trig2_PIN, Echo2_PIN, Max_Dist2);  //////
  57.  
  58. void setup() {
  59.   Serial.begin(9600); // set up Serial library at 9600 bps
  60.   AFMS.begin();  // create with the default frequency 1.6KHz
  61.   // RELEASE function of library allows to stop motors if they are for some reason running
  62.   myMotor1->run(RELEASE);
  63.   myMotor3->run(RELEASE);
  64.   /////////// Setting Pins to I/Os - Sensor 1 ///////////
  65.   pinMode(Gnd_PIN, OUTPUT);             //Setting the ground pin to an output
  66.   digitalWrite(Gnd_PIN, LOW);           //Setting the ground pin low, 0v
  67.   pinMode(Vcc_PIN, OUTPUT);             //Setting the supply voltage to an output
  68.   digitalWrite(Vcc_PIN, HIGH);          //Setting the supply voltage to 5v
  69.   pinMode(Echo_PIN, INPUT);              //Setting ECHO pin to an input
  70.   pinMode(Trig_PIN, OUTPUT);             //Setting TRIGGER pin to an output
  71.  
  72. /////////// Setting Pins to I/Os - Sensor 2 ///////////
  73.   pinMode(Gnd2_PIN, OUTPUT);             //Setting the ground pin to an output
  74.   digitalWrite(Gnd2_PIN, LOW);           //Setting the ground pin low, 0v
  75.   pinMode(Vcc2_PIN, OUTPUT);             //Setting the supply voltage to an output
  76.   digitalWrite(Vcc2_PIN, HIGH);          //Setting the supply voltage to 5v
  77.   pinMode(Echo2_PIN, INPUT);              //Setting ECHO pin to an input
  78.   pinMode(Trig2_PIN, OUTPUT);             //Setting TRIGGER pin to an output
  79.   pinMode(13, OUTPUT); //set arduino LED to an output
  80.   digitalWrite(13, HIGH);    // turn on Arduino's LED to indicate we are in calibration mode
  81.   Serial.println("Calibrating");
  82.  
  83.   // make the calibration take about 10 seconds
  84.   for (int i = 0; i < 400; i++)  
  85.   {
  86.     qtra.calibrate();       // reads all sensors 10 times at 2.5 ms per six sensors (i.e. ~25 ms per call)
  87.   }
  88.   digitalWrite(13, LOW);     // turn off Arduino's LED to indicate we are through with calibration
  89.  
  90.   // print the calibration minimum values measured when emitters were on
  91.   for (int i = 0; i < NUM_SENSORS; i++)
  92.   {
  93.     Serial.print(qtra.calibratedMinimumOn[i]);
  94.     Serial.print(' ');
  95.   }
  96.   Serial.println();
  97.  
  98.   // print the calibration maximum values measured when emitters were on
  99.   for (int i = 0; i < NUM_SENSORS; i++)
  100.   {
  101.     Serial.print(qtra.calibratedMaximumOn[i]);
  102.     Serial.print(' ');
  103.   }
  104.   Serial.println();
  105. }
  106.  
  107. void loop() {
  108. /////////// Ultrasonic Sensor Serial Coms to display value //////////
  109. unsigned int position = qtra.readLine(sensorValues);
  110. for (unsigned char i = 0; i < NUM_SENSORS; i++)
  111. {
  112.   Serial.print(sensorValues[i]);
  113.   Serial.print('\t');
  114. }
  115.   unsigned int uS1 = sonar1.ping();       // Send ping, get ping time in microseconds (uS).
  116.   Distance1 = (uS1 / US_ROUNDTRIP_CM);
  117.   delay(50);
  118.   unsigned int uS2 = sonar2.ping();       // Send ping, get ping time in microseconds (uS).
  119.   Distance2 = (uS2 / US_ROUNDTRIP_CM);
  120.  
  121. /////////// Serial Print Ultrasonic Sensor 1 ///////////
  122.   Serial.print("Ping1: ");
  123.   Serial.print(Distance1);                // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  124.   Serial.print("cm  ");                   // Serial Display Units in Centimeters
  125.  
  126. /////////// Serial Print Ultrasonic Sensor 2 ///////////
  127.   Serial.print("Ping2: ");
  128.   Serial.print(Distance2);                // Convert ping time to distance and print result (0 = outside set distance range, no ping echo)
  129.   Serial.println("cm");                   // Serial Display Units in Centimeters
  130.  
  131.  
  132.  
  133.  
  134.  
  135.  
  136.  
  137.  
  138.  
  139.  
  140. ///////////////////////SIMPLE LINE FOLLOWER/////////////////////////////
  141. //If robot have not triggered many sensors and our current state is not 10(no black line)
  142. if(Program == 0){
  143. int wasUsed = 0; //Sensor readings could read dark for more than 1 sensor, this will ensure condition is only triggered once
  144.  
  145.   //left hard
  146.   if(sensorValues[7] > dark && wasUsed == 0){
  147.     myMotor1->run(BACKWARD);
  148.     myMotor3->run(FORWARD);
  149.     myMotor1->setSpeed(30);  
  150.     myMotor3->setSpeed(110);
  151.     Serial.println("left hard");
  152.     wasUsed = 1;
  153.     delay(waiting);
  154.   }
  155.   //right hard
  156.   if(sensorValues[0] > dark && wasUsed == 0){
  157.     myMotor1->run(FORWARD);
  158.     myMotor3->run(BACKWARD);
  159.     myMotor1->setSpeed(110);  
  160.     myMotor3->setSpeed(30); // was 25  
  161.     Serial.println("right hard");
  162.     wasUsed = 1;
  163.     delay(waiting);
  164.   }
  165.  
  166.   //left medium
  167.   if(sensorValues[6] > dark && wasUsed == 0){
  168.     myMotor1->run(BACKWARD);
  169.     myMotor3->run(FORWARD);
  170.     myMotor1->setSpeed(30);  
  171.     myMotor3->setSpeed(100);
  172.     Serial.println("left medium");
  173.     wasUsed = 1;
  174.     delay(waiting);
  175.   }
  176.  
  177.   //right medium
  178.   if(sensorValues[1] > dark && wasUsed == 0){
  179.     myMotor1->run(FORWARD);
  180.     myMotor3->run(BACKWARD);
  181.     myMotor1->setSpeed(100);  
  182.     myMotor3->setSpeed(30); // was 25  
  183.     Serial.println("right medium");
  184.     wasUsed = 1;
  185.     delay(waiting);
  186.   }
  187.  
  188.   //left low
  189.   if(sensorValues[5] > dark && wasUsed == 0){
  190.     myMotor1->run(FORWARD);
  191.     myMotor3->run(FORWARD);
  192.     myMotor1->setSpeed(50);  
  193.     myMotor3->setSpeed(75); // was 25  
  194.     Serial.println("left low");
  195.     wasUsed = 1;
  196.     delay(waiting);
  197.   }
  198.  
  199.   //right low
  200.   if(sensorValues[2] > dark && wasUsed == 0){
  201.     myMotor1->run(FORWARD);
  202.     myMotor3->run(FORWARD);
  203.     myMotor1->setSpeed(75);  
  204.     myMotor3->setSpeed(50); // was 25  
  205.     Serial.println("right low");
  206.     wasUsed = 1;
  207.     delay(waiting);
  208.   }
  209.  
  210.   //forward
  211.   if((sensorValues[4] > dark || sensorValues[3] > dark) && (wasUsed == 0)){
  212.     myMotor1->run(FORWARD);
  213.     myMotor3->run(FORWARD);
  214.     myMotor1->setSpeed(70);  
  215.     myMotor3->setSpeed(75);
  216.     Serial.println("forward");
  217.     wasUsed = 1;
  218.     delay(waiting);
  219.   }
  220.   if ((Distance1 <= 15) && (Distance1 != 0)){
  221.     Program = 1;                   /////////// Detect obstacle at <=10cm - Set 1 in Data Register
  222.     myMotor1->run(RELEASE);
  223.     myMotor3->run(RELEASE);
  224.     Serial.println("Program set to 1");
  225.     delay(1000);
  226.   }
  227. }// simple line follower ends here
  228. ///////////////////////SIMPLE LINE FOLLOWER///////////////
  229.  
  230.  
  231. /////////// If the Ultrasonic Sensor 1 detects an object less than 10cm, a 1 is set in Program Data Register to initiate Obstacle Avoidance ///////////
  232. if(Program == 1){
  233.   Serial.println("Im in Program 1");
  234.   if((Distance2 <= 15) && (Distance2 != 0)){                   /////////// Once uS2 detects object, +1 into Program register to move program on
  235.     Program = 2;
  236. }
  237.     myMotor1->run(BACKWARD);
  238.     myMotor3->run(FORWARD);
  239.     myMotor1->setSpeed(50);  
  240.     myMotor3->setSpeed(50);
  241.     Serial.println("Turn left");
  242. }
  243.  
  244. ///////////////////////////////////
  245. if(Program == 2){
  246.   Serial.println("Im in program 2");
  247.   if((sensorValues[0] > dark) || (sensorValues[1] > dark) || (sensorValues[2] > dark) || (sensorValues[3] > dark) || (sensorValues[4] > dark) || (sensorValues[5] > dark)
  248.   || (sensorValues[6] > dark) || (sensorValues[7] > dark)){
  249.     Program = 0;
  250.     myMotor1->run(RELEASE);
  251.     myMotor3->run(RELEASE);  
  252.     Serial.println("Set program to Line Follower");
  253.     delay (1000);
  254. }
  255.     myMotor1->run(FORWARD);
  256.     myMotor3->run(FORWARD);
  257.     myMotor1->setSpeed(60);  
  258.     myMotor3->setSpeed(40);
  259.     Serial.println("Driving in arc");
  260. }
  261. } //LOOP
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!
 
Top