From 3971048caa06c24f233c0db2319066c30cf33cb4 Mon Sep 17 00:00:00 2001
From: Yves Ehrlich <yves.ehrlich@informatik.hs-fulda.de>
Date: Wed, 12 Feb 2020 18:57:38 +0100
Subject: [PATCH] Distanzmessung den Anforderungen entsprechend designed

---
 Code/miniRobot/miniRobot.ino           | 67 ++++++++++++--------------
 Code/miniRobot/temperatureDistance.ino | 64 ------------------------
 Code/miniRobotRC/_main.ino             |  4 +-
 3 files changed, 32 insertions(+), 103 deletions(-)

diff --git a/Code/miniRobot/miniRobot.ino b/Code/miniRobot/miniRobot.ino
index eec400d..f87e226 100644
--- a/Code/miniRobot/miniRobot.ino
+++ b/Code/miniRobot/miniRobot.ino
@@ -12,9 +12,9 @@
 RF24 radio(A0, 3); // CE, CSN
 
 byte commands[32];       //byte 0 = command
-unsigned long timer;
-int16_t temperature = 0;
-uint16_t distance = 50;
+
+
+
 
 void inline clearCommands() {
   for(uint8_t i=0; i<32; i++) {
@@ -64,6 +64,17 @@ volatile bool driveOn = false;
 volatile long driveTimeout = 0;
 volatile long driveTimeDiff = 0;
 
+volatile bool startNewMeasurement = true;
+
+volatile uint32_t pulseStart = 0;
+volatile uint32_t pulseLength = 1;
+
+volatile bool newResult = false;
+
+uint32_t timer = 0;
+int16_t temperature = 0;
+uint16_t distance = 50;
+
 void setup() {
   Serial.begin(115200);
 //  motorA.setPWM16(2,RESOLUTION);
@@ -83,49 +94,38 @@ void setup() {
 }
 
 void loop() {
-  //Temperatur- und Abstandsmessung
-  //Serial.println(temperature);
-  //Serial.println(distance);
-  
-
-  unsigned long currentMillis = millis();
-  
-  if((unsigned long)(currentMillis - timer) >= 100){
+  //Temperatur- und Abstandsmessung 
+  if((millis() - timer) >= 100){
     temperature = dallas(4, 0); 
-    measureDistance();
-    distance = calculateDistance(); 
-    timer = currentMillis;
-    
+    if(startNewMeasurement) {
+      measureDistance();
+    } 
+    timer = millis();
   }
-  
-  
-
+  if(newResult) {
+       // Serial.println("berechnen");
+        distance = calculateDistance();
+      }
   
   if (radio.available()) {
     radio.read(&commands, sizeof(commands));
     commandInterpretation();
   }
-  //Serial.println(driveOn);
+//Antrieb abschalten wenn kein neuer Fahrbefehl kommt
   if(((millis() - driveTimeDiff) > driveTimeout)) {
     pwmA = 0;
     pwmB = 0;
   }
 
-    
+//Wenn Mindestdistanz unterschritten, stopp 
     if(distance < 20){
-    //Serial.println("Achtung!");
     if(pwmA < 0 && pwmB < 0){
         pwmA = 0;
         pwmB = 0;
       }
     }
-    
-    
     drive.setPWM_A(pwmA);
-    drive.setPWM_B(pwmB);
-    
-
-    
+    drive.setPWM_B(pwmB); 
 }
 
 void commandInterpretation() {
@@ -184,21 +184,14 @@ void commandInterpretation() {
                         driveTime |= (0x00FF & commands[i+2]); 
                         driveTimeout = (long)driveTime;
                         driveTimeDiff = millis();
-                        Serial.println(driveTimeout);
+                        //Serial.println(driveTimeout);
                         break;
       }
-      default :  {      /* pwmA = 0;
-                      pwmB = 0;
-                      forwardA = true;
-                      forwardB = true;
-                      driveOn = false; */
-
-                      
-                      break; 
+      default :  {  
+                        break; 
       }
     }
   }
-  
   clearCommands();
   
 }
diff --git a/Code/miniRobot/temperatureDistance.ino b/Code/miniRobot/temperatureDistance.ino
index 8ef7dc2..d090b3d 100644
--- a/Code/miniRobot/temperatureDistance.ino
+++ b/Code/miniRobot/temperatureDistance.ino
@@ -1,58 +1,5 @@
 #include <OneWire.h>
 
-#define maxDistance 400
-
-int trig;
-int echo;
-
-unsigned long distance3 = 0;
-unsigned long distance2 = 0;
-
-//ISR for PCINT2
-ISR(PCINT2_vect) {
-  distance3 = pulseIn(echo, HIGH);
-  if(distance3 > 0){
-    distance2 = distance3;
-  }
-  
-  
-  PCICR &= ~0b00000111;
-  PCMSK2 &= ~0b00010000;
-  delayMicroseconds(10);
-}
-
-void setEchoPins(int pin1, int pin2){
-  
-  trig = pin1;
-  echo = pin2;
-  
-}
-
-int16_t calculateDistance(){
-  int16_t result2 = (int16_t)(((float) distance2/ 58.0));
-  if(result2 > maxDistance){
-    result2 = maxDistance;
-  }
-  return result2;
-}
-
-void measureDistance(){
-    
-    digitalWrite(trig, HIGH);
-    // ... wait for 10 µs ...
-    
-    delayMicroseconds(10);
-    // ... put the trigger down ...
-
-    digitalWrite(trig, LOW); 
-
-    Serial.println("messe...");
-    PCICR |= 0b00000100;
-    PCMSK2 |= (1 << echo);
-    
-    
-}
-
 int16_t dallas(int x, byte start){
   
   OneWire ds(x);
@@ -85,17 +32,6 @@ int16_t dallas(int x, byte start){
 
 void tempDistSetup(){
   dallas(4, 1);
-
-  // Initializing Trigger Output and Echo Input
-   pinMode(trig, OUTPUT);
-   pinMode(echo, INPUT);
-
-   // Reset the trigger pin and wait a half a second
-   digitalWrite(trig, LOW);
-   delayMicroseconds(500);
-
-   sei();
- 
 }
 
 void runMeasurements(){
diff --git a/Code/miniRobotRC/_main.ino b/Code/miniRobotRC/_main.ino
index 6fc4f7b..c845d9c 100644
--- a/Code/miniRobotRC/_main.ino
+++ b/Code/miniRobotRC/_main.ino
@@ -28,13 +28,13 @@ void loop() {
       lcdLines[5] = temp_str;
     refreshLCD();
   } 
-  tasten.clearButton(buttonStart);
+  tasten.clearButton(buttonStart); /*
  while(!tasten.getButtonCycle(buttonStart)){
     motorMapping();
     //updateTemp();
     refreshLCD();
   }
-  tasten.clearButton(buttonStart);
+  tasten.clearButton(buttonStart); */
   while(!tasten.getButtonCycle(buttonStart)){
     joystickSteuerung(); //TODO ()
     //updateTemp();