Advertisement
Guest User

Untitled

a guest
Sep 17th, 2019
109
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /**
  2.  * @file         mBot Ranger Autonomous Mode.ino
  3.  * @author       IntoRobotics.com
  4.  * @version      V1.1
  5.  * @date         2016/05/13
  6.  * @description  this file is sample code for the mBot Ranger kit
  7.  */
  8.  
  9. #include "MeAuriga.h"
  10. #include <SoftwareSerial.h>
  11.  
  12. MeUltrasonicSensor ultraSensor(PORT_7); // Ultrasonic module
  13.  
  14. int distanceLimit=50;    // Maximum distance we want to ping for (in centimeters).
  15. int maxPwm=255;
  16. int halfPwm=200;
  17. int turnPwm=150;
  18. int d=500;
  19. int frontDistance;
  20.  
  21.  
  22. //Motor Left
  23. const int pwmMotor1 = 11;
  24. const int inMotor1_1 = 49;
  25. const int inMotor1_2 = 48;
  26.  
  27. //Motor Right
  28. const int pwmMotor2 = 10;
  29. const int inMotor2_1 = 47;
  30. const int inMotor2_2 = 46;
  31.  
  32. void setup()
  33. {
  34.   Serial.begin(9600);
  35.  
  36.   pinMode(pwmMotor1,OUTPUT);    //We have to set PWM pin as output
  37.   pinMode(inMotor1_1,OUTPUT);  //Logic pins are also set as output
  38.   pinMode(inMotor1_2,OUTPUT);
  39.   pinMode(pwmMotor2,OUTPUT);    //We have to set PWM pin as output
  40.   pinMode(inMotor2_1,OUTPUT);  //Logic pins are also set as output
  41.   pinMode(inMotor2_2,OUTPUT);
  42.  
  43. }
  44.  
  45. void loop()
  46. {
  47.    GetFrontDistance();  //Get the ultrasonic distance in cm
  48.  
  49.    if(frontDistance>=distanceLimit){
  50.       FullSpeedMode();  //The robot is in full speed mode
  51.     }
  52.  
  53.    else {  
  54.       ReduceSpeed();   //Reduce the speed of the robot
  55.       Stop();          //Stop the robot
  56.       do {  
  57.           TurnRight();
  58.           delay(200);  // Wait for sensor to stabilize
  59.           GetFrontDistance();  //Get the ultrasonic distance in cm  
  60.           } while(frontDistance<distanceLimit);
  61.       }
  62. }
  63.  
  64.  int GetFrontDistance(){
  65.    frontDistance=ultraSensor.distanceCm();
  66.    Serial.print("Front distance (in cm) is: "); //For debugging
  67.    Serial.println(frontDistance);
  68.    return frontDistance;
  69.    }
  70.  
  71.  void FullSpeedMode(){
  72.     digitalWrite(inMotor1_1, HIGH);
  73.     digitalWrite(inMotor1_2, LOW);
  74.     analogWrite(pwmMotor1,maxPwm);
  75.     digitalWrite(inMotor2_1, LOW);
  76.     digitalWrite(inMotor2_2, HIGH);
  77.     analogWrite(pwmMotor2,maxPwm);
  78.     Serial.println("Full speed mode"); //For debugging
  79.    }
  80.  
  81.   void ReduceSpeed(){
  82.     digitalWrite(inMotor1_1, HIGH);
  83.     digitalWrite(inMotor1_2, LOW);
  84.     analogWrite(pwmMotor1,halfPwm);//Set speed via PWM
  85.     digitalWrite(inMotor2_1, LOW);
  86.     digitalWrite(inMotor2_2, HIGH);
  87.     analogWrite(pwmMotor2,halfPwm);//Set speed via PWM
  88.     Serial.println("Reduce the speed"); //For debugging
  89.     delay(d);
  90.     }
  91.  
  92.   void Stop(){
  93.     analogWrite(pwmMotor1, 0);
  94.     analogWrite(pwmMotor2, 0);
  95.     Serial.println("Stop");
  96.     delay(d);
  97.    }
  98.  
  99.    void TurnRight(){
  100.     digitalWrite(inMotor1_1, HIGH);
  101.     digitalWrite(inMotor1_2, LOW);
  102.     analogWrite(pwmMotor1,turnPwm);//Set speed via PWM
  103.     digitalWrite(inMotor2_1, HIGH);
  104.     digitalWrite(inMotor2_2, LOW);
  105.     analogWrite(pwmMotor2,turnPwm);//Set speed via PWM
  106.     Serial.println("Turn the robot right"); //For debugging
  107.     }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement