Simulation

.gitignore

@@ -1,2 +1,34 @@
 .pioenvs
 .piolibdeps
+
+
+# Cache files for Sublime Text
+*.tmlanguage.cache
+*.tmPreferences.cache
+*.stTheme.cache
+
+# Workspace files are user-specific
+*.sublime-workspace
+
+# Project files should be checked into the repository, unless a significant
+# proportion of contributors will probably not be using Sublime Text
+# *.sublime-project
+
+# SFTP configuration file
+sftp-config.json
+
+# Package control specific files
+Package Control.last-run
+Package Control.ca-list
+Package Control.ca-bundle
+Package Control.system-ca-bundle
+Package Control.cache/
+Package Control.ca-certs/
+Package Control.merged-ca-bundle
+Package Control.user-ca-bundle
+oscrypto-ca-bundle.crt
+bh_unicode_properties.cache
+
+# Sublime-github package stores a github token in this file
+# https://packagecontrol.io/packages/sublime-github
+GitHub.sublime-settings

src/main.cpp

@@ -29,6 +29,7 @@ const int SERVO_PIN = 7;
 
 Ultrasonic ultrasonic(A2,A3);
 int distance = 21;
+int motorPower = 255;
 
   
 // put your setup code here, to run once:
@@ -58,6 +59,8 @@ void setup() {
   display.println("Leipzig");
   display.display();
 }
+
+#if __GNUC__ && __AVR__
 /**
  * @param leftMotor -255 bis 255, Stärke
  * @param rightMotor -255 bis 255, Stärke
@@ -96,6 +99,8 @@ void enableHigh() {
   digitalWrite(PIN_ENB,HIGH);
 }
 
+
+
 void setServo(int degrees) {
   int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 300;
   int lenMicroSecondsOfPeriod = 20 * 1000; // 20 milliseconds (ms) 
@@ -115,6 +120,21 @@ void setServo(int degrees) {
 
 }
 
+int scanDirection(int angle) {
+  setServo(angle);
+  delay(200);
+  return ultrasonic.distanceRead();
+}
+
+void setDisplaytext(String text, Adafruit_SSD1306 display) {
+  display.clearDisplay();
+  display.setTextSize(2);
+  display.setTextColor(WHITE);
+  display.setCursor(0,0);
+  display.println(text);
+  display.display();
+}
+
 int lookLeft(){
   setServo(180);
   delay(1000);
@@ -127,73 +147,52 @@ int lookRight(){
   return ultrasonic.distanceRead();
 }
 
+#endif
+
+int getX(int b, int alpha) {
+  if (alpha<=90)
+  {
+    return cos(alpha)*b;
+  } else
+  {
+    return cos(alpha)*b;
+  }
+  
+}
+
+int getY(int b, int alpha) {
+  return sin(alpha)*b;
+}
 
 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);
-    if(lookLeft()>lookRight()){
-      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);
+  char s[50];
+  for (int i = 0; i <= 180; i+=1)
+  {
+    int distance = scanDirection(i);
+    int x = getX(distance, i);
+    int y = getY(distance, i);
+    sprintf(s,"X: %d, Y: %d, b: %d, alpha: %d\n",x,y,distance,i);
+    Serial.println(s);
+    setDisplaytext(s, display);
   }
-      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);
-      }
-      delay(500);
 }
 
-  } else {
-    if((distance>215)||(distance<=0)){
-      setMotors(255, 255);
-    } else {
-      setMotors(distance+70, distance+70);
-    }    
-  }
-  delay(10);
 
+
+
+#ifndef __AVR__
+
+int main()
+{
+ setup();
+ while(1){loop();}
 }
+
+int scanDirection(int angle) {
+  return 20;
+}
+
+int setDisplaytext(String text, Adafruit_SSD1306 display) {
+  std::cout<<text;
+}
+#endif