/* Name: FindAConePlusPING.ino Created: 8/29/2018 12:57:16 PM Author: DELLGARAGE\Ron Designed for the Parallax Board of Education Shield for Arduino UNO Updated 9/2/2018 Added PING sensor detection. If the cone is detected then approach slowly and wait, then back off as per original code. If ANY other object is detected then backoff Finished testing. Need to hold small cones off the ground for accurate detection. 1300 = CW Full Speed 1500 = Stop 1700 = CCW Full Speed */ #include // Include servo library #include //be sure to add this! Serial to Peripheral Interface Protocol #include // This is the main myPixy object Pixy2 myPixy; Servo LServo; // Declare left servo signal Pin 5 Servo RServo; // Declare right servo signal Pin 6 //---PWM settings int const CW = 1300; int const CCW = 1700; int const HALT = 1500; //---PING Stuff (9/2/2018) int const mySonarPin = 7; //pin 7 on Arduino. DO NOT USE ANY SERVO CONNECTORS WHILE PIXY IS INSTALLED long duration = 0; //time it takes for sonar to bounce off object. It is about 74us / inch long inchesFromObject = 0; //used to measure distance from object void setup() { LServo.attach(5); // Attach left signal to pin 5, not 13 Interferes with ICSP bus that the Pixy Uses! RServo.attach(6); // Attach right signal to pin 6, not 12 delay(2000); Stop(); //start clean myPixy.init(); //initialize camera Forward(); //start moving } void loop() { //---we will use the camera to detect a cone and the sonar to detect anything else. myPixy.ccc.getBlocks(); // If there are detected blocks if (myPixy.ccc.numBlocks) { for (int i = 0; i < myPixy.ccc.numBlocks; i++) { if (myPixy.ccc.blocks[i].m_height >= 110) //is it close to the cone? { Stop(); ApproachSlowly(); Stop(); BackWards(); LeftTurn(); Forward(); } } } else if (checkPing() <= 8) { Stop(); BackWards(); RightTurn(); Forward(); } else Forward(); } //*****************Helper Functions***************** void Forward() { //---forward LServo.writeMicroseconds(CCW); RServo.writeMicroseconds(CW); delay(2000); } void ApproachSlowly() { for (int speed = 0; speed <= 40; speed += 2) // Ramp up slowly for 1.5 seconds { LServo.writeMicroseconds(1500 + speed); // us = 1500,1502,...1598,1600 RServo.writeMicroseconds(1500 - speed); // us = 1500,1498,...1402,1400 delay(20); // 20 ms at each speed (pulse width timing) } delay(1500); } void BackWards() { //---back LServo.writeMicroseconds(CW); RServo.writeMicroseconds(CCW); delay(2000);} void LeftTurn() { //---left LServo.writeMicroseconds(CCW); RServo.writeMicroseconds(CCW); delay(1000); } void RightTurn() { //---right LServo.writeMicroseconds(CW); RServo.writeMicroseconds(CW); delay(1000); } void Stop() { //---stop LServo.writeMicroseconds(HALT); RServo.writeMicroseconds(HALT); delay(2000); } //---here we detect objects that are not cones! Function returns the distance from the object (long int) long checkPing() { pinMode(mySonarPin, OUTPUT); //set pin to output mode digitalWrite(mySonarPin, LOW); //always start with a low 2us signal before transmitting a pulse (cleaner/reliable) delayMicroseconds(2); digitalWrite(mySonarPin, HIGH); //PING it delayMicroseconds(5); //for 5 us digitalWrite(mySonarPin, LOW); //now off //---now read the return ping pinMode (mySonarPin, INPUT); //read on same digital pin duration = pulseIn(mySonarPin, HIGH); //returns the length of the pulse once the pin goes high. inchesFromObject = (duration / 74) / 2; // 74 microseconds/inch out and back... only want distance away so divide by 2. return inchesFromObject; }