color

2 files changed, 175 insertions, 99 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);  
diff --git a/CODE/debug/color/color.ino b/CODE/debug/color/color.ino
index 9f8b472..3397f72 100644
--- a/CODE/debug/color/color.ino
+++ b/CODE/debug/color/color.ino
@@ -1,37 +1,58 @@
-/*
-  Blink
+int pr1 = 13;
+int pr2 = 41;
+int pr3 = 40;
+int pr4 = 39;
+int pr5 = 38;
+int pr6 = 37;
+int pr7 = 36;
+int pr8 = 35;
 
-  Turns an LED on for one second, then off for one second, repeatedly.
+int dpr1 = "LOW";
+int dpr2 = "LOW";
+int dpr3 = "LOW";
+int dpr4 = "LOW";
+int dpr5 = "LOW";
+int dpr6 = "LOW";
+int dpr7 = "LOW";
+int dpr8 = "LOW";
 
-  Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO
-  it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to
-  the correct LED pin independent of which board is used.
-  If you want to know what pin the on-board LED is connected to on your Arduino
-  model, check the Technical Specs of your board at:
-  https://www.arduino.cc/en/Main/Products
 
-  modified 8 May 2014
-  by Scott Fitzgerald
-  modified 2 Sep 2016
-  by Arturo Guadalupi
-  modified 8 Sep 2016
-  by Colby Newman
-
-  This example code is in the public domain.
-
-  https://www.arduino.cc/en/Tutorial/BuiltInExamples/Blink
-*/
-
-// the setup function runs once when you press reset or power the board
 void setup() {
-  // initialize digital pin LED_BUILTIN as an output.
-  pinMode(LED_BUILTIN, OUTPUT);
+  pinMode(pr1, INPUT);
+  pinMode(pr2, INPUT);
+  pinMode(pr3, INPUT);
+  pinMode(pr4, INPUT);
+  pinMode(pr5, INPUT);
+  pinMode(pr6, INPUT);
+  pinMode(pr7, INPUT);
+  pinMode(pr8, INPUT);
 }
 
-// the loop function runs over and over again forever
 void loop() {
-  digitalWrite(LED_BUILTIN, HIGH);  // turn the LED on (HIGH is the voltage level)
-  delay(1000);                      // wait for a second
-  digitalWrite(LED_BUILTIN, LOW);   // turn the LED off by making the voltage LOW
-  delay(1000);                      // wait for a second
+  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
+  }
 }