Codigo Auto Velocidad

// Setup Motor A (front and rear) pins
    int enableA = 4;
    int pinA1 = 5;
    int pinA2 = 6;
    // Setup Motor B (front and rear) pins
    int enableB = 8;
    int pinB1 = 9;
    int pinB2 = 10;
    // Setup Ultrasonic Sensor pins
    #define trigPin 2
    #define echoPin 3
    int distance_0;
    
    void setup() {
      // The setup code goes here and runs once only
      // Configure the pin modes for each drive motor
       pinMode (enableA, OUTPUT);
       pinMode (pinA1, OUTPUT);
       pinMode (pinA2, OUTPUT);
       pinMode (enableB, OUTPUT);
       pinMode (pinB1, OUTPUT);
       pinMode (pinB2, OUTPUT);
       // Configure the pin modes for the Ultrasonic Sensor
       pinMode(trigPin, OUTPUT);
       pinMode(echoPin, INPUT);
       
       Serial.begin(9600);
    }
    void loop() {
      // Main code goes here and will run repeatedly:
      car(); // function keeps moving car forward while distance of objects in front are > 15cm away
     //avoid(); // function makes car go back, turn slightly right to move forward in new direction
     //motorAon();
     //motorBon();
     //forward(1);
     //distance_0 = distance();
     Serial.println(distance_0);
    }
    // Create motor functions
    void motorAforward() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, LOW);
    }
    void motorBforward() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, HIGH);
    }
    
    void motorAforwardV(int v) {
     analogWrite (pinA1, v);
     digitalWrite (pinA2, LOW);
    }
    void motorBforwardV(int v) {
     digitalWrite (pinB1, LOW);
     analogWrite (pinB2, v);
    }

    
    void motorAbackward() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, HIGH);
    }
    void motorBbackward() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, LOW);
    }
    void motorAstop() {
     digitalWrite (pinA1, HIGH);
     digitalWrite (pinA2, HIGH);
    }
    void motorBstop() {
     digitalWrite (pinB1, HIGH);
     digitalWrite (pinB2, HIGH);
    }
    void motorAcoast() {
     digitalWrite (pinA1, LOW);
     digitalWrite (pinA2, LOW);
    }
    void motorBcoast() {
     digitalWrite (pinB1, LOW);
     digitalWrite (pinB2, LOW);
    }
    void motorAon() {
     digitalWrite (enableA, HIGH);
    }
    void motorBon() {
     digitalWrite (enableB, HIGH);
    }
    void motorAoff() {
     digitalWrite (enableA, LOW);
    }
    void motorBoff() {
     digitalWrite (enableB, LOW);
    }
    // Setup movement functions
    void forward (int duration) {
     motorAforward();
     motorBforward();
     delay (duration);
    }

    void velocidad (int duration, int v) {
     motorAforwardV(v);
     motorBforwardV(v);
     delay (duration);
    }
    void backward (int duration) {
     motorAbackward();
     motorBbackward();
     delay (duration);
    }
    void right (int duration) {
     motorAbackward();
     motorBforward();
     delay (duration);
    }
    void left (int duration) {
     motorAforward();
     motorBbackward();
     delay (duration);
    }
    void coast (int duration) {
     motorAcoast();
     motorBcoast();
     delay (duration);
    }
    void breakRobot (int duration) {
     motorAstop();
     motorBstop();
     delay (duration);
    }
    void disableMotors() {
     motorAoff();
     motorBoff();
    }
    void enableMotors() {
     motorAon();
     motorBon();
    }
    // Setup Ultrasonic Sensor distance measuring
    int distance() {
      int duration, distance;
      digitalWrite(trigPin, HIGH);
      delayMicroseconds(1000);
      digitalWrite(trigPin, LOW);
      duration = pulseIn(echoPin, HIGH);
      distance = (duration/2) / 29.1;
      return distance;
    }
    // Setup the main car function
    void car() {
    int distance_0;
    distance_0 = distance();
      // Keep moving forward in a straight line while distance of objects in front > 15cm away
      while(distance_0 < 255/3)
      {
         motorAon();
         motorBon();
         //forward(1);
         velocidad(1,255-distance_0*3);
         distance_0 = distance();
         Serial.println(distance_0);
      }
      breakRobot(0);
    }
    // Go back and turn slightly right to move car in new direction
    // This function only runs if an obstacle within 15cm is detected
    void avoid()
    {
        backward(500);
        right(360);
    }

Deja un comentario

Tu dirección de correo electrónico no será publicada. Los campos obligatorios están marcados con *