Improve test function, fix "tuning" values

.gitignore

@@ -1,2 +1,3 @@
 .pioenvs
 .piolibdeps
+*a.out.*

src/main.cpp

@@ -1,23 +1,12 @@
 #include <Arduino.h>
 #include <Ultrasonic.h>
 
-//// Adafruit Display
-#include <SPI.h>
-#include <Wire.h>
-#include <Adafruit_GFX.h>
-#include <Adafruit_SSD1306.h>
-
 #define OLED_RESET 4
-Adafruit_SSD1306 display(OLED_RESET);
 #define NUMFLAKES 10
 #define XPOS 0
 #define YPOS 1
 #define DELTAY 2
 
-#if (SSD1306_LCDHEIGHT != 64)
-#error("Height incorrect, please fix Adafruit_SSD1306.h!");
-#endif
-
 const int PIN_ENA = 0;
 const int PIN_ENB = 1;
 const int PIN_IN1 = 3; 
@@ -27,37 +16,10 @@ const int PIN_IN4 = 10;
 
 const int SERVO_PIN = 7; 
 
-Ultrasonic ultrasonic(A2,A3);
+Ultrasonic ultrasonic(A2,A3, 80000UL);
 int distance = 21;
 
   
-// put your setup code here, to run once:
-
-void setup() {
-  Serial.begin(9600);
-  pinMode(PIN_ENA,OUTPUT); 
-  pinMode(PIN_ENB, OUTPUT);
-  pinMode(PIN_IN1, OUTPUT);
-  pinMode(PIN_IN2, OUTPUT);
-  pinMode(PIN_IN3, OUTPUT);
-  pinMode(PIN_IN4, OUTPUT);
-  pinMode(SERVO_PIN, OUTPUT);
-  digitalWrite(PIN_ENA, HIGH);
-  digitalWrite(PIN_ENB, HIGH);
-
-  // Display Initialisieren
-  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);
-  display.clearDisplay();
-  display.setTextSize(2);
-  display.setTextColor(WHITE);
-  display.setCursor(0,0);
-  display.println("2WD China Bots");
-  //display.setTextSize(3);
-  display.println("HTWK-");
-  display.setTextSize(2);
-  display.println("Leipzig");
-  display.display();
-}
 /**
  * @param leftMotor -255 bis 255, Stärke
  * @param rightMotor -255 bis 255, Stärke
@@ -65,11 +27,11 @@ void setup() {
 void setMotors(int leftMotor, int rightMotor) {
   if (rightMotor>=0)
   {
-    analogWrite(PIN_IN2, rightMotor);
-    digitalWrite(PIN_IN1, LOW);
+    analogWrite(PIN_IN1, rightMotor);
+    digitalWrite(PIN_IN2, LOW);
   } else {
-    analogWrite(PIN_IN2, rightMotor);
-    digitalWrite(PIN_IN1, HIGH);
+    analogWrite(PIN_IN1, rightMotor);
+    digitalWrite(PIN_IN2, HIGH);
   }
 
   if (leftMotor>=0)
@@ -97,9 +59,9 @@ void enableHigh() {
 }
 
 void setServo(int degrees) {
-  int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 300;
+  int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 550;
   int lenMicroSecondsOfPeriod = 20 * 1000; // 20 milliseconds (ms) 
-  for (int i = 0; i < 200; ++i) {
+  for (int i = 0; i < 300; ++i) {
     digitalWrite(SERVO_PIN, HIGH); 
     // Delay for the length of the pulse
     delayMicroseconds(lenMicroSecondsOfPulse);
@@ -127,21 +89,63 @@ int lookRight(){
   return ultrasonic.distanceRead();
 }
 
+void test(){
+  enableHigh();
+  setMotors(255, 255);
+  delay(2000);
+  stopMotors();
+  setMotors(0, 255);
+  delay(2000);
+  stopMotors();
+  setMotors(255, 0);
+  delay(2000);
+  stopMotors();
+  setMotors(-255, -255);
+  delay(2000);
+  stopMotors();
+  setMotors(0, -255);
+  delay(2000);
+  stopMotors();
+  setMotors(-255, 0);
+  delay(2000);
+  stopMotors();
+}
+
+// put your setup code here, to run once:
+
+void setup() {
+  Serial.begin(9600);
+  pinMode(PIN_ENA,OUTPUT); 
+  pinMode(PIN_ENB, OUTPUT);
+  pinMode(PIN_IN1, OUTPUT);
+  pinMode(PIN_IN2, OUTPUT);
+  pinMode(PIN_IN3, OUTPUT);
+  pinMode(PIN_IN4, OUTPUT);
+  pinMode(SERVO_PIN, OUTPUT);
+  digitalWrite(PIN_ENA, HIGH);
+  digitalWrite(PIN_ENB, HIGH);
+
+  // Display Initialisieren 
+  setServo(90);
+  //test();
+ 
+ 
+}
 
 void loop() {
   enableHigh();
   setServo(90);
   distance = ultrasonic.distanceRead();
-  display.clearDisplay();
-  display.setTextSize(2);
-  display.setTextColor(WHITE);
-  display.setCursor(0,0);
-  display.println("Abstand: ");
-  display.setTextSize(3);
-  display.print(distance);
-  display.setTextSize(2);
-  display.println(" cm");
-  display.display();
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
   Serial.println(distance);
   if (distance<30 && distance>0) {
     setMotors(0, 0);
@@ -149,49 +153,29 @@ void loop() {
       setServo(90);
       while(distance<50) {
         distance = ultrasonic.distanceRead();
-        display.clearDisplay();
-        display.setTextSize(2);
-        display.setTextColor(WHITE);
-        display.setCursor(0,0);
-        display.println("Abstand (turning left): ");
-        display.setTextSize(3);
-        display.print(distance);
-        display.setTextSize(2);
-        display.println(" cm");
-        display.display();
         Serial.println("Turning left: ");
         Serial.println(distance);
-        setMotors(-120,0);
-        delay(10);
+        setMotors(-140,0);
+        //delay(10);
       }
       delay(500);
     } else {
       setServo(90);
       while(distance<50) {
         distance = ultrasonic.distanceRead();
-        display.clearDisplay();
-        display.setTextSize(2);
-        display.setTextColor(WHITE);
-        display.setCursor(0,0);
-        display.println("Abstand (turning right): ");
-        display.setTextSize(3);
-        display.print(distance);
-        display.setTextSize(2);
-        display.println(" cm");
-        display.display();
         Serial.println("Turning right: ");
         Serial.println(distance);
-        setMotors(0,-120);
-        delay(10);
+        setMotors(0,-140);
+        //delay(10);
       }
       delay(500);
     }
     
   } else {
-    if((distance>215)||(distance<=0)){
+    if((distance>155)||(distance<=0)){
       setMotors(255, 255);
     } else {
-      setMotors(distance+70, distance+70);
+      setMotors(distance+100, distance+100);
     }    
   }
   delay(10);