color

1 files changed, 125 insertions, 70 deletions

diff --git a/CODE/CODE.ino b/CODE/CODE.ino
index 8cd2ef7..d923b2c 100644
--- a/CODE/CODE.ino
+++ b/CODE/CODE.ino
@@ -39,6 +39,25 @@ int m3encoB = 23;
 
 int softpwr = 2;
 
+int pr1 = 13;
+int pr2 = 41;
+int pr3 = 40;
+int pr4 = 39;
+int pr5 = 38;
+int pr6 = 37;
+int pr7 = 36;
+int pr8 = 35;
+
+int dpr1 = "LOW";
+int dpr2 = "LOW";
+int dpr3 = "LOW";
+int dpr4 = "LOW";
+int dpr5 = "LOW";
+int dpr6 = "LOW";
+int dpr7 = "LOW";
+int dpr8 = "LOW";
+
+
 void setup() {
   pinMode(m1enA, OUTPUT);
   pinMode(m1enB, OUTPUT);
@@ -57,6 +76,15 @@ void setup() {
 
   pinMode(softpwr, INPUT_PULLUP);
 
+  pinMode(pr1, INPUT);
+  pinMode(pr2, INPUT);
+  pinMode(pr3, INPUT);
+  pinMode(pr4, INPUT);
+  pinMode(pr5, INPUT);
+  pinMode(pr6, INPUT);
+  pinMode(pr7, INPUT);
+  pinMode(pr8, INPUT);
+
   Serial.begin(9600);
   Serial1.begin(9600);
   Serial2.begin(9600);
@@ -150,80 +178,107 @@ void drive(int speed, int direction){
 void loop() {
   int sprst = digitalRead(softpwr);  //softpower status
   if(sprst == LOW){
-    if (Serial1.available() > 0) {
-      incomingbyte1 = Serial1.read();
-      //Serial.print(char(incomingbyte1));
-      if (incomingbyte1 == 's'){
-        iswriting1 = true;
-        tlocdat1 = "";
-      }else if(incomingbyte1 == 'e'){
-        iswriting1 = false;
-        locdat1 = tlocdat1;        
-      }else if(iswriting1 == true){
+    dpr1 = digitalRead(pr1);
+    dpr2 = digitalRead(pr2); 
+    dpr3 = digitalRead(pr3); 
+    dpr4 = digitalRead(pr4); 
+    dpr5 = digitalRead(pr5); 
+    dpr6 = digitalRead(pr6); 
+    dpr7 = digitalRead(pr7); 
+    dpr8 = digitalRead(pr8); 
+  
+    if(dpr1 == HIGH){
+      //go back to ?deg
+    }else if (dpr2 == HIGH){
+      //go back to ?deg
+    }else if (dpr3 == HIGH){
+      //go back to ?deg
+    }else if (dpr4 == HIGH){
+      //go back to ?deg
+    }else if (dpr5 == HIGH){
+      //go back to ?deg
+    }else if (dpr6 == HIGH){
+      //go back to ?deg
+    }else if (dpr7 == HIGH){
+      //go back to ?deg
+    }else if (dpr8 == HIGH){
+      //go back to ?deg
+    }else{
+      if (Serial1.available() > 0) {
+        incomingbyte1 = Serial1.read();
         //Serial.print(char(incomingbyte1));
-        tlocdat1 += (char)incomingbyte1;  
+        if (incomingbyte1 == 's'){
+          iswriting1 = true;
+          tlocdat1 = "";
+        }else if(incomingbyte1 == 'e'){
+          iswriting1 = false;
+          locdat1 = tlocdat1;        
+        }else if(iswriting1 == true){
+          //Serial.print(char(incomingbyte1));
+          tlocdat1 += (char)incomingbyte1;  
+        }
       }
-    }
-    if (Serial2.available() > 0) {
-      incomingbyte2 = Serial2.read();
-      //Serial.print(char(incomingbyte2));
-      if (incomingbyte2 == 's'){
-        iswriting2 = true;
-        tlocdat2 = "";
-      }else if(incomingbyte1 == 'e'){
-        iswriting2 = false;
-        locdat2 = tlocdat1;        
-      }else if(iswriting2 == true){
-        //Serial.print(char(incomingbyte1));
-        tlocdat2 += (char)incomingbyte1;  
+      if (Serial2.available() > 0) {
+        incomingbyte2 = Serial2.read();
+        //Serial.print(char(incomingbyte2));
+        if (incomingbyte2 == 's'){
+          iswriting2 = true;
+          tlocdat2 = "";
+        }else if(incomingbyte1 == 'e'){
+          iswriting2 = false;
+          locdat2 = tlocdat1;        
+        }else if(iswriting2 == true){
+          //Serial.print(char(incomingbyte1));
+          tlocdat2 += (char)incomingbyte1;  
+        }
       }
+      //Serial.println(locdat);
+      sscanf(locdat1.c_str(), "%d, %d", &nir1, &pir1);
+      sscanf(locdat2.c_str(), "%d, %d", &nir2, &pir2);
+      
+      
+      if(pir1 > pir2){
+         if(nir1 = 1){    //not good looking code because posbile wrong wireing
+          degwhere = 0;      
+         }else if(nir1 = 2){
+          degwhere = 22.5;
+         }else if(nir1 = 3){
+          degwhere = 45;
+         }else if(nir1 = 4){
+          degwhere = 67.5;
+         }else if(nir1 = 5){
+          degwhere = 90;
+         }else if(nir1 = 6){
+          degwhere = 112.5;
+         }else if(nir1 = 7){
+          degwhere = 135;
+         }else if(nir1 = 8){
+          degwhere = 157.5;
+         }
+      }else{     
+         if(nir2 = 1){
+          degwhere = 180;
+         }else if(nir2 = 2){
+          degwhere = 202.5;
+         }else if(nir2 = 3){
+          degwhere = 225;
+         }else if(nir2 = 4){
+          degwhere = 247.5;
+         }else if(nir2 = 5){
+          degwhere = 270;
+         }else if(nir2 = 6){
+          degwhere = 292.5;
+         }else if(nir2 = 7){
+          degwhere = 315; 
+         }else if(nir2 = 8){
+          degwhere = 337.5; 
+         }
+      }
+      Serial.print(degwhere);
+      Serial.println(" degrees.");
+      drive(255, degwhere);
+      delay(1);
     }
-    //Serial.println(locdat);
-    sscanf(locdat1.c_str(), "%d, %d", &nir1, &pir1);
-    sscanf(locdat2.c_str(), "%d, %d", &nir2, &pir2);
-    
-    
-    if(pir1 > pir2){
-       if(nir1 = 1){    //not good looking code because posbile wrong wireing
-        degwhere = 0;      
-       }else if(nir1 = 2){
-        degwhere = 22.5;
-       }else if(nir1 = 3){
-        degwhere = 45;
-       }else if(nir1 = 4){
-        degwhere = 67.5;
-       }else if(nir1 = 5){
-        degwhere = 90;
-       }else if(nir1 = 6){
-        degwhere = 112.5;
-       }else if(nir1 = 7){
-        degwhere = 135;
-       }else if(nir1 = 8){
-        degwhere = 157.5;
-       }
-    }else{     
-       if(nir2 = 1){
-        degwhere = 180;
-       }else if(nir2 = 2){
-        degwhere = 202.5;
-       }else if(nir2 = 3){
-        degwhere = 225;
-       }else if(nir2 = 4){
-        degwhere = 247.5;
-       }else if(nir2 = 5){
-        degwhere = 270;
-       }else if(nir2 = 6){
-        degwhere = 292.5;
-       }else if(nir2 = 7){
-        degwhere = 315; 
-       }else if(nir2 = 8){
-        degwhere = 337.5; 
-       }
-    }
-    Serial.print(degwhere);
-    Serial.println(" degrees.");
-    drive(255, degwhere);
-    delay(1);
   }else{
     Serial.println("softPWR is off, waiting")
     delay(100);