Roboter should now stop if the distance is lower than 20cm

Code/miniRobot/miniRobot.ino

@@ -13,8 +13,8 @@ RF24 radio(A0, 3); // CE, CSN
 
 byte commands[32];       //byte 0 = command
 long timer;
-int16_t temperature;
-int distance = 0;
+int16_t temperature = 0;
+int16_t distance = 0;
 
 void inline clearCommands() {
   for(uint8_t i=0; i<32; i++) {
@@ -61,9 +61,6 @@ bool forwardA = true;
 bool forwardB = true;
 volatile bool driveOn = false;
 
-int16_t temperatur = 0;
-
-
 volatile long driveTimeout = 0;
 volatile long driveTimeDiff = 0;
 
@@ -79,13 +76,26 @@ void setup() {
   clearCommands();
   
   //Temperatur- und Abstandsmessung
+  
+  setEchoPins(16, 6); //16: A2, 5: D5
   tempDistSetup();
-  setEchoPins(16, 5); //16: A2, 5: D5
   timer = millis(); 
 }
 
 void loop() {
+  //Temperatur- und Abstandsmessung
   Serial.println(temperature);
+  Serial.println(distance);
+  temperature = dallas(4, 0);
+  
+  if((unsigned long)(millis() - timer) >= 100){
+    measureDistance();
+    timer = millis();  
+  }
+  
+  distance = calculateDistance();
+
+  
   if (radio.available()) {
     radio.read(&commands, sizeof(commands));
     commandInterpretation();
@@ -96,20 +106,21 @@ void loop() {
     pwmB = 0;
   }
 
+    
+    if(distance < 20){
+    Serial.println("Achtung!");
+    if(pwmA < 0 && pwmB < 0){
+        pwmA = 0;
+        pwmB = 0;
+      }
+    }
+
+    
     drive.setPWM_A(pwmA);
     drive.setPWM_B(pwmB);
     
 
-  //Temperatur- und Abstandsmessung
-  
-  temperature = dallas(4, 0);
-  
-  if(millis() - timer >= 100){
-    measureDistance();
-    timer = millis();  
-  }
-  
-  distance = calculateDistance();
+    
 }
 
 void commandInterpretation() {
@@ -154,8 +165,8 @@ void commandInterpretation() {
                       driveOn = false;      
                       break;
       }
-      case getTemp :  {                     
-                      Serial.println("Senden!");
+      case getTemp :  { 
+                      Serial.println("Senden!");                    
                       radio.stopListening();                    
                       radio.write(&temperature, sizeof(int16_t));
                       radio.startListening();
@@ -180,5 +191,6 @@ void commandInterpretation() {
       }
     }
   }
+  
   clearCommands();
 }

Code/miniRobot/temperatureDistance.ino

@@ -2,8 +2,8 @@
 
 #define maxDistance 400
 
-int trig = 0;
-int echo = 0;
+int trig;
+int echo;
 
 //int distance = 0;
 int distance2 = 0;
@@ -25,8 +25,8 @@ void setEchoPins(int pin1, int pin2){
   echo = pin2;
 }
 
-int calculateDistance(){
-  int result = distance2/ 58;
+int16_t calculateDistance(){
+  int16_t result = distance2/ 58;
 
   if(result > maxDistance){
     result = maxDistance;

Code/miniRobotRC/fahrsteuerung_old.ino

@@ -52,17 +52,17 @@ void manualDigitalDrive() {
 
     //Temperatur <3
     
-    if((millis() - temp_time) >= 5000){ // jede  Sekunden
+    if((unsigned long)(millis() - temp_time) >= 1000){ // jede  Sekunden
       temp_time = millis();
       bool err = false;
       clearCommands();
       commands[0] = getTemp;
       radio.write(&commands, sizeof(commands) && !err);
-      long start = micros();
+      unsigned long start = micros();
       radio.startListening();
       while(!radio.available()){
         //Serial.println("nix");
-        if((micros()- start) >= 1){
+        if((unsigned long)(micros()- start) >= 1){
         
           err = true;
         }

Code/miniRobotRC/miniRobotRC.ino

@@ -42,7 +42,7 @@ uint16_t driveTimeout = 0;
 int16_t distance;
 int16_t temperature = 0;
 
-int16_t temp_time = millis();
+unsigned long temp_time = millis();
 
 //Funk
 #include <SPI.h>