Update, Matrix funktioniert

.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/Nibble_arr.cpp

@@ -0,0 +1,72 @@
+#include "Nibble_arr.h"
+
+unsigned char Nibble_arr::extractHighNibble(unsigned char nibblePair) {
+	return (nibblePair & 0xF0)>>4;
+}
+unsigned char Nibble_arr::extractLowNibble(unsigned char nibblePair) {
+	return (nibblePair & 0x0F);
+}
+unsigned char Nibble_arr::setLowNibble(unsigned char val, unsigned char nibblePair) {
+	unsigned char nibblePairNoLower = nibblePair & 0xF0;
+	return nibblePairNoLower | (val & 0xF);
+}
+unsigned char Nibble_arr::setHighNibble(unsigned char val, unsigned char nibblePair) {
+	unsigned char nibblePairNoHigher = nibblePair & 0x0F;
+	return nibblePairNoHigher | ((val & 0xF)<<4);
+}
+
+unsigned char Nibble_arr::get(unsigned char x, unsigned char y) {
+	if (y%2==0) {
+		return extractLowNibble(array[x][y/2]);
+	} else {
+		return extractHighNibble(array[x][y/2]);
+	}
+}
+
+void Nibble_arr::increment(unsigned char x, unsigned char y) {
+	if (y%2==0)
+	{
+		unsigned char tmp = extractLowNibble(array[x][y/2]);
+		if (tmp<0xF)
+		{
+			tmp = setLowNibble(tmp+1, array[x][y/2]);
+			array[x][y/2]=tmp;
+		}
+	} else {
+		unsigned char tmp = extractHighNibble(array[x][y/2]);
+		if (tmp<0xF)
+		{
+			tmp = setHighNibble(tmp+1, array[x][y/2]);
+			array[x][y/2]=tmp;
+		}
+	}
+}
+
+void Nibble_arr::decrement(unsigned char x, unsigned char y) {
+	if (y%2==0)
+	{
+		unsigned char tmp = extractLowNibble(array[x][y/2]);
+		if (tmp>0)
+		{
+			tmp = setLowNibble(tmp-1, array[x][y/2]);
+			array[x][y/2]=tmp;
+		}
+		
+	} else {
+		unsigned char tmp = extractHighNibble(array[x][y/2]);
+		if (tmp>0)
+		{
+			char tmp = setHighNibble(tmp-1, array[x][y/2]);
+			array[x][y/2]=tmp;
+		}
+	}
+}
+
+void Nibble_arr::set(unsigned char val, unsigned char x, unsigned char y) {
+	if (y%2==0)
+	{
+    array[x][y/2] = setLowNibble(val, array[x][y/2]);
+	} else {
+		array[x][y/2] = setHighNibble(val, array[x][y/2]);
+	}
+}

src/Nibble_arr.h

@@ -0,0 +1,20 @@
+#ifndef SIZE
+#define SIZE 15
+#endif
+#ifndef SCALE
+#define SCALE 10
+#endif
+
+class Nibble_arr
+{
+	unsigned char array[SIZE][SIZE/2+1]  = {{0}};
+	unsigned char extractHighNibble(unsigned char nibblePair);
+	unsigned char extractLowNibble(unsigned char nibblePair);
+	unsigned char setLowNibble(unsigned char val, unsigned char nibblePair);
+	unsigned char setHighNibble(unsigned char val, unsigned char nibblePair);
+public:
+	unsigned char get(unsigned char x,unsigned char y);
+	void increment(unsigned char x,unsigned char y);
+	void decrement(unsigned char x,unsigned char y);
+	void set(unsigned char val, unsigned char x,unsigned char y);
+};

src/main.cpp

@@ -1,3 +1,4 @@
+#if __GNUC__ && __AVR__
 #include <Arduino.h>
 #include <Ultrasonic.h>
 
@@ -18,6 +19,18 @@ Adafruit_SSD1306 display(OLED_RESET);
 #error("Height incorrect, please fix Adafruit_SSD1306.h!");
 #endif
 
+#endif
+
+#include "Nibble_arr.h"
+#include <string.h>
+
+#ifndef __AVR__
+#include <math.h>
+#include <cstdio>
+#include <iostream>
+#include <unistd.h>
+#endif
+
 const int PIN_ENA = 0;
 const int PIN_ENB = 1;
 const int PIN_IN1 = 3; 
