SHARE
TWEET

Untitled

a guest Nov 19th, 2019 76 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. const int AIN1 = 13;        
  2. const int AIN2 = 12;            
  3. const int PWMA = 11;            
  4.  
  5.  
  6. const int PWMB = 8;          
  7. const int BIN2 = 9;          
  8. const int BIN1 = 10;          
  9.  
  10. const int trigPinLeft=4;
  11. const int echoPinLeft=3;
  12. const int trigPinRight=2;
  13. const int echoPinRight=1;
  14. const int trigPin = 6;
  15. const int echoPin = 5;
  16. int switchPin = 7;            
  17.  
  18. float distance = 0;
  19. float distanceRight=0;
  20. float distanceLeft;
  21. /********************************************************************************/
  22. void setup()
  23. {
  24.   pinMode(trigPin, OUTPUT);      
  25.   pinMode(echoPin, INPUT);      
  26.  
  27.   pinMode(AIN1, OUTPUT);
  28.   pinMode(AIN2, OUTPUT);
  29.   pinMode(PWMA, OUTPUT);
  30.  
  31.   pinMode(BIN1, OUTPUT);
  32.   pinMode(BIN2, OUTPUT);
  33.   pinMode(PWMB, OUTPUT);
  34.  
  35.   Serial.begin(9600);                      
  36. }
  37.  
  38. /********************************************************************************/
  39. void loop()
  40. {
  41.   distance = getDistance();
  42.     if(distance < 13.1){              
  43.         stop();
  44.         delay(100);
  45.         goLeft();
  46.         delay(100);
  47.         distanceLeft=getDistanceLeft();
  48.         distanceRight=getDistanceRight();
  49.         while(distanceLeft<1||distanceRight<1){
  50.             if(distanceRight<1&&distanceLeft<1){
  51.                 goForwards(255);
  52.                 break;
  53.             }
  54.             else if(distanceLeft<1){
  55.                 goRight();
  56.             }
  57.             else if(distanceRight<1){
  58.                 goLeft();
  59.             }
  60.             distanceLeft=getDistanceLeft();
  61.             distanceRight=getDistanceRight();
  62.             delay(50);
  63.         }
  64.     }
  65.     else{                        
  66.       goForwards(255);
  67.     }
  68.   delay(50);                      
  69. }
  70.  
  71. /********************************************************************************/
  72. void rightMotor(int motorSpeed)                    
  73. {
  74.   if (motorSpeed > 0)                                
  75.     {
  76.         digitalWrite(AIN1, HIGH);                        
  77.         digitalWrite(AIN2, LOW);                        
  78.     }
  79.   else if (motorSpeed < 0)                          
  80.     {
  81.         digitalWrite(AIN1, LOW);                          
  82.         digitalWrite(AIN2, HIGH);                        
  83.     }
  84.   else                                                
  85.     {
  86.         digitalWrite(AIN1, LOW);                          
  87.         digitalWrite(AIN2, LOW);                        
  88.     }
  89.   analogWrite(PWMA, abs(motorSpeed));                
  90. }
  91.  
  92. /********************************************************************************/
  93. void leftMotor(int motorSpeed)                        
  94. {
  95.   if (motorSpeed > 0)
  96.     {
  97.         digitalWrite(BIN1, HIGH);                        
  98.         digitalWrite(BIN2, LOW);                        
  99.     }  
  100.   else if (motorSpeed < 0)                            
  101.     {
  102.         digitalWrite(BIN1, LOW);                          
  103.         digitalWrite(BIN2, HIGH);                        
  104.     }
  105.   else                                              
  106.     {
  107.         digitalWrite(BIN1, LOW);                          
  108.         digitalWrite(BIN2, LOW);                          
  109.     }
  110.   analogWrite(PWMB, abs(motorSpeed));                
  111. }
  112. /********************************************************************************/
  113. void goForwards(int speed){
  114.     rightMotor(speed);
  115.     leftMotor(speed);
  116. }
  117. /********************************************************************************/
  118. void goBackwards(int speed){
  119.     rightMotor(speed*-1);
  120.     leftMotor(speed*-1);
  121. }
  122. /********************************************************************************/
  123. void goRight(){
  124.     rightMotor(-255);
  125.     leftMotor(255);
  126. }
  127. /********************************************************************************/
  128. void goLeft(){
  129.     rightMotor(255);
  130.     leftMotor(-255);
  131. }
  132. /********************************************************************************/
  133. void stop(){
  134.     rightMotor(0);
  135.     leftMotor(0);
  136. }
  137. /********************************************************************************/
  138. float getDistance()
  139. {
  140.   float echoTime;                  
  141.   float calculatedDistance;        
  142.   digitalWrite(trigPin, HIGH);
  143.   delayMicroseconds(10);
  144.   digitalWrite(trigPin, LOW);
  145.  
  146.   echoTime = pulseIn(echoPin, HIGH);      
  147.   calculatedDistance = echoTime / 148.0;  
  148.   return calculatedDistance;            
  149. }
  150. /********************************************************************************/
  151. float getDistanceRight()
  152. {
  153.   float echoTimeRight;                  
  154.   float calculatedDistanceRight;        
  155.  
  156.   digitalWrite(trigPinRight, HIGH);
  157.   delayMicroseconds(10);
  158.   digitalWrite(trigPinRight, LOW);
  159.  
  160.   echoTimeRight = pulseIn(echoPinRight, HIGH);      
  161.   calculatedDistanceRight = echoTimeRight / 148.0;
  162.  
  163.   return calculatedDistanceRight;            
  164. }
  165. /********************************************************************************/
  166. float getDistanceLeft()
  167. {
  168.   float echoTimeLeft;                  
  169.   float calculatedDistanceLeft;        
  170.  
  171.   digitalWrite(trigPinLeft, HIGH);
  172.   delayMicroseconds(10);
  173.   digitalWrite(trigPinLeft, LOW);
  174.  
  175.   echoTimeLeft = pulseIn(echoPinLeft, HIGH);      
  176.   calculatedDistanceLeft = echoTimeLeft/ 148.0;
  177.  
  178.   return calculatedDistanceLeft;            
  179. }
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