KRITSADA

SumoRobot-Fast-Motors-Code-NEW

Feb 24th, 2021
868
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 = 30; // 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(5);
  33. }
  34.  
  35. void loop() {
  36.  
  37.   sensorState = digitalRead(sensorPin);
  38.   // if the sensorState is HIGH:
  39.   if (sensorState == HIGH) {//HIGH
  40.     digitalWrite(LEDPin, HIGH);
  41.     fight();
  42.   }
  43.   else {
  44.     digitalWrite(LEDPin, LOW);
  45.     back();
  46.     delay(2);
  47.     left();
  48.     delay(2);
  49.   }
  50.  
  51. }
  52.  
  53.  
  54.  
  55.  
  56.  
  57.  
  58.  
  59.  
  60.  
  61.  
  62.  
  63.  
  64.  
  65.  
  66.  
  67.  
  68.  
  69.  
  70.  
  71.  
  72.  
  73. void stopM() {
  74.   analogWrite(ENA, 0);
  75.   analogWrite(ENB, 0);
  76. }
  77.  
  78. void right() {
  79.   digitalWrite(IN1, 0);
  80.   digitalWrite(IN2, 1);
  81.   digitalWrite(IN3, 0);
  82.   digitalWrite(IN4, 1);
  83.   analogWrite(ENA, 255);
  84.   analogWrite(ENB, 255);
  85. }
  86.  
  87. void left() {
  88.   digitalWrite(IN1, 1);
  89.   digitalWrite(IN2, 0);
  90.   digitalWrite(IN3, 1);
  91.   digitalWrite(IN4, 0);
  92.   analogWrite(ENA, 255);
  93.   analogWrite(ENB, 255);
  94. }
  95.  
  96. void front() {
  97.   digitalWrite(IN1, 1);
  98.   digitalWrite(IN2, 0);
  99.   digitalWrite(IN3, 0);
  100.   digitalWrite(IN4, 1);
  101.   analogWrite(ENA, 150);
  102.   analogWrite(ENB, 150);
  103. }
  104.  
  105. void back() {
  106.   digitalWrite(IN1, 0);
  107.   digitalWrite(IN2, 1);
  108.   digitalWrite(IN3, 1);
  109.   digitalWrite(IN4, 0);
  110.   analogWrite(ENA, 150);
  111.   analogWrite(ENB, 150);
  112. }
  113.  
  114.  
  115.  
  116.  
  117.  
  118.  
  119. void fight() {
  120.   digitalWrite(trigPin, LOW);
  121.   delayMicroseconds(2);
  122.  
  123.   digitalWrite(trigPin, HIGH);
  124.   delayMicroseconds(10);
  125.   digitalWrite(trigPin, LOW);
  126.   duration = pulseIn(echoPin, HIGH);
  127.  
  128.   //Calculate the distance (in cm) based on the speed of sound.
  129.   distance = duration / 58.2;
  130.  
  131.   if (distance <= maximumRange) {
  132.     /* Send a negative number to computer and Turn LED ON
  133.       to indicate "out of range" */
  134.     //Serial.println(distance);
  135.     digitalWrite(LEDPin, HIGH);
  136.     front();
  137.     delay(2);
  138.   }
  139.   else {
  140.     /* Send the distance to the computer using Serial protocol, and
  141.       turn LED OFF to indicate successful reading. */
  142.     //Serial.println(distance);
  143.     digitalWrite(LEDPin, LOW);
  144.     left();
  145.     delay(2);
  146.     front();
  147.     delay(2);
  148.   }
  149.   //Delay 50ms before next reading.
  150.   //delay(50);
  151. }
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.

×