KRITSADA

SumoRobot-Fast-Motors-Code

Feb 24th, 2021
996
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define sensorPin 4//3     // the number of the sensor pin
  2. int sensorState = 0;         // variable for reading the sensor status
  3.  
  4. #define echoPin 11//7 // Echo Pin
  5. #define trigPin 12//8 // Trigger Pin
  6. int maximumRange = 20; // Maximum range needed
  7. long duration, distance; // Duration used to calculate distance
  8.  
  9.  
  10. #define LEDPin 13 // Onboard LED
  11.  
  12. int IN1 = 5;
  13. int IN2 = 6;
  14. int IN3 = 7;
  15. int IN4 = 8;
  16. int ENA = 9;
  17. int ENB = 10;
  18.  
  19. void setup() {
  20.   //Serial.begin (9600);
  21.   delay(5000);
  22.   for (int i = 5; i < 11; i ++)
  23.   {
  24.     pinMode(i, OUTPUT);
  25.   }
  26.   pinMode(trigPin, OUTPUT);
  27.   pinMode(echoPin, INPUT);
  28.   pinMode(sensorPin, INPUT);
  29.   pinMode(LEDPin, OUTPUT); // Use LED indicator (if required)
  30.  
  31.   front();
  32.   delay(300);
  33. }
  34.  
  35. void loop() {
  36.  
  37.   sensorState = digitalRead(sensorPin);
  38.   // if the sensorState is HIGH:
  39.   if (sensorState == HIGH) {
  40.     digitalWrite(LEDPin, HIGH);
  41.     fight();
  42.   }
  43.   else {
  44.     digitalWrite(LEDPin, LOW);
  45.     back();
  46.     delay(200);
  47.     left();
  48.     delay(500);
  49.     //stopM();
  50.     //delay(250);
  51.   }
  52.   /*
  53.     if (black() == 0) {
  54.     //digitalWrite(LEDPin, HIGH);
  55.     right();
  56.     delay(250);
  57.     front();
  58.     delay(250);
  59.     fight();
  60.     } else if (black() == 1) {
  61.     digitalWrite(LEDPin, LOW);
  62.     back();
  63.     delay(250);
  64.     left();
  65.     delay(250);
  66.     stopM();
  67.     delay(250);
  68.     }*/
  69.  
  70. }
  71.  
  72.  
  73.  
  74.  
  75.  
  76.  
  77.  
  78.  
  79.  
  80.  
  81.  
  82.  
  83.  
  84.  
  85.  
  86.  
  87.  
  88.  
  89.  
  90.  
  91.  
  92. void stopM() {
  93.   analogWrite(ENA, 0);
  94.   analogWrite(ENB, 0);
  95. }
  96.  
  97. void front() { //left
  98.   digitalWrite(IN1, 0);
  99.   digitalWrite(IN2, 1);
  100.   digitalWrite(IN3, 0);
  101.   digitalWrite(IN4, 1);
  102.   analogWrite(ENA, 255);
  103.   analogWrite(ENB, 255);
  104. }
  105.  
  106. void back() { //right
  107.   digitalWrite(IN1, 1);
  108.   digitalWrite(IN2, 0);
  109.   digitalWrite(IN3, 1);
  110.   digitalWrite(IN4, 0);
  111.   analogWrite(ENA, 255);
  112.   analogWrite(ENB, 255);
  113. }
  114.  
  115. void right() { //back
  116.   digitalWrite(IN1, 1);
  117.   digitalWrite(IN2, 0);
  118.   digitalWrite(IN3, 0);
  119.   digitalWrite(IN4, 1);
  120.   analogWrite(ENA, 150);
  121.   analogWrite(ENB, 150);
  122. }
  123.  
  124. void left() { //front
  125.   digitalWrite(IN1, 0);
  126.   digitalWrite(IN2, 1);
  127.   digitalWrite(IN3, 1);
  128.   digitalWrite(IN4, 0);
  129.   analogWrite(ENA, 150);
  130.   analogWrite(ENB, 150);
  131. }
  132.  
  133.  
  134.  
  135.  
  136.  
  137.  
  138. void fight() {
  139.   digitalWrite(trigPin, LOW);
  140.   delayMicroseconds(2);
  141.  
  142.   digitalWrite(trigPin, HIGH);
  143.   delayMicroseconds(10);
  144.   digitalWrite(trigPin, LOW);
  145.   duration = pulseIn(echoPin, HIGH);
  146.  
  147.   //Calculate the distance (in cm) based on the speed of sound.
  148.   distance = duration / 58.2;
  149.  
  150.   if (distance <= maximumRange) {
  151.     /* Send a negative number to computer and Turn LED ON
  152.       to indicate "out of range" */
  153.     //Serial.println(distance);
  154.     digitalWrite(LEDPin, HIGH);
  155.     front();
  156.     delay(200);
  157.   }
  158.   else {
  159.     /* Send the distance to the computer using Serial protocol, and
  160.       turn LED OFF to indicate successful reading. */
  161.     //Serial.println(distance);
  162.     digitalWrite(LEDPin, LOW);
  163.     left();
  164.     delay(500);
  165.   }
  166.   //Delay 50ms before next reading.
  167.   //delay(5);
  168. }
RAW Paste Data

Adblocker detected! Please consider disabling it...

We've detected AdBlock Plus or some other adblocking software preventing Pastebin.com from fully loading.

We don't have any obnoxious sound, or popup ads, we actively block these annoying types of ads!

Please add Pastebin.com to your ad blocker whitelist or disable your adblocking software.

×