im using a pretty simple dual ultrasonic setup. the works perfectly. stops, reverses, turns depending on the sensor that tripped. however i also need to the motors to be taken over by the pixycam to turn right/ left depending on where the object is in its field of view. is there anything im missing here? the camera is never able to take over the motors.
#include <SoftwareSerial.h>
#include <Pixy2.h>
#include <NewPing.h> //Ultrasonic sensor function library. You must install this library
Pixy2 pixy;
SoftwareSerial mySerial(A3, A2);//RX, TX brown, black
//control pins
#define ExtendDump A0//
#define RetractDump A1//
//#define TXofBT A3
//#define RXofBT A2
//#define ExtendDumpCommand A2//
//#define RetractDumpCommand A3//
#define Pickup 3//green
#define LeftMotorForward 4//blue
#define LeftMotorBackward 5//green
#define RightMotorForward 6//blue
#define RightMotorBackward 7//green
//#define TurnLeftCommand 10//
//#define TurnRightCommand 11//
//#define MoveForwardCommand 12//
//#define MoveBackwardCommand 13//
//Define Constants
#define trigger_Left_Sensor 8//red
#define echo_Left_Sensor 8//
#define trigger_Right_Sensor 9//orange
#define echo_Right_Sensor 9//
#define MinDistance 50
#define max_cm_distance 250
NewPing sonar1(trigger_Left_Sensor, echo_Left_Sensor, max_cm_distance);
NewPing sonar2(trigger_Right_Sensor, echo_Right_Sensor, max_cm_distance);
boolean goesForward = false;
int signatue, x, y, width, height;
float cx, cy, area;
// Define Variables
int distance1;
int distance2;
int i;
void setup(){
pixy.init();
pinMode(RightMotorForward, OUTPUT);
pinMode(LeftMotorForward, OUTPUT);
pinMode(LeftMotorBackward, OUTPUT);
pinMode(RightMotorBackward, OUTPUT);
pinMode(Pickup, OUTPUT);
//pinMode(TurnLeftCommand, INPUT);
//pinMode(TurnRightCommand, INPUT);
//pinMode(MoveForwardCommand, INPUT);
//pinMode(MoveBackwardCommand, INPUT);
//pinMode(ExtendDumpCommand, INPUT);
//pinMode(RetractDumpCommand, INPUT);
pinMode(ExtendDump, OUTPUT);
pinMode(RetractDump, OUTPUT);
Serial.begin(115200);
}
void loop(){
delay(50); // Wait 50ms between pings (about 20 pings/sec). 29ms should be the shortest delay between pings.
Serial.print("Ping: ");
Serial.print(sonar1.ping_cm()); // Send ping, get distance in cm and print result (0 = outside set distance range)
Serial.print(sonar2.ping_cm());
Serial.println("cm");
delay(500);
pixy.ccc.getBlocks();
if(pixy.ccc.numBlocks)
{
Serial.print("Detected ");
Serial.println(pixy.ccc.numBlocks);
for (i=0; i<pixy.ccc.numBlocks; i++)
{
Serial.print(" block ");
Serial.print(i);
Serial.print(": ");
pixy.ccc.blocks[i].print();
Serial.println(pixy.ccc.blocks[i].m_x);
Serial.println(pixy.ccc.blocks[i].m_y);
}
}
if(pixy.ccc.numBlocks){//if object is detected
while((pixy.ccc.blocks[0].m_x >10) && (pixy.ccc.blocks[0].m_x <305));//object in field of view
moveStop();
delay(500);
if((pixy.ccc.blocks[0].m_x <145) && (pixy.ccc.blocks[0].m_x >155)){//if object is not centered on x axis
if(pixy.ccc.blocks[0].m_x <145){//if detected object is left of center x
turnLeft();
delay(500);
}
else if(pixy.ccc.blocks[0].m_x >155){//if detected object is right of center x
turnRight();
delay(500);
}
}
if((pixy.ccc.blocks[0].m_x >=145) && (pixy.ccc.blocks[0].m_x <=155)){//if object is centered on x axis
if((pixy.ccc.blocks[0].m_y <125) || (pixy.ccc.blocks[0].m_y >135)){//if object is not centered on y axis
if(pixy.ccc.blocks[0].m_y <125){//if object is further away from center y
moveForward();
delay(500);
}
else if(pixy.ccc.blocks[0].m_y >135){//if object is closer than center y
moveBackward();
delay(500);
}
}
if((pixy.ccc.blocks[0].m_x >=145) && (pixy.ccc.blocks[0].m_x <=155) && (pixy.ccc.blocks[0].m_y >=125) && (pixy.ccc.blocks[0].m_y <=135)){//if object is centered
moveStop();
digitalWrite(3, HIGH);//pick up object
delay(30000);
}
else{
digitalWrite(3, LOW);
}
}
}
distance1 = sonar1.ping_cm(max_cm_distance); //measure duration of sesnor 1 ping
delay(250);
distance2 = sonar2.ping_cm(max_cm_distance); //measure duration of sensor 2 ping
delay(250);
moveForward();
if(distance1 <= 35 || distance2 <= 35){
moveStop();
Serial.println('STOP');
delay(2000);
moveBackward();
Serial.println('BACKWARDS');
delay(2000);
moveStop();
Serial.println('STOP');
delay(1000);
if (distance1 > distance2){
turnRight();
Serial.println('RIGHT');
delay(3000);
moveForward();
}
else if(distance1 < distance2){
turnLeft();
Serial.println('LEFT');
delay(3000);
moveForward();
}
else if(distance1 > 35 && distance2 > 35){
moveForward();
Serial.println('FORWARD');
}
}
}
void moveStop(){
analogWrite(RightMotorForward, 0);
analogWrite(LeftMotorForward, 0);
analogWrite(RightMotorBackward, 0);
analogWrite(LeftMotorBackward, 0);
}
void moveForward(){
if(!goesForward){
goesForward=true;
analogWrite(LeftMotorForward, 125);
analogWrite(RightMotorForward, 125);
analogWrite(LeftMotorBackward, 0);
analogWrite(RightMotorBackward, 0);
}
}
void moveBackward(){
goesForward=false;
analogWrite(LeftMotorBackward, 125);
analogWrite(RightMotorBackward, 125);
analogWrite(LeftMotorForward, 0);
analogWrite(RightMotorForward, 0);
}
void turnRight(){
analogWrite(LeftMotorForward, 125);
analogWrite(RightMotorBackward, 125);
analogWrite(LeftMotorBackward, 0);
analogWrite(RightMotorForward, 0);
}
void turnLeft(){
analogWrite(LeftMotorBackward, 125);
analogWrite(RightMotorForward, 125);
analogWrite(LeftMotorForward, 0);
analogWrite(RightMotorBackward, 0);
}
Thank you very much in advance. still learning.