Update, Matrix funktioniert

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,13 +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); 
@@ -58,9 +89,13 @@ void setup() {
   display.setTextSize(2);
   display.println("Leipzig");
   display.display();
+  Serial.println("Hello.");
+}
+
+void dbg_println(char* val){
+  Serial.println(val);
 }
 
-#if __GNUC__ && __AVR__
 /**
  * @param leftMotor -255 bis 255, Stärke
  * @param rightMotor -255 bis 255, Stärke
@@ -83,6 +118,7 @@ void setMotors(int leftMotor, int rightMotor) {
     analogWrite(PIN_IN4, leftMotor);
     digitalWrite(PIN_IN3, HIGH);
   }
+
 }
 
 void stopMotors() {
@@ -149,50 +185,75 @@ int lookRight(){
 
 #endif
 
-int getX(int b, int alpha) {
-  if (alpha<=90)
-  {
-    return cos(alpha)*b;
-  } else
-  {
-    return cos(alpha)*b;
+#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;
   }
 
-int getY(int b, int alpha) {
-  return sin(alpha)*b;
+  void setDisplaytext(char* text) {
+    std::cout<<"[LCD]: "<<text;
   }
 
+#endif
+
 void loop() {
   char s[50];
-  for (int i = 0; i <= 180; i+=1)
+  for (int i = 0; i <= 180; i+=10)
   {
     int distance = scanDirection(i);
     int x = getX(distance, i);
     int y = getY(distance, i);
+    test.increment((x/SCALE)+SIZE/2,y/SCALE);
     sprintf(s,"X: %d, Y: %d, b: %d, alpha: %d\n",x,y,distance,i);
-    Serial.println(s);
-    setDisplaytext(s, display);
+    dbg_println(s);
+    //setDisplaytext(s);
+    dbg_println("============ARRAY================");
+    char* tmpstring;
+    for (unsigned char i = SIZE; i > 0; --i)
+    {
+      for (unsigned char j = 0; j < SIZE; ++j)
+      {
+        #if __GNUC__ && __AVR__
+          Serial.print(+test.get(j,i));
+          Serial.print(" ");
+        #else
+          std::cout << +test.get(j, i) << " ";
+        #endif
+      }
+      #if __GNUC__ && __AVR__
+        Serial.println();
+      #else
+        std::cout << std::endl;
+      #endif
+    }
+    dbg_println("============ARRAY=ENDE===========");
+    {
+      
+    };
+    #ifndef __AVR__
+      usleep(500000);
+    #endif
   }
 }
 
 
-
-
 #ifndef __AVR__
 
 int main()
 { 
- setup();
+ //setup(); //Not needed (at least until now)
  while(1){loop();}
 }
 
-int scanDirection(int angle) {
-  return 20;
-}
 
-int setDisplaytext(String text, Adafruit_SSD1306 display) {
-  std::cout<<text;
-}
 #endif
+
+
+
+