Stap 4: Code deel
#include
#define utrigger 13 / / Arduino pin13 gekoppeld aan de trigger pin op de ultrasone sensor.
#define uecho 12 / / Arduino pin12 gebonden aan echo pin op de ultrasone sensor.
#define umaxdistance 250 / / maximale afstand die wij wilt pingen voor (in centimeters). Maximale sensor afstand wordt begroot op 400-500cm.
#define rightmotorinput1 3 //defining de 11 speld aan motor logic1 in H-brug
#define rightmotorinput2 4 //defining de 10-pin aan motor logic2 in H-brug
#define leftmotorinput1 5 //defining de 9 pin te motor logic3 in H-brug
#define leftmotorinput2 6 //defining de 8 pin om motor logic4 in H-brug
#define updownmotorinput1 7 //defining de 8 pin om motor logic4 in H-brug
#define updownmotorinput2 6
NewPing sonar (utrigger, uecho, umaxdistance);
int inByte; variabele de waarde op te slaan lees
int enb = 2;
VOID Setup
{/ / seriële poort op 9600 bps start:
Serial.begin(9200);
terwijl (!. Serieel)
{; / / wait voor seriële poort aansluiten. Alleen nodig voor Leonardo
}
pinMode(rightmotorinput1,OUTPUT);
pinMode(rightmotorinput2,OUTPUT);
pinMode(leftmotorinput1,OUTPUT);
pinMode(leftmotorinput2,OUTPUT);
pinMode(updownmotorinput1,OUTPUT);
pinMode(updownmotorinput2,OUTPUT);
pinMode(enb,INPUT);
}
void loop
{
delay(500);
digitalWrite(enb,HIGH);
unsigned int ons = sonar.ping(); Ping te sturen, ping tijd in microseconden (VS).
Serial.Print ("Ping:"); de gegevens tussen worden afgedrukt ""
Serial.Print(US / US_ROUNDTRIP_CM); Ping tijd omzetten in afstand in cm en print resultaat (0 = buiten bepaalde afstand bereik)
Serial.println("cm");
Als (Serial.available() > 0)
{
inByte = Serial.read();
schakelaar (inByte)
{
geval 'r': / / als seriële print r bot rechts wordt verplaatst.
digitalWrite(rightmotorinput1,LOW);
digitalWrite(rightmotorinput2,HIGH);
digitalWrite(leftmotorinput1,HIGH);
digitalWrite(leftmotorinput2,LOW);
breken;
geval 'l': / / als seriële print l bot links beweegt.
digitalWrite(rightmotorinput1,HIGH);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,LOW);
digitalWrite(leftmotorinput2,HIGH);
breken;
geval 'f': / / als seriële print f bot vooruit gaat.
digitalWrite(rightmotorinput1,HIGH);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,HIGH);
digitalWrite(leftmotorinput2,LOW);
breken;
Case "b": / / als seriële print b bot revers beweegt.
digitalWrite(rightmotorinput1,LOW);
digitalWrite(rightmotorinput2,HIGH);
digitalWrite(leftmotorinput1,LOW);
digitalWrite(leftmotorinput2,HIGH);
breken;
geval zou ': / / als seriële print b bot revers beweegt.
digitalWrite(updownmotorinput1,HIGH);
digitalWrite(updownmotorinput2,LOW);
delay(1000);
digitalWrite(updownmotorinput1,LOW);
digitalWrite(updownmotorinput2,LOW);
inByte = "0";
breken;
geval 'u': / / als seriële print b bot revers beweegt.
digitalWrite(updownmotorinput1,LOW);
digitalWrite(updownmotorinput2,HIGH);
delay(1000);
digitalWrite(updownmotorinput1,LOW);
digitalWrite(updownmotorinput2,LOW);
inByte = "0";
breken;
't geval ':
digitalWrite(rightmotorinput1,HIGH);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,HIGH);
digitalWrite(leftmotorinput2,LOW);
breken;
geval van ': / / als seriële print b bot revers beweegt. al:
digitalWrite(rightmotorinput1,LOW);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,LOW);
digitalWrite(leftmotorinput2,LOW);
breken;
kast '2': / / als seriële print b bot revers beweegt.
Als (ons/US_ROUNDTRIP_CM < 20 & & ons/US_ROUNDTRIP_CM! = 0) / / op afstand controleren als afstand minder dan 5cm en is niet gelijk aan nul
{
Serial.println("IOF");
digitalWrite(rightmotorinput1,LOW);
digitalWrite(leftmotorinput2,LOW);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,LOW);
Als (Serial.available()==1)
{
inByte = Serial.read();
if(inByte=='s')
{
goto al;
}
}
delay(1000);
digitalWrite(rightmotorinput2,HIGH);
digitalWrite(rightmotorinput1,LOW);
digitalWrite(leftmotorinput2,HIGH);
digitalWrite(leftmotorinput1,LOW);
Als (Serial.available()==1)
{
inByte = Serial.read();
if(inByte=='s')
{
goto al;
}
}
delay(2000);
digitalWrite(leftmotorinput1,HIGH);
digitalWrite(leftmotorinput2,LOW);
Als (Serial.available()==1)
{
inByte = Serial.read();
if(inByte=='s')
{
goto al;
}
}
delay(2000);
digitalWrite(rightmotorinput1,LOW);
digitalWrite(rightmotorinput2,HIGH);
Als (Serial.available()==1)
{
inByte = Serial.read();
if(inByte=='s')
{
goto al;
}
}
delay(1000);
}
anders
{
Serial.println("Else");
digitalWrite(rightmotorinput1,HIGH);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,HIGH);
digitalWrite(leftmotorinput2,LOW);
}
breken;
}
} /*
delay(1000);
digitalWrite(rightmotorinput1,LOW);
digitalWrite(rightmotorinput2,LOW);
digitalWrite(leftmotorinput1,LOW);
digitalWrite(leftmotorinput2,LOW);
digitalWrite(rightmotorinput1,LOW);
digitalWrite(rightmotorinput2,LOW);
*/
}