#autobot.ino

Dernière modification : 2016/12/31 02:22

const short int BUILTIN_LED2 = 16;

  1. include <NewPing.h>
NewPing sonar(D6, D7, 100);

void stop(void)
{
        analogWrite(5, 0);
        analogWrite(4, 0);
}

void forward(void)
{
        analogWrite(5, 1024);
        analogWrite(4, 1024);
        digitalWrite(0, HIGH);
        digitalWrite(2, HIGH);
}

void backward(void)
{
        analogWrite(5, 1023);
        analogWrite(4, 1023);
        digitalWrite(0, LOW);
        digitalWrite(2, LOW);
}

void left(void)
{
        analogWrite(5, 1023);
        analogWrite(4, 1023);
        digitalWrite(0, LOW);
        digitalWrite(2, HIGH);
}

void right(void)
{
        analogWrite(5, 1023);
        analogWrite(4, 1023);
        digitalWrite(0, HIGH);
        digitalWrite(2, LOW);
}

void setup()
{
        Serial.begin(115200);
        pinMode(BUILTIN_LED2, OUTPUT);
        digitalWrite(BUILTIN_LED2, LOW);

        pinMode(5, OUTPUT); // 1,2EN aka D1 pwm left
        pinMode(4, OUTPUT); // 3,4EN aka D2 pwm right
        pinMode(0, OUTPUT); // 1A,2A aka D3
        pinMode(2, OUTPUT); // 3A,4A aka D4
}

void loop()
{
        delay(50);
        Serial.print("Ping: ");
        Serial.print(sonar.ping_cm());
        Serial.println("cm");

        if (sonar.ping_cm() > 20) {
                forward();
                delay(300);
        }
        else {
                stop();
                backward();
                delay(600);
        }
}