Hello,
I want to use Pixycam and an ultrasonic sensor, but the doesn’t measure the distance correct. It just keeps saying 0. I’m not sure how to format the text correctly, but here is my code: (please ignore the comments, they are in my mother tongue)
#include <Pixy2.h> //De libraries worden geïmporteerd.
#include <PIDLoop.h>
#include <ZumoMotors.h>
#define ZUMO_FAST 400 //De snelheid voor de robot als die snel moet gaan.
#define ZUMO_SLOW 200 //De snelheid voor de robot als die langzaam moet gaan.
#define X_CENTER (pixy.frameWidth/2) //Het middden van het beeld.
Pixy2 pixy; //De Pixycam initiëren.
ZumoMotors motors; //De motoren initiëren.
const int trigPin = 12; //De pins waar de trigger pin en de echo pin op aangesloten zitten.
const int echoPin = 11;
long duration; //Een getal voor de duratie hoelang de echo pin weer het geluid terug ontvangt.
int distanceCm; //Een getal voor de afstand tot een object.
PIDLoop headingLoop(5000, 0, 0, false);
void setup()
{
Serial.begin(9600); //De seriële poort starten.
Serial.print("Starting...\n");
motors.setLeftSpeed(0); //De motoren moeten uit staan in het begin.
motors.setRightSpeed(0);
pinMode(5, OUTPUT); //De aansluitingen naar de lampjes (5 en 6)
pinMode(6, OUTPUT);
pinMode(trigPin, OUTPUT); //De aansluitingen voor de echo pin en de trigger pins van de ultrasone sensor.
pinMode(echoPin, INPUT);
pixy.init(); //De pixycam aanzetten.
pixy.setLamp(1, 1); //De lampjes aanzetten.
pixy.changeProg("line_tracking"); //Het programma van de Pixycam op line tracking zetten.
}
void loop()
{
int8_t res; //Getallen (integers) voor de Pixycam.
int32_t error;
int left, right;
char buf[96];
digitalWrite(trigPin, LOW); //De trigger pin van de ultrasone sensor uitzetten.
delay(2); //2 ms wachten.
digitalWrite(trigPin, HIGH); //De trigger pin aanzetten, zodat er een geluid voor 10 ms gemaakt wordt.
delay(10);
digitalWrite(trigPin, LOW); //Daarna weer uit.
duration = pulseIn(echoPin, HIGH); //Meten hoelang het duurt voordat het verzonden geluidsignaal weer teruggekaatst is.
distanceCm = duration*0.034/2; //De afstand is de tijd keer de snelheid (343m/s). Gedeeld door 2 omdat het geluid heen en weer gaat.
Serial.println(distanceCm);
res = pixy.line.getMainFeatures(); //De data van de Pixycam verzamelen.
if (distanceCm <=20) { //Als de afstand van de robot tot het object kleiner is dan 20cm.
motors.setLeftSpeed(0); //Motoren uit
motors.setRightSpeed(0);
}
else {
left += ZUMO_FAST; //Anders gewoon doorrijden.
right += ZUMO_FAST;
}
if (res<=0) //Als de Pixycam niks detecteert
{
motors.setLeftSpeed(0); //Motoren uit
motors.setRightSpeed(0);
digitalWrite(5, HIGH); //Zet de remlichten aan als de robot stilstaat.
digitalWrite(6, HIGH);
Serial.print("stop ");
Serial.println(res);
return;
}
else {
digitalWrite(5, LOW); //Anders remlichten uit.
digitalWrite(6, LOW);
}
if (res&LINE_VECTOR) //Als de vector gevonden is, ten opzichte van het midden, rijd daar dan heen.
{
error = (int32_t)pixy.line.vectors->m_x1 - (int32_t)X_CENTER; //Bereken waar de lijn is T.O.V. het midden
pixy.line.vectors->print();
headingLoop.update(error); //rijd naar het punt waar de error gevonden in.
left = headingLoop.m_command;
right = -headingLoop.m_command; //De motoren die kant op laten rijden waar de vector is.
if (pixy.line.vectors->m_y0 > pixy.line.vectors->m_y1) //Als de vector gewoon voor de robot staat, is er niks aan de hand.
{
if (pixy.line.vectors->m_flags&LINE_FLAG_INTERSECTION_PRESENT) //Als er een kruising aankomt, langzamer rijden.
{
left += ZUMO_SLOW;
right += ZUMO_SLOW;
}
else
{
left += ZUMO_FAST; //Als er geen kruising is, gewoon snel.
right += ZUMO_FAST;
}
}
else
{
left -= ZUMO_SLOW;
right -= ZUMO_SLOW;
}
motors.setLeftSpeed(left);
motors.setRightSpeed(right);
}
if (res&LINE_INTERSECTION) //Als er een kruising is, doe dan niks, maar ga rechtdoor.
{
pixy.line.intersections->print();
}
}
If I use the code below, it does work. The weird thing is, is that it just won’t work with the main code.
const int trigPin = 12;
const int echoPin = 11;
long duration;
int distanceCm;
void setup() {
Serial.begin(9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
void loop() {
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distanceCm= duration*0.034/2;
Serial.println(distanceCm);
}
Thanks in advance!
-Milo
