From d3d1f8afe0dada9aac954ce1648609025d818c02 Mon Sep 17 00:00:00 2001 From: HaileyP Date: Fri, 8 Feb 2019 22:52:42 -0600 Subject: [PATCH 1/2] Added percentages to pixy cam line follower --- Arduino/.DS_Store | Bin 8196 -> 8196 bytes Arduino/pixyTest/pixyTest.ino | 48 +++++++++------------------------- 2 files changed, 12 insertions(+), 36 deletions(-) diff --git a/Arduino/.DS_Store b/Arduino/.DS_Store index 78039115fd02ce1cc20e0cc2e6d6ffe240ba9715..96902a9486532640c90fc75b12dc1ab0aa0bf966 100644 GIT binary patch delta 13 UcmZp1XmQxUEX2sTm_>*W02_z{7XSbN delta 13 UcmZp1XmQxUEX2sLm_>*W02_k?761SM diff --git a/Arduino/pixyTest/pixyTest.ino b/Arduino/pixyTest/pixyTest.ino index 92f0d84..c10cc1b 100644 --- a/Arduino/pixyTest/pixyTest.ino +++ b/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"); } From 82b0219cc0c589152f68e8007c4dc42dc14a4753 Mon Sep 17 00:00:00 2001 From: HaileyP Date: Sun, 10 Feb 2019 15:47:45 -0600 Subject: [PATCH 2/2] Combined ultrasonics with line following --- Arduino/.DS_Store | Bin 8196 -> 8196 bytes Arduino/UltrasonicPixy.ino | 146 ++++++++++++++++++ .../ultrasonicSetTest.ino | 0 3 files changed, 146 insertions(+) create mode 100644 Arduino/UltrasonicPixy.ino rename Arduino/{ => ultrasonicSetTest}/ultrasonicSetTest.ino (100%) diff --git a/Arduino/.DS_Store b/Arduino/.DS_Store index 96902a9486532640c90fc75b12dc1ab0aa0bf966..8c5ec9be60314293d81025a25162406a8b1d580c 100644 GIT binary patch delta 299 zcmZp1XmOa}FDlBwz`)4BAi%(o$&km8&)^KiIU5V7u}^H^-OSFx!l4dQAQSysIrK`ks${tmd=njSzb(>R~)Ez*?%x#V3^D!periC-~v>Y$xz0S3RYdrP|T1y zSz1h*wH2gxv!1{?R#_p2P@pj-Kn00F>3pC=G8vK?0)S)%LnY8)F(C$bpfhD)CIfW} iPW~&ZG#2 jej_l8buy3m?~M%tjGNgdzOhUW6fBzjQ?wT%$jb--uwWT| diff --git a/Arduino/UltrasonicPixy.ino b/Arduino/UltrasonicPixy.ino new file mode 100644 index 0000000..cf8544e --- /dev/null +++ b/Arduino/UltrasonicPixy.ino @@ -0,0 +1,146 @@ +#include +#include +#include +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); +} diff --git a/Arduino/ultrasonicSetTest.ino b/Arduino/ultrasonicSetTest/ultrasonicSetTest.ino similarity index 100% rename from Arduino/ultrasonicSetTest.ino rename to Arduino/ultrasonicSetTest/ultrasonicSetTest.ino