Code Download: Arduino_Roboter_mit_Ultraschall_sensor

short motorL_1 = 5; //Motor L vorwärts
short motorL_2 = 6; //Motor L rückwärts
short motorR_1 = 10; //Motor R vorwärts
short motorR_2 = 11; //Motor R rückwärts
short v = 255;
short led = 3;
int t = 390;

int echo = 8;
int trigger = 9;
int echomessung = 0;

void setup() {
  Serial.begin(9600);
  pinMode(motorL_1, OUTPUT);
  pinMode(motorL_2, OUTPUT);
  pinMode(motorR_1, OUTPUT);
  pinMode(motorR_2, OUTPUT);
  pinMode(led, OUTPUT);
  digitalWrite(led, LOW);
 
  pinMode(echo, INPUT);
  pinMode(trigger, OUTPUT);
 
  fahren( 0, 0 );
 
}
void loop() {
 
  digitalWrite(trigger, HIGH);
  delayMicroseconds(100);
  digitalWrite(trigger, LOW);
  echomessung = (pulseIn(echo, HIGH, 100000000)*34.3)/2/600;
 
  if(echomessung == 8) {echomessung = 1000000; }
 
  Serial.println(echomessung);
 
 
 
  if(echomessung > 15){
    fahren( 215, 200 );
    delay(200);
  }else{
    digitalWrite(led, HIGH);
    fahren( 215, -200 );
    delay( t );
    digitalWrite(led, HIGH);
    fahren( 215, 200 );
    delay( t );
    digitalWrite(led, LOW);
  }
 
 
}


void fahren(int links, int rechts) {
  if(links != 0 || rechts != 0){
 
    Serial.print("Links: ");
    Serial.print(links);
    Serial.print("   Rechts: ");
    Serial.println(rechts);
    
  }
 
  if(links < 0){
    links = -links;
    analogWrite(motorL_1, 0);
    analogWrite(motorL_2, links);
  }else{
    analogWrite(motorL_1, links);
    analogWrite(motorL_2, 0);    
  }
 
  if(rechts < 0){
    rechts = -rechts;
    analogWrite(motorR_1, 0);
    analogWrite(motorR_2, rechts);
  }else{
    analogWrite(motorR_1, rechts);
    analogWrite(motorR_2, 0);    
  }
 
}