//Bevægelse setup
#define dirPinR 11
#define pwmPinR 6
#define dirPinL 10
#define pwmPinL 5
#define motSpdR 255
#define motSpdL 200
#define turnTime 400
//sensor setup
#define trigPin 2 ´
#define echoPin 3
#define Afstand 10
#define TriggerAfstand 10
#define enclosedDistance 10
//objecter
int Foran;
int Right;
int Left;
int timeoutEcho;
int ByteReceived
void setup()
{
Serial.begin(9600);
pinMode(dirPinR,OUTPUT);
pinMode(pwmPinR,OUTPUT);
pinMode(dirPinL,OUTPUT);
pinMode(pwmPinL, OUTPUT);
pinMode(trigPin, OUTPUT);
pinMode(echoPin,INPUT);
timeoutEcho = (Afstand/30000)*2000000;
}
void loop()
{
Afstand = scan();
if(Foran < TriggerAfstand)
{
Serial.print(“Obstacle detected”);
Stop();
TurnVenstre();
delay(turnTime0.75);
Stop();
delay(200);
Left = scan();
delay(10);
TurnHojre();
delay(turnTime2);
Stop();
delay(200);
Right = scan();
if(Left < enclosedDistance && Right < enclosedDistance)
{
TurnHojre();
delay(turnTime);
}
else
{
if (Left < Right)
{
KorForan();
}
else
{
TurnVenstre();
delay(turnTime*1.75);
KorForan();
}
}
}
else{
if (Serial.available() > 0)
{
ByteReceived = Serial.read();
Serial.print(ByteReceived);
Serial.print(ByteReceived, HEX);
Serial.print(char(ByteReceived));
if(ByteReceived == ‘1’)
{
Serial.print(" Kører foran ");
KorForan();
}
if(ByteReceived == ‘2’)
{
Tilbage();
}
if(ByteReceived == ‘3’)
{
TurnHojre();
}
if(ByteReceived == ‘4’)
{
TurnVenstre();
}
Serial.println(); // End the line
}
}
delay(50);
}
int scan()
{
long duration, distance;
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin,LOW);
duration = pulseIn(echoPin, HIGH, 2000);
distance = duration / 58;
if(distance == 0){
distance = 999;
}
return distance;
}
void KorForan()
{
digitalWrite(dirPinR, LOW);
digitalWrite(dirPinL, HIGH);
analogWrite(pwmPinR, motSpdR);
analogWrite(pwmPinL, motSpdL);
}
void Tilbage()
{
digitalWrite(dirPinR, HIGH);
digitalWrite(dirPinL, LOW);
analogWrite(pwmPinR, motSpdR);
analogWrite(pwmPinL, motSpdL);
}
void TurnHojre()
{
digitalWrite(dirPinR, HIGH);
digitalWrite(dirPinL, HIGH);
analogWrite(pwmPinR, motSpdR);
analogWrite(pwmPinL, motSpdL);
}
void TurnVenstre()
{
digitalWrite(dirPinR, LOW);
digitalWrite(dirPinL, LOW);
analogWrite(pwmPinR, motSpdR);
analogWrite(pwmPinL, motSpdL);
}
void Stop()
{
digitalWrite(dirPinR, LOW);
digitalWrite(dirPinL, LOW);
analogWrite(pwmPinR, 0);
analogWrite(pwmPinL, 0);
}