GreyE30:
I've had some help with the coding of an ongoing project (see previous my previous posts if you want background) which is now working, since I have tried to develop it to include an ultrasonic (SRF05) but cannot even get it to serial.print distances.
I am trying to use the new ping library and think it could be conflicting with some of the other libraries / code.
There are some other posts on here which discuss using event-driven interrupts within this library but I could not get a clear answer from it.I have tried accessing the ultrasonic data at different baud rates, to no avail. However, with the same setup (connections) and a simple script the ultrasonic works fine.
Simply put, I am trying to have the ultrasonic cause my robot to stop and reverse when it finds an object in front of it before continuing with the program (object removed).
#include <Wire.h>
#include <NewPing.h>
#include <Adafruit_MotorShield.h>
#include "utility/Adafruit_PWMServoDriver.h"
#include <SPI.h>
#include <Pixy.h>
#define TRIGGER_PIN 12 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define ECHO_PIN 11 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define MAX_DISTANCE 200 // Maximum distance we want to ping for (in centimeters). Maximum sensor distance is rated at 400-500cm.
NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
#define swap(a,b) {
int c = (a);
(a) = (b);
(b) = c;
}
Pixy pixy;
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
Adafruit_DCMotor *MotorL = AFMS.getMotor(1);
Adafruit_DCMotor *MotorR = AFMS.getMotor(2);
void setup()
{
Serial.begin(9600); // Open serial monitor at 115200 baud to see ping results.
AFMS.begin(); // create with the default frequency 1.6KHz
MotorL->setSpeed(255); // Set the speed to start, from 0 (off) to 255 (max speed)
MotorL->run(FORWARD);// turn on motor
MotorL->run(BACKWARD);
MotorL->run(RELEASE);
MotorR->setSpeed(255); // Set the speed to start, from 0 (off) to 255 (max speed)
MotorR->run(FORWARD);// turn on motor
MotorR->run(BACKWARD);
MotorR->run(RELEASE);
}
void loop()
{
uint16_t blockCount;
blockCount = pixy.getBlocks();
Serial.print(" Blocks : ");
Serial.print(blockCount);
Serial.print(" ");
const double minRequiredRatio = 0.8;
const double maxRequiredRatio = 1.2;
int chosenBlock = -1;
int chosenSignature = 1;
for (int idx = 0; idx < blockCount; ++idx)
{
Block& pixyBlock = pixy.blocks[idx];
if (pixyBlock.signature != chosenSignature)
{
continue;
}
int objectSize = pixyBlock.width * pixyBlock.height;
double objectRatio = (double)pixyBlock.width / (double)pixyBlock.height;
Serial.print("Size : ");
Serial.print(objectSize);
Serial.print(", ");
Serial.print("Ratio : ");
Serial.print(objectRatio);
Serial.print(" ");
const double squareTolerance = 0.5; // Adjust to suit
double squareness = fabs(1.0 - objectRatio);
const double minRatio = 0.8; // Adjust to suit
const double maxRatio = 2.0; // Adjust to suit
if (objectRatio > minRatio && objectRatio < maxRatio)
{
chosenBlock = idx;
break;
}
}
if (chosenBlock < 0)
{
Serial.println("Could not find desired object");
MotorL->run(RELEASE);
MotorR->run(RELEASE);
return;
}
Block& singlePixyBlock = pixy.blocks[chosenBlock];
int objectX = singlePixyBlock.x;
int objectY = singlePixyBlock.y;
Serial.print(", X Value : ");
Serial.print(objectX);
Serial.print(", Y Value : ");
Serial.println(objectY);
const int cameraWidth = 320;
const int cameraHeight = 200;
const int centreX = cameraWidth / 2;
const int centreY = cameraHeight / 2;
const int idealDistanceSize = 5000; // Adjust to suit
const int idealDistanceTolerance = idealDistanceSize / 10; // Adjust to suit
int objectSize = singlePixyBlock.width * singlePixyBlock.height;
const int maxSpeed = 255;
const int slowestSpeed = maxSpeed - 128; // Adjust to suit
const int distLimit = 10; //Limit for obstacles (in cm)
int dist = sonar.ping_cm();
int centreOffset = objectX - centreX;
int speedOffset = ((double)centreOffset / (double)centreX) * (double)(maxSpeed - slowestSpeed);
int speedLeft = slowestSpeed - speedOffset;
int speedRight = slowestSpeed + speedOffset;
int motorDirection = RELEASE;
Serial.print("objectSize: ");
Serial.print(objectSize);
if (dist<distLimit)
{
motorDirection = BACKWARD;
Serial.print("Ping: ");
Serial.print(dist); // Convert ping time to distance in cm and print result (0 = outside set distance range)
Serial.println("cm ");
}
if (objectSize < (idealDistanceSize - idealDistanceTolerance))
{
motorDirection = FORWARD;
Serial.print(" FORWARD ");
}
else if (objectSize > (idealDistanceSize + idealDistanceTolerance))
{
swap(speedLeft, speedRight);
motorDirection = BACKWARD;
Serial.print(" BACKWARD ");
}
else
{
Serial.print(" RELEASE ");
}
MotorL->setSpeed(speedLeft);
Serial.print(" MotorL: ");
Serial.print(speedLeft);
MotorR->setSpeed(speedRight);
Serial.print(" MotorR: ");
Serial.print(speedRight);
MotorL->run(motorDirection);
MotorR->run(motorDirection);
}
I would agree with everyone else here. There's a LOT you could do better with that code. I sent you an email reply that fixes most of your sketch. But, I'd like to post here that it sounds like there's a conflict with a timer. One of the pins you're using with the ultrasonic sensor could be used as a timer for the motor or camera sensor. So, try using two pins that don't have a timer attached, like pins 7 & 8.
That's where I would start, clean up your code so it's more understandable and logical, then try using different pins. Next, adopt an event-driven paradigm instead of the block mode programming style you're currently using. Doesn't work well for robots.
Tim