AUTONOME obstakel ONTWIJKT (5 / 5 stap)

Stap 5: PROGRAM:-

Const int loopPeriod = 20;

unsigned long loopdelay = 0;

Const int sensor = 12; ARDUINO UITGANG PINS VOOR SENSOR INPUT/OUTPUT

Const int bp1 = 7; ARDUINO UITGANG PINS VOOR MOTOR INGANGEN

Const int bp2 = 8; ARDUINO UITGANG PINS VOOR MOTOR INGANGEN

Const int bp3 = 4; ARDUINO UITGANG PINS VOOR MOTOR INGANGEN

Const int bp4 = 2; ARDUINO UITGANG PINS VOOR MOTOR INGANGEN

int ultraDistance;

int ultraDuration;

VOID Setup

{

Serial.begin(9600);

pinMode (bp1, OUTPUT);

pinMode (bp2, OUTPUT);

pinMode (bp3, OUTPUT);

pinMode (bp4, OUTPUT);

}

void loop

{

if(Millis() - loopdelay > = loopPeriod)

{

readUltrasonicSensors();

motorstate();

loopdelay = millis();

}

}

VOID readUltrasonicSensors()

{

pinMode (sensor, OUTPUT);

digitalWrite (sensor, laag);

delay(2);

digitalWrite (sensor, hoge);

delayMicroseconds(10);

digitalWrite (sensor, laag);

pinMode (sensor, INPUT);

ultraDuration = pulseIn (sensor, hoge);

ultraDistance = (ultraDuration/2) / 29;

}

VOID motorstate() / / MOTOR controle functie

{

Als ((ultraDistance > 15) || (ultraDistance < 0))

{

digitalWrite (bp3, laag);

digitalWrite (bp1, hoge);

digitalWrite (bp2, hoge);

digitalWrite (bp4, laag);

}

anders

{

digitalWrite (bp1, hoge);

digitalWrite (bp2, laag);

digitalWrite (bp3, laag);

digitalWrite (bp4, hoge);

}

}