Update, Matrix funktioniert

.gitignore

@@ -1,3 +1,34 @@
 .pioenvs
 .piolibdeps
-*a.out.*
+
+
+# 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,12 +1,36 @@
+#if __GNUC__ && __AVR__
 #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
+
+#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; 
@@ -16,10 +40,62 @@ const int PIN_IN4 = 10;
 
 const int SERVO_PIN = 7; 
 
-Ultrasonic ultrasonic(A2,A3, 80000UL);
+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); 
+  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();
+  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
@@ -27,11 +103,11 @@ int distance = 21;
 void setMotors(int leftMotor, int rightMotor) {
   if (rightMotor>=0)
   {
-    analogWrite(PIN_IN1, rightMotor);
-    digitalWrite(PIN_IN2, LOW);
+    analogWrite(PIN_IN2, rightMotor);
+    digitalWrite(PIN_IN1, LOW);
   } else {
-    analogWrite(PIN_IN1, rightMotor);
-    digitalWrite(PIN_IN2, HIGH);
+    analogWrite(PIN_IN2, rightMotor);
+    digitalWrite(PIN_IN1, HIGH);
   }
 
   if (leftMotor>=0)
@@ -42,6 +118,7 @@ void setMotors(int leftMotor, int rightMotor) {
     analogWrite(PIN_IN4, leftMotor);
     digitalWrite(PIN_IN3, HIGH);
   }
+
 }
 
 void stopMotors() {
@@ -58,10 +135,12 @@ void enableHigh() {
   digitalWrite(PIN_ENB,HIGH);
 }
 
+
+
 void setServo(int degrees) {
-  int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 550;
+  int lenMicroSecondsOfPulse = (float) degrees/ 90.0 * 1000 + 300;
   int lenMicroSecondsOfPeriod = 20 * 1000; // 20 milliseconds (ms) 
-  for (int i = 0; i < 300; ++i) {
+  for (int i = 0; i < 200; ++i) {
     digitalWrite(SERVO_PIN, HIGH); 
     // Delay for the length of the pulse
     delayMicroseconds(lenMicroSecondsOfPulse);
@@ -77,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);
@@ -89,95 +183,77 @@ 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();
-}
+#endif
 
-// put your setup code here, to run once:
+#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 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);
+  void dbg_println(char* val){
+    std::cout << val << std::endl;
+  }
 
-  // Display Initialisieren 
-  setServo(90);
-  //test();
+  void setDisplaytext(char* text) {
+    std::cout<<"[LCD]: "<<text;
+  }
 
- 
-}
+#endif
 
 void loop() {
-  enableHigh();
-  setServo(90);
-  distance = ultrasonic.distanceRead();
- 
- 
- 
- 
- 
- 
- 
- 
- 
- 
-  Serial.println(distance);
-  if (distance<30 && distance>0) {
-    setMotors(0, 0);
-    if(lookLeft()>lookRight()){
-      setServo(90);
-      while(distance<50) {
-        distance = ultrasonic.distanceRead();
-        Serial.println("Turning left: ");
-        Serial.println(distance);
-        setMotors(-140,0);
-        //delay(10);
+  char s[50];
+  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);
+    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
       }
-      delay(500);
-    } else {
-      setServo(90);
-      while(distance<50) {
-        distance = ultrasonic.distanceRead();
-        Serial.println("Turning right: ");
-        Serial.println(distance);
-        setMotors(0,-140);
-        //delay(10);
-      }
-      delay(500);
+      #if __GNUC__ && __AVR__
+        Serial.println();
+      #else
+        std::cout << std::endl;
+      #endif
     }
+    dbg_println("============ARRAY=ENDE===========");
+    {
       
-  } else {
-    if((distance>155)||(distance<=0)){
-      setMotors(255, 255);
-    } else {
-      setMotors(distance+100, distance+100);
+    };
+    #ifndef __AVR__
+      usleep(500000);
+    #endif
   }
-  }
-  delay(10);
-  
 }
+
+
+#ifndef __AVR__
+
+int main()
+{ 
+ //setup(); //Not needed (at least until now)
+ while(1){loop();}
+}
+
+
+#endif
+
+
+
+