Improve test function, fix "tuning" values

.gitignore

@@ -1,34 +1,3 @@
 .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
+*a.out.*

src/Nibble_arr.cpp

@@ -1,72 +0,0 @@
-#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

@@ -1,20 +0,0 @@
-#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,36 +1,12 @@
-#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; 
@@ -40,62 +16,10 @@ const int PIN_IN4 = 10;
 
 const int SERVO_PIN = 7; 
 
-Nibble_arr test;
-
-
-
+Ultrasonic ultrasonic(A2,A3, 80000UL);
 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
@@ -103,11 +27,11 @@ void dbg_println(char* val){
 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)
@@ -118,7 +42,6 @@ void setMotors(int leftMotor, int rightMotor) {
     analogWrite(PIN_IN4, leftMotor);
     digitalWrite(PIN_IN3, HIGH);
   }
-
 }
 
 void stopMotors() {
@@ -135,12 +58,10 @@ void enableHigh() {
   digitalWrite(PIN_ENB,HIGH);
 }
 
-
-
 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);
@@ -156,21 +77,6 @@ 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);
@@ -183,77 +89,95 @@ int lookRight(){
   return ultrasonic.distanceRead();
 }
 
-#endif
+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();
+}
 
-#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;
-  }
+// put your setup code here, to run once:
 
-  void dbg_println(char* val){
-    std::cout << val << std::endl;
-  }
+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 setDisplaytext(char* text) {
-    std::cout<<"[LCD]: "<<text;
-  }
+  // Display Initialisieren 
+  setServo(90);
+  //test();
  
-#endif
+ 
+}
 
 void loop() {
-  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
-      }
-      #if __GNUC__ && __AVR__
-        Serial.println();
-      #else
-        std::cout << std::endl;
-      #endif
-    }
-    dbg_println("============ARRAY=ENDE===========");
-    {
+  enableHigh();
+  setServo(90);
+  distance = ultrasonic.distanceRead();
  
-    };
-    #ifndef __AVR__
-      usleep(500000);
-    #endif
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+ 
+  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);
       }
+      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);
+    }
+    
+  } else {
+    if((distance>155)||(distance<=0)){
+      setMotors(255, 255);
+    } else {
+      setMotors(distance+100, distance+100);
+    }    
+  }
+  delay(10);
+  
 }
-
-
-#ifndef __AVR__
-
-int main()
-{ 
- //setup(); //Not needed (at least until now)
- while(1){loop();}
-}
-
-
-#endif
-
-
-
-