//NICHT AKTUELL #include "WWSR.h" #include "Arduino.h" SonarRaumwahrnehmung::SonarRaumwahrnehmung(){ trigger = 34; echo = 36; rotation=0; array[180]; weitesteEntfernung; optimalerWinkel; step2=26; //Rechtes Rad dir2=28; step3=30; //Linkes Rad dir3=32; pinMode(step1, OUTPUT); //Einstellen der Stepperpins pinMode(dir1, OUTPUT); pinMode(step2, OUTPUT); pinMode(dir2, OUTPUT); pinMode(step3, OUTPUT); pinMode(dir3, OUTPUT); Serial.begin(9600); myservo.attach(42); pinMode(trigger, OUTPUT); pinMode(echo, INPUT); cm=0; inches=0; duration=0; } long SonarRaumwahrnehmung::entfernungsMesser(){ digitalWrite(trigger, LOW); delayMicroseconds(2); digitalWrite(trigger, HIGH); delayMicroseconds(10); digitalWrite(trigger, LOW); duration = pulseIn(echo, HIGH); inches = microsecondsToInches(duration); cm = microsecondsToCentimeters(duration); //Serial.println(cm, DEC); delay(10); return cm; } long microsecondsToInches(long microseconds){ return microseconds / 74 / 2; } long microsecondsToCentimeters(long microseconds){ return microseconds / 29 / 2; } void SonarRaumwahrnehmung::ausweichen(){ for(int i=90;i<180;i++) { myservo.write(i); delay(30); entfernungsMesser(); array[i]=cm; if(array[i]>weitesteEntfernung) { weitesteEntfernung=array[i]; //optimalerWinkel und weitesteEntfernung optimalerWinkel=i; //werden im Programm bis jetzt noch nicht } //verwendet. Serial.println(optimalerWinkel); } for(int i=179;i>0;i--) { myservo.write(i); delay(30); entfernungsMesser(); array[i]=cm; if(array[i]>weitesteEntfernung) { weitesteEntfernung=array[i]; optimalerWinkel=i; } Serial.println(optimalerWinkel); } myservo.write(90); if(optimalerWinkel>90) { digitalWrite(dir2, LOW); digitalWrite(dir3, LOW); Serial.println("LINKS"); } else { digitalWrite(dir2, HIGH); digitalWrite(dir3, HIGH); Serial.println("RECHTS"); } for(int j=1;j<143;j++) { //Rechnerei mit dem Abstand und Durch- digitalWrite(step2, HIGH); //messer der Räder hat ergeben, dass der digitalWrite(step3, HIGH); //sich der Roboter nach 143 steps um delay(10); //90° gedreht hat. digitalWrite(step2, LOW); digitalWrite(step3, LOW); delay(10); } delay(100); }