Buenas,
He probado a usar la libreria NewPing para la lectura del sonar, y al igual que pasaba antes, sin camara todo va genial, pero con ella la lectura del sonar es 0.
He depurado el codigo como me lo habeis recomendado. A ver si asi podeis ayudarme a saber que pasa.
#include <NewPing.h>
#include <AFMotor.h>
#include <Servo.h>
#include <Pixy2.h>
// SONARRAREN PINAK
#define trigPin 50 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define echoPin 48 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define led 46
// PI-ren ezarpena
#define PI 3.1415926535897932384626433832795
int encoderPos=0;
AF_DCMotor aurezk(4,MOTOR34_64KHZ); // Aurreko ezkerreko gurpila
AF_DCMotor auresk(3,MOTOR34_64KHZ); // Aurreko eskumako gurpila
AF_DCMotor atzesk(1,MOTOR12_64KHZ); // Atzeko ezkerreko gurpila
AF_DCMotor atzezk(2,MOTOR12_64KHZ); // Atzeko eskumako gurpila
Servo servoMotor;
NewPing sonar(trigPin,echoPin,500);
Pixy2 pixy; // Pixy objetuaren deklarazioa
void setup()
{
Serial.begin(9600); // Open serial monitor at 115200 baud to see ping results.
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(led, OUTPUT);
pinMode(52,OUTPUT);
digitalWrite(52,HIGH);
attachInterrupt(2, interrupcion, FALLING);
servoMotor.attach(10);
pixy.init(); // Kamera hasierarazi
}
void loop()
{
Serial.println("ITXAROTEN");
pixy.ccc.getBlocks(); //Leer datos de la cámara
if(pixy.ccc.numBlocks) // Chequear si detecta algún objeto
{
if(pixy.ccc.blocks[0].m_signature==1)
{
Abat(); // Ir a A1
}
else if(pixy.ccc.blocks[0].m_signature==2)
{
Abi(); // Ir a A2
}
else if(pixy.ccc.blocks[0].m_signature==3)
{
digitalWrite(30,HIGH);
}
}
}
void interrupcion() // Interrupcion del encoder para saber cuanta distancia anda
{
encoderPos++;
}
void JATORRItikAP() // Subrutina para ir a A'
{
int angelua; // angulo
int aldaketa; // cambio
double minimoa; // distancia minima
long distancia; // distancia medida
abiadurak(100); // Poner velocidades
distancia=0;
encoderPos=0;
angelua=90;
aldaketa=-5;
itxaron(1000); // Esperar 1000 milisegundos
do
{
aurrerantza(); // Ir hacia delante
distancia=SONAR(angelua); // Medir distancia mediante sonar
minimoa=kalminimoa(angelua); // Calcular distancia minima para colision
Serial.print("Angelua: ");
Serial.print(angelua);
Serial.print(" Distantzia minimoa: ");
Serial.print(minimoa);
Serial.print(" Distantzia erreala: ");
Serial.println(distancia);
if(distancia<minimoa) // Mirar si la distancia real es inferior a la real y si es asi parar los motores
{
gelditu(); // Parar
do
{
distancia=SONAR(angelua); // Medir distancia
Serial.print("ARRISKUA! ");
Serial.print("Angelua: ");
Serial.print(angelua);
Serial.print(" Distantzia minimoa: ");
Serial.print(minimoa);
Serial.print(" Distantzia erreala: ");
Serial.println(distancia);
}while(distancia<minimoa); // Medir distancia mientras la real sea inferior a la minima
aurrerantza(); // Avanzar
}
angelua=angelua+aldaketa; // Actualizar variable del angulo
if(angelua==155)
{
aldaketa=-5;
angelua=145;
}
if(angelua==25)
{
aldaketa=5;
angelua=35;
}
}while(encoderPos<=405); // Comparar lo andado con la distancia que queremos que recorra
gelditu();// parar
}
void APtikA1() // Los programas de movimiento son totalmente identicos excepto en la distancia que se quiere recorrer
{
int angelua;
int aldaketa;
long distancia;
double minimoa;
minimoa=0;
distancia=0;
angelua=90;
aldaketa=-5;
encoderPos=0;
abiadurak(100);
itxaron(1000);
do
{
//Mugitzen hasi
aurrerantza();
distancia=SONAR(angelua);
minimoa=kalminimoa(angelua);
if(distancia<minimoa)
{
gelditu();
do
{
distancia=SONAR(angelua);
}while(distancia<minimoa);
aurrerantza();
}
angelua=angelua+aldaketa;
}while(encoderPos<=405);
//Gelditu
gelditu();
}
void APtikA2() // A'-tik A2-era joateko azpiprograma
{
int angelua;
int aldaketa;
long distancia;
double minimoa;
minimoa=0;
distancia=0;
angelua=90;
aldaketa=-5;
encoderPos=0;
// Abiadurak ezarri
abiadurak(100);
itxaron(1000);
do
{
//Mugitzen hasi
aurrerantza();
distancia=SONAR(angelua);
minimoa=kalminimoa(angelua);
if(distancia<minimoa)
{
gelditu();
do
{
distancia=SONAR(angelua);
}while(distancia<minimoa);
aurrerantza();
}
angelua=angelua+aldaketa;
}while(encoderPos<=560);
//Gelditu
gelditu();
}
void Abat() // Rutina para ir a A1
{
JATORRItikAP();
BIRAKETA();
APtikA1();
itxaron(5000);
KONTRABIRAKETA();
APtikA1();
BIRAKETA2();
JATORRItikAP();
KONTRABIRAKETA();
}
void Abi() // Rutina para ir a A2
{
JATORRItikAP();
BIRAKETA();
APtikA2();
itxaron(5000); // Itxaroteko, gero kamara bidez objeturik ez dagoenenan gertatuko da.
KONTRABIRAKETA();
APtikA2();
BIRAKETA2();
JATORRItikAP();
KONTRABIRAKETA();
}
void KONTRABIRAKETA() // Realizar el giro 360º
{
//Abiadurak ezarri
abiadurak(120);
encoderPos=0;
itxaron(1000);
do{
//Mugitzen hasi
atzezk.run(FORWARD);
aurezk.run(FORWARD);
atzesk.run(BACKWARD);
auresk.run(BACKWARD);
Serial.print(encoderPos);
}while(encoderPos<=135);
gelditu();
}
void BIRAKETA2() // Rutina para girar 90º
{
abiadurak(120); // Establecer velocidades
encoderPos=0;
itxaron(1000);
do{
//Mugitzen hasi
atzezk.run(FORWARD);
aurezk.run(FORWARD);
atzesk.run(BACKWARD);
auresk.run(BACKWARD);
Serial.print(encoderPos);
}while(encoderPos<=60);
//Gelditu
gelditu();
}
void BIRAKETA() // Rutina para girar 90º
{
//Abiadurak ezarri
abiadurak(120);
encoderPos=0;
itxaron(1000);
do{
//Mugitzen hasi
atzezk.run(BACKWARD);
aurezk.run(BACKWARD);
atzesk.run(FORWARD);
auresk.run(FORWARD);
Serial.print(encoderPos);
}while(encoderPos<=60);
//Gelditu
gelditu();
}
long SONAR( int angelua) // Subrutina del sonar
{
long distancia=0 ;
servoMotor.write(angelua);
itxaron(50);
distancia= sonar.ping_cm();
Serial.println(distancia);
return distancia;
}
float kalminimoa(int angelua) // Subrutina para calcular la distancia minima a colision
{
float radianetan;
float minimo;
radianetan=0;
minimo=0;
if(angelua<90)
{
radianetan=angelua*(2*PI/360); // Pasar el angulo a radianes
minimo=(7.5/cos(radianetan))+1
; // Distantzia minima+ 1 seguridad
}
if(angelua>90)
{
radianetan=angelua*(2*PI/360);
minimo=(-7.5/cos(radianetan))+1;
}
if (angelua==90)
{
minimo=25; // Cos de 90º es 0 por eso se establece a mano
}
// Si la distancia minima da muy alta se limita a 25
if (minimo>25)
{
minimo=25;
}
return minimo; //Distantzia minimoa itzuli.
}
void abiadurak(int v) //Subrutina para establecer velocidades
{
atzezk.setSpeed(v);
atzesk.setSpeed(v);
aurezk.setSpeed(v);
auresk.setSpeed(v);
}
void gelditu() //Subrutina para parar
{
atzezk.run(RELEASE);
atzesk.run(RELEASE);
auresk.run(RELEASE);
aurezk.run(RELEASE);
}
void aurrerantza() //Subrutina para avanzar
{
atzezk.run(FORWARD); // Aurrerantza
atzesk.run(FORWARD);
auresk.run(FORWARD);
aurezk.run(FORWARD);
}
void itxaron(int t) // Subrutina para esperar, simular un delay()
{
unsigned long A=0;
unsigned long B=0;
B=millis();
do{
A=millis();
}while(A<t+B);
}