@@ -27,12 +40,31 @@ const int PIN_IN4 = 10;
 
 const int SERVO_PIN = 7; 
 
-Ultrasonic ultrasonic(A2,A3);
+Nibble_arr test;
+
+
+
 int distance = 21;
 
 
+int getX(int b, int alpha) {
+  return cos((M_PI/180)*alpha) * b;
+}
+
+int getY(int b, int alpha) {
+  return sin((M_PI/180)*alpha) * b;
+}
+
+
+#if __GNUC__ && __AVR__
+int motorPower = 255;
+Ultrasonic ultrasonic(A2,A3);
+
+  
 // put your setup code here, to run once:
 
+
+
 void setup() {
   Serial.begin(9600);
   pinMode(PIN_ENA,OUTPUT); 
@@ -57,7 +89,13 @@ void setup() {
   display.setTextSize(2);
   display.println("Leipzig");
   display.display();
+  Serial.println("Hello.");
 }
+
+void dbg_println(char* val){
+  Serial.println(val);
+}
+
 /**
  * @param leftMotor -255 bis 255, Stärke
  * @param rightMotor -255 bis 255, Stärke
@@ -80,6 +118,7 @@ void setMotors(int leftMotor, int rightMotor) {
     analogWrite(PIN_IN4, leftMotor);
     digitalWrite(PIN_IN3, HIGH);
   }
+
 }
 
 void stopMotors() {
@@ -96,6 +135,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 +156,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 +183,77 @@ int lookRight(){
   return ultrasonic.distanceRead();
 }
 
+#endif
+
+#ifndef __AVR__
+  int testData[] = {10,20,20,20,10,20,60,80,100,110,10,32,42,18,15,16,17,0};
+  int scanDirection(int i) {
+    //return testData[i/10];
+    return 20;
+  }
+
+  void dbg_println(char* val){
+    std::cout << val << std::endl;
+  }
+
+  void setDisplaytext(char* text) {
+    std::cout<<"[LCD]: "<<text;
+  }
+
+#endif
 
 void loop() {
-  enableHigh();
+  char s[50];
-  setServo(90);
+  for (int i = 0; i <= 180; i+=10)
-  distance = ultrasonic.distanceRead();
+  {
-  display.clearDisplay();
+    int distance = scanDirection(i);
-  display.setTextSize(2);
+    int x = getX(distance, i);
-  display.setTextColor(WHITE);
+    int y = getY(distance, i);
-  display.setCursor(0,0);
+    test.increment((x/SCALE)+SIZE/2,y/SCALE);
-  display.println("Abstand: ");
+    sprintf(s,"X: %d, Y: %d, b: %d, alpha: %d\n",x,y,distance,i);
-  display.setTextSize(3);
+    dbg_println(s);
-  display.print(distance);
+    //setDisplaytext(s);
-  display.setTextSize(2);
+    dbg_println("============ARRAY================");
-  display.println(" cm");
+    char* tmpstring;
-  display.display();
+    for (unsigned char i = SIZE; i > 0; --i)
-  Serial.println(distance);
+    {
-  if (distance<30 && distance>0) {
+      for (unsigned char j = 0; j < SIZE; ++j)
-    setMotors(0, 0);
+      {
-    if(lookLeft()>lookRight()){
+        #if __GNUC__ && __AVR__
-      setServo(90);
+          Serial.print(+test.get(j,i));
-      while(distance<50) {
+          Serial.print(" ");
-        distance = ultrasonic.distanceRead();
+        #else
-        display.clearDisplay();
+          std::cout << +test.get(j, i) << " ";
-        display.setTextSize(2);
+        #endif
-        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);
       }
-      delay(500);
+      #if __GNUC__ && __AVR__
-    } else {
+        Serial.println();
-      setServo(90);
+      #else
-      while(distance<50) {
+        std::cout << std::endl;
-        distance = ultrasonic.distanceRead();
+      #endif
-        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);
     }
+    dbg_println("============ARRAY=ENDE===========");
+    {
       
-  } else {
+    };
-    if((distance>215)||(distance<=0)){
+    #ifndef __AVR__
-      setMotors(255, 255);
+      usleep(500000);
-    } else {
+    #endif
-      setMotors(distance+70, distance+70);
   }
-  }
-  delay(10);
-  
 }
+
+
+#ifndef __AVR__
+
+int main()
+{ 
+ //setup(); //Not needed (at least until now)
+ while(1){loop();}
+}
+
+
+#endif
+
+
+
+