Added percentages to pixy cam line follower

Arduino/UltrasonicPixy.ino

@@ -0,0 +1,146 @@
+#include <math.h>
+#include <SPI.h>  
+#include <Pixy2.h>
+Pixy2 pixy2;
+
+int trigPinLeft = 11;    // Trigger
+int echoPinLeft = 12;    // Echo
+int trigPinRight = 9;    // Trigger
+int echoPinRight = 10;    // Echo
+
+float previousLeftDistance = 0;
+float previousRightDistance = 0;
+float left;
+float right;
+
+const float maxAllowableDelta = 10; // Any greater distance than this between reads is thrown out.
+
+bool trustworthy = false;
+
+
+//begin setups
+void setupElegooUltrasonicSensor(){
+  pinMode(trigPinLeft, OUTPUT);
+  pinMode(trigPinRight, OUTPUT);
+  pinMode(echoPinLeft, INPUT);
+  pinMode(echoPinRight, INPUT);
+}
+
+void setupPixyCam(){
+  pixy2.init();
+}
+//end setups
+
+float ultrasonicLeft(){
+  float inches, duration;
+  digitalWrite(trigPinLeft, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPinLeft, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPinLeft, LOW);
+  pinMode(echoPinLeft, INPUT);
+  duration = pulseIn(echoPinLeft, HIGH);
+  inches= (duration/2.0) / 74.0;   // Divide by 74 or multiply by 0.0135
+  return inches;
+}
+
+float ultrasonicRight(){
+  float inches, duration;
+  digitalWrite(trigPinRight, LOW);
+  delayMicroseconds(5);
+  digitalWrite(trigPinRight, HIGH);
+  delayMicroseconds(10);
+  digitalWrite(trigPinRight, LOW);
+  pinMode(echoPinRight, INPUT);
+  duration = pulseIn(echoPinRight, HIGH);
+  inches = (duration/2.0) / 74.0;   // Divide by 74 or multiply by 0.0135
+  return inches;
+}
+
+int getDistance() {
+  int center;
+  center = (left+right)/2.0;
+  return center;
+}
+
+// being rotation
+double convertToDegrees(double r) {
+  return r*180.0/3.14159;
+}
+
+double getRotationInDegrees() {
+  double disBetweenSensors = 10; //Believe it or not this is the distance between the sensors on the robot
+  double angleInRadians = atan(abs(left-right)/(disBetweenSensors)); //returns angle to be parallel
+  if (left < right) {
+    return convertToDegrees(angleInRadians);
+  } else if (right < left) {
+    return -(convertToDegrees(angleInRadians));
+  } else if (right == left) {
+    return 0;
+  }
+}
+//end rotation
+
+bool isUltrasonicGood() {
+  float leftDelta = abs(previousLeftDistance - left);
+  float rightDelta = abs(previousRightDistance - right);
+
+  if (leftDelta > maxAllowableDelta) {
+    trustworthy = false;
+  } else if (rightDelta > maxAllowableDelta) {
+    trustworthy = false;
+  } else {
+    trustworthy = true;
+  }
+
+  previousLeftDistance = left;
+  previousRightDistance = right;
+
+  return trustworthy;
+}
+
+void updateUltrasonic() {
+  left = ultrasonicLeft();
+  right = ultrasonicRight();
+}
+
+//begin pixy cam code
+float getLineRefrence() {
+  float lineLocation, percentage;
+  // put your main code here, to run repeatedly:
+  pixy2.ccc.getBlocks();
+
+  lineLocation = pixy2.ccc.blocks[0].m_x;
+  percentage = (lineLocation - 157.5)/157.5;
+  return percentage;
+}
+//end pixy cam code
+
+//setup
+void setup() {
+    Serial.begin (9600);
+  setupElegooUltrasonicSensor();
+  setupPixyCam();
+}
+
+//print
+void loop() {
+  updateUltrasonic();
+  if (isUltrasonicGood()) {
+    Serial.print(left);
+    Serial.print(",");
+    Serial.print(right);
+    Serial.print (" ");
+    Serial.print(getDistance());
+    Serial.print("in. @");
+    Serial.print(getRotationInDegrees());
+    Serial.print("°");
+    Serial.print("\n");
+    Serial.print(getLineRefrence());
+    Serial.print("%");
+  } else {
+    Serial.print("bad data");
+  }
+  Serial.print("\n");
+  delay(1000);
+}

Arduino/pixyTest/pixyTest.ino

@@ -8,44 +8,20 @@ void setup() {
   pixy2.init();
 }
 
-//Prints YOUR location in refrence to the Line
+//Prints your location in percentages left or right of the line
 
-void loop() {
+float getLineRefrence() {
+  float lineLocation, percentage;
   // put your main code here, to run repeatedly:
   pixy2.ccc.getBlocks();
 
-
-  if (pixy2.ccc.blocks[0].m_x > 0 && pixy2.ccc.blocks[0].m_x < 50) {
-    digitalWrite(6, HIGH); //turn left a lot
-    Serial.print("1 1/4 in to the left");
-    Serial.print('\n');
-    delay(500);
-  } 
-  if(pixy2.ccc.blocks[0].m_x > 50 && pixy2.ccc.blocks[0].m_x < 125) {
-    digitalWrite(7, HIGH); //turn left a little
-    Serial.print("1/2 in to the left");
-    Serial.print('\n');
-    delay(500);
-  }
-  if (pixy2.ccc.blocks[0].m_x > 125 && pixy2.ccc.blocks[0].m_x < 175) {
-    digitalWrite(6, HIGH); 
-    digitalWrite (7, HIGH);
-    digitalWrite(8, HIGH); 
-    digitalWrite (9, HIGH); //go straight
-    Serial.print("straight");
-    Serial.print('\n');
-    delay(500);
-  }
-  if (pixy2.ccc.blocks[0].m_x > 175 && pixy2.ccc.blocks[0].m_x < 250) {
-    digitalWrite(8, HIGH); //turn right a little
-    Serial.print("1/2 in to the right");
-    Serial.print('\n');
-    delay(500);
-  }
-  if (pixy2.ccc.blocks[0].m_x > 250 && pixy2.ccc.blocks[0].m_x < 300) {
-    digitalWrite(9, HIGH); //turn right a lot
-    Serial.print("1 1/4 in to the right");
-    Serial.print('\n');
-    delay(500);
-  }
+  lineLocation = pixy2.ccc.blocks[0].m_x;
+  percentage = (lineLocation - 157.5)/157.5;
+  return percentage;
+}
+
+void loop() {
+  Serial.print(getLineRefrence());
+  Serial.print("%");
+  Serial.print("\n");
 }