Diff: main.cpp

Revision:

11:394ab193971f

Parent:

10:d18768e771ac

Child:

12:dfc717775297

--- a/main.cpp	Wed Nov 21 12:23:52 2018 +0000
+++ b/main.cpp	Wed Nov 21 13:02:46 2018 +0000
@@ -12,7 +12,7 @@
 #define I_TERM 0
 #define D_TERM 8
 
-void sensor_all(void) //lav en funktion? ved ikke hvad jeg skal her
+int sensor_all(void) //lav en funktion? ved ikke hvad jeg skal her
      {
         int sensor_value[5]; // sensor int
         m3pi.putc(0x86); // send noget sensor data
@@ -26,7 +26,25 @@
         m3pi.cls();
         m3pi.locate(0,1);
         m3pi.printf("%d", sensor_value[0]);
-     }
+        return sensor_value[0];
+        }
+        
+int sensor_all2(void) //lav en funktion? ved ikke hvad jeg skal her
+    {
+        int sensor_value[5]; // sensor int
+        m3pi.putc(0x86); // send noget sensor data
+    
+        for(int n=0; n < 5; n++) // forloop så sensor for data
+        {
+        char lowbyte = m3pi.getc();
+        char hibyte  = m3pi.getc();
+        sensor_value[n] = ((lowbyte + (hibyte << 8))); // sensor value
+        }
+        m3pi.cls();
+        m3pi.locate(0,0);
+        m3pi.printf("%d", sensor_value[4]);
+        return sensor_value[4];
+        }
 
 int main() {
 
@@ -44,9 +62,11 @@
     float derivative,proportional,integral = 0;
     float power;
     float speed = MAX;
+    int sensor_val = 0;
+    int sensor_val2 = 0;
     
     while (1) {
-        // Get the position of the line.
+      // Get the position of the line.
         current_pos_of_line = m3pi.line_position();        
         proportional = current_pos_of_line;
         
@@ -80,9 +100,14 @@
        // set speed 
         m3pi.left_motor(left);
         m3pi.right_motor(right);
-       
+        
         //Marc's sensor test
-        sensor_all(); // sensor_value gets fkt value
+        sensor_val = sensor_all(); // sensor_value gets fkt value
+        sensor_val2 = sensor_all(); // sensor_value gets fkt value
+        if ((sensor_val > 1500) && (sensor_val2 > 1500))
+        {
+            speed = 0.2;
+            }
         /*
         sensor_value4 = sensor_all(4); // sensor_value gets fkt value
         */