Menu
▾
▴
[RoboComp-svn] SF.net SVN: robocomp:[840] Components/experimental/paletComp/src
|
From: <rc...@us...> - 2009-08-31 19:12:52
|
Revision: 840
http://robocomp.svn.sourceforge.net/robocomp/?rev=840&view=rev
Author: rcintas
Date: 2009-08-31 19:12:43 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
Modified Paths:
--------------
Components/experimental/paletComp/src/pale.cpp
Components/experimental/paletComp/src/pale.h
Components/experimental/paletComp/src/tablaColor.h
Components/experimental/paletComp/src/worker.cpp
Components/experimental/paletComp/src/worker.h
Modified: Components/experimental/paletComp/src/pale.cpp
===================================================================
--- Components/experimental/paletComp/src/pale.cpp 2009-08-31 16:49:38 UTC (rev 839)
+++ Components/experimental/paletComp/src/pale.cpp 2009-08-31 19:12:43 UTC (rev 840)
@@ -1,8 +1,11 @@
#include "pale.h"
-Pale::Pale(){
+Pale::Pale(color _centro,color _externo,int _distancia){
+ centro=_centro;
+ externo=_externo;
+ posicion=_distancia;
pos.init();
- first=false;
+ first=true;
};
Pale::~Pale(){};
@@ -25,7 +28,7 @@
QMat x(3);
x(0)=0;
x(1)=0;
- x(2)=+75;
+ x(2)=-posicion; //palet -125 //pared -300
return pos.robotToWorld( x );
}
void Pale::updatePropioception(T _x, T _z, T _alfa){
@@ -45,6 +48,10 @@
}
pos.setRT(x,z,alfa);
}
+void Pale::getCodificacion(color &_externo,color &_centro){
+ _externo=externo;
+ _centro=centro;
+};
// void Pale::drawPale(qWorld *world){
// QMat p(3);
// p=pp.getPuntoAnclaje();
Modified: Components/experimental/paletComp/src/pale.h
===================================================================
--- Components/experimental/paletComp/src/pale.h 2009-08-31 16:49:38 UTC (rev 839)
+++ Components/experimental/paletComp/src/pale.h 2009-08-31 19:12:43 UTC (rev 840)
@@ -2,6 +2,7 @@
#define PALE_H
#include <qmatriz/qmovingrobot.h>
+#include "tablaColor.h"
#define WIDTH_PALE 300
#define HEIGHT_PALE 150
@@ -17,16 +18,18 @@
private:
bool first;
MovingRobot pos;
+ color centro,externo;
+ int posicion;
-
public:
- Pale();
+ Pale(color _centro,color _externo,int _distancia);
~Pale();
void setRT ( T x, T z, T alfa );
void updatePropioception(T x, T z, T alfa);
RMat::QMat getPoint(float distancia);
RMat::QMat getPosicion();
float getAlfa();
+ void getCodificacion(color &_externo,color &_centro);
// void drawPalet(qworld *q);
};
Modified: Components/experimental/paletComp/src/tablaColor.h
===================================================================
--- Components/experimental/paletComp/src/tablaColor.h 2009-08-31 16:49:38 UTC (rev 839)
+++ Components/experimental/paletComp/src/tablaColor.h 2009-08-31 19:12:43 UTC (rev 840)
@@ -1,5 +1,5 @@
-#ifndef BALIZA_H
-#define BALIZAS_H
+#ifndef TABLACOLOR_H
+#define TABLACOLOR_H
#include <QtGui>
#include <math.h>
Modified: Components/experimental/paletComp/src/worker.cpp
===================================================================
--- Components/experimental/paletComp/src/worker.cpp 2009-08-31 16:49:38 UTC (rev 839)
+++ Components/experimental/paletComp/src/worker.cpp 2009-08-31 19:12:43 UTC (rev 840)
@@ -94,23 +94,29 @@
connect(&tAproximacion,SIGNAL(timeout()),this,SLOT(aproximacion()));
connect(&tPlan,SIGNAL(timeout()),this,SLOT(ejecutarPlan()));
- connect(&tIrPale,SIGNAL(timeout()),this,SLOT(irPale()));
+ connect(&tIrObjetivo,SIGNAL(timeout()),this,SLOT(irObjetivo()));
+ connect(&tBuscarObjetivo,SIGNAL(timeout()),this,SLOT(buscarObjetivo()));
-
posible_pale=false;
pale_detectado=false;
Ejecucion=false;
Plan.clear();
- Plan.push_front(bajarPinza);
+// Plan.push_front(bajarPinza);
Plan.push_front(irDestinoPale);
// Plan.push_front(aproximarsePale);
- Plan.push_front(subirPinza);
- Plan.push_front(irOrigenPale);
+// Plan.push_front(subirPinza);
+// Plan.push_front(irOrigenPale);
-
qDebug()<<Plan;
+ Objetivos.clear();
+ P=new Pale(green,red,125);
+ Destino=new Pale(red, green,350);
+ Objetivos.push_front(Destino);
+ Objetivos.push_front(P);
+
+
}
Worker::~Worker()
{
@@ -162,28 +168,27 @@
zonaI=QRect(0,0,camParams.width,camParams.height);
tAproximacion.stop();
tPlan.stop();
- tIrPale.stop();
+ tIrObjetivo.stop();
}
-void Worker::irPale(){
+void Worker::irObjetivo(){
bool recalcular=false;
RMat::QMat p3(3);
- identificarPale(red,red);
+ identificarObjetivo(Objetivo);
if(!bezier.Activo){
- qDebug()<<"Ir pale first time";
- //first time arrancar timer de aproximacion
+ qDebug()<<"Ir objetivo primera vez";
bezier.orig.setX(bState.x);
bezier.orig.setY(bState.z);
- RMat::QMat p3=P.getPoint(200);
+ RMat::QMat p3=Objetivo->getPosicion();
recalcular=true;
bezier.Redirigir=false;
bezier.t=0.1;
tAproximacion.start(T_APROXIMACION);
}
else{
- p3=P.getPoint(200);
+ p3=Objetivo->getPosicion();
float d=distancia(p3(0),p3(2),bezier.dest.x(),bezier.dest.y());
// qDebug()<<"Bezier:"<<bezier.dest.x()<<bezier.dest.y()<<"Posicion palet"<<p3(0)<<p3(2)<<"distancia:"<<d;
@@ -193,14 +198,12 @@
}
if(recalcular){ //es la primera vez o ha cambiado la posicion del palet
+ qDebug()<<"Recalculando Bezier";
//calculamos distancia al objetivo para establecer los puntos intermdios
float d=distancia(p3(0),p3(2),bezier.orig.x(),bezier.orig.y());
-/**///necesario comprobar que el palet este lejos sino puede fallar
- qDebug()<<"Recalculando Bezier";
-
- RMat::QMat p1=P.getPoint(0.35*d);
- RMat::QMat p2=P.getPoint(0.8*d);
+ RMat::QMat p1=Objetivo->getPoint(0.35*d);
+ RMat::QMat p2=Objetivo->getPoint(0.8*d);
generarBezier(QPoint(p1(0),p1(2)),QPoint(p2(0),p2(2)),QPoint(p3(0),p3(2)));
bezier.Redirigir=true;
}
@@ -209,7 +212,6 @@
void Worker::aproximacion(){
//comprobar la distancia que hemos recorrido hasta el siguiente punto Bezier
float dist=distancia(bezier.nextp.x(),bezier.nextp.y(),bState.x,bState.z);
-// qDebug()<<"Posicion actual"<<bState.x<<bState.z<<"next point: "<<bezier.nextp.x()<<bezier.nextp.y()<<"distancia siguiente punto"<<dist<<"p final"<<bezier.dest.x()<<bezier.dest.y();
if(dist < bezier.dMedia || bezier.Redirigir){ //primera vez o estamos cerca del punto (-5 cm)
bezier.Activo=true;
//generamos el siguiente punto de la zanahoria
@@ -295,24 +297,22 @@
-void Worker::identificarPale(color externo,color centro){
+void Worker::identificarObjetivo(Pale *Objetivo){
QList<baliza *>lexternos,lcentro;
QPointF p1,p2,p3;
RMat::QMat p1m(3), p2m(3), p3m(3),p1i(2),p2i(2),p3i(2),p3id(2);
float dis,alpha=0.,dp1p3,dp2p3;
-
-// if(palet_detectado)
-// B.generateBaliza(imgD,zonaI);
-// else
- B.generateBaliza(imgD);
-// B.acotar(5);
+ color externo,centro;
+ B.generateBaliza(imgD);
+ B.acotar(5);
+
+ Objetivo->getCodificacion(externo,centro);//obtiene la codificacion a buscar
lexternos=B.getList(externo);
lcentro=B.getList(centro);
-
- //muestra la lista de zonas rojas y pinta las zonas en la pantalla
+ //muestra la lista de balizas obtenidas y las dibuja en la pantalla en la pantalla
mostrarBalizas(lcentro+lexternos);
-
+
//generar rectas para los cuadros de la lista
for(int i=0;i<lexternos.length();i++){
p1i(0)=lexternos.at(i)->centro.x();
@@ -323,12 +323,8 @@
//transforma p1 y p2 a su posición en el mundo
p1m = innerModel->leftCamFromFloorToWorld(p1i);
p2m = innerModel->leftCamFromFloorToWorld(p2i);
-
//primero, la distancia entre los puntos ha de ser mas o menos la longitud del palet
dis=distancia(p1m(0),p1m(2),p2m(0),p2m(2));
-
-//qDebug()<<"puntos imagen"<<p1i(0)<<p1i(1)<<p2i(0)<<p2i(1);
-//qDebug()<<"puntos del mundo"<<p1m(0)<<p1m(2)<<p2m(0)<<p2m(2) << "distancia"<<dis;
if(dis > WIDTH_PALE - ERROR && dis < WIDTH_PALE + ERROR){
//obtenemos el angulo que forma el palet con la horizontal mediante la pendiente de la recta entre los extremos del palet
if (p1m(0)!=p2m(0))
@@ -345,20 +341,14 @@
dis=distancia(p3id(0),p3id(2),p3m(0),p3m(2));
dp1p3=distancia(p1m(0),p1m(2),p3m(0),p3m(2));
dp2p3=distancia(p2m(0),p2m(2),p3m(0),p3m(2));
-
-// world->drawEllipse(QPoint(p3id(0),p3id(2)),8,Qt::black);
-// world->drawEllipse(QPoint(p3m(0),p3m(2)),8,Qt::red);
-// qDebug()<<"calculos:" <<dis<<" hh"<<dp1p3<<dp2p3;
if(dis < ERROR/2 && fabs(dp1p3-dp2p3) < ERROR/2){
qvisorI->drawLine(QLine(QPoint(p1i(0),p1i(1)),QPoint(p2i(0),p2i(1))),Qt::white,2);
-// qDebug()<<"PALET DETECTADO"<<p1i(0)<<p1i(1)<<p2i(0)<<p2i(1)<<alpha;
-// qDebug()<<"puntos mundo"<<p1m(0)<<p1m(2)<<p3m(0)<<p3m(2)<<p2m(0)<<p2m(2);
- actualizarPos(p3m(0),p3m(2),-alpha);
+ Objetivo->updatePropioception(p3m(0),p3m(2),-alpha);
pale_detectado=true;
-
+
actualizarZonaBusqueda(p1i(0),p1i(1),p2i(0),p2i(1));
actualizarPosHead((int)p3i(0), (int)p3i(1) );
}//if
@@ -368,53 +358,47 @@
}//for(i)
qvisorI->update();
}
-void Worker::actualizarPos(int x,int y,float alfa){
- if(!pale_detectado)
- P.setRT(x,y,alfa);
- else
- P.updatePropioception(x,y,alfa);
-}
+
//en fucion de la poiscion del palet en la imagen orienta la cabeza
-void Worker::actualizarPosHead(int ximagen,int yimagen){
+//centramos el palet en la imagen para mejorar la precision
+int Worker::actualizarPosHead(int ximagen,int yimagen){
float tilt,pan,posx,posy;
tilt=hState.tilt.pos;
pan=hState.left.pos;
-//centramos el palet en la imagen para mejorar la precision
posy=120-yimagen;
tilt+=0.0035*posy;
_head_prx->setTiltBoth(tilt);
- posx=180-ximagen;
- pan+=-0.0035*posx;
+// posx=180-ximagen;
+// pan+=-0.0035*posx;
// _head_prx->setPanLeft(pan);
-
+ return posy; //devuelve la diferencia en la imagen respecto al centro
}
void Worker::on_pbPrueba_clicked(){
+ buscar_activo=false;
+ Objetivo=Destino;
+
+ buscarObjetivo();
+ tBuscarObjetivo.start(TIME);
+
+// identificarObjetivo(Objetivo);
- identificarPale(red,red);
-
}
-void Worker::buscarPale(){
-// B.generateBaliza(imgD,2);
-// B.acotar(10);
-// QList<baliza *>lrojos,lazules,list;
-// lrojos=B.getList(red);
-// lazules=B.getList(blue);
-
-// qDebug()<<"rojos: "<<lrojos.length()<<"azules"<<lazules.length();
- identificarPale(red,red);
+void Worker::buscarObjetivo(){
+ if(!buscar_activo){
+ _base_prx->setSpeedBase(0.1,0.1);
+ buscar_activo=true;
+// _head_prx->resetHead();
+ }
+ identificarObjetivo(Objetivo);
if(pale_detectado){
-// if(lrojos.length()>=2 && lazules.length()>=1){
-// posible_palet=true;
+ tBuscarObjetivo.stop();
_base_prx->stopBase();
}
- else{
- _base_prx->setSpeedBase(0.1,0.1);
- }
}
//en funcion de la deteccion del palet recalcula la zona de la pantalla en la que buscar
@@ -445,18 +429,18 @@
else{
zonaI=QRect(0,0,camParams.width,camParams.height);
}
- // qDebug()<<"puntos: "<<x1<<y1<<x2<<y2;
- // qDebug()<<"zona: "<<zonaI;
qvisorI->drawSquare(zonaI,Qt::black);
+
+///**/eliminar cuando corresponda
+zonaI=QRect(0,0,camParams.width,camParams.height);
+
}
///Auxiliary
void Worker::newMouseCoor(QPoint p){
-// _base_prx->setPosBase(0,200,0.,50,2.);
-// qDebug()<<bState.alfa;
qDebug()<<"Prueba";
qDebug()<<tc.isColor(255,0,0)<<tc.isColor(0,255,0)<<tc.isColor(0,0,255);
@@ -477,8 +461,8 @@
void Worker::drawPale(){
if(pale_detectado){
QMat p(3);
- float angle=P.getAlfa() * -180/M_PI; //para pintar se utlizan grados
- p=P.getPosicion();
+ float angle=P->getAlfa() * -180/M_PI; //para pintar se utlizan grados
+ p=P->getPosicion();
world->drawSquare(QPoint(p(0),p(2)), WIDTH_PALE, HEIGHT_PALE, Qt::blue,angle);
// world->drawSquare(QPoint(p(0),p(2)), WIDTH_PALE/13, HEIGHT_PALE, Qt::black, angle);
// world->drawSquare(QPoint(p(0),p(2)), WIDTH_PALE, HEIGHT_PALE/3, Qt::blue, angle);
@@ -552,14 +536,17 @@
bezier.Activo=false;
_base_prx->stopBase();
tAproximacion.stop();
+ tIrObjetivo.stop();
}
break;
case irOrigenPale: if(posicionAlcanzada()){
satisfactorio=true;
+ bezier.Activo=false;
_base_prx->stopBase();
tAproximacion.stop();
- tIrPale.stop();
+ tIrObjetivo.stop();
+ pinza_vacia=false;
}
break;
case localizarPale: break;
@@ -584,19 +571,17 @@
case subirPinza: _pinza_prx->setUpPosition();
break;
- case cogerPale: //aproximar robot
- //subirpinza
-
- pinza_vacia=false;
- break;
case dejarPale: //bajar pinza
//retroceder robot
pinza_vacia=true;
break;
- case irOrigenPale: irPale();
- tIrPale.start(T_APROXIMACION);
+ case irOrigenPale: Objetivo=P;
+ irObjetivo();
+ tIrObjetivo.start(T_APROXIMACION);
break;
- case irDestinoPale: irDestino();
+ case irDestinoPale: Objetivo=Destino;
+ irObjetivo();
+ tIrObjetivo.start(T_APROXIMACION);
break;
default: break;
}
@@ -733,3 +718,10 @@
qDebug() << "Requested zero freq!";
}
+//cuadno palet codigo, draw palet que se mueva con el robot
+
+//se puede hacer una lista de objetivos en los que se inserte cada palet con el sitio al que hay que llevarlo
+//en ejecutar plan ir sacando de la lsita los objetivos a medida que sean cumplidos.
+//funcion retroceder que lleve al robot hacia atras un tiempo para que al girar no toque el palet.
+//funcion buscarObjetivo mueva al robot sin direccion hasta localizar el objetivo. podria ser un giro de 360 grados
+ //a la hora de buscar se podría mover la cabeza para simplificarlo
Modified: Components/experimental/paletComp/src/worker.h
===================================================================
--- Components/experimental/paletComp/src/worker.h 2009-08-31 16:49:38 UTC (rev 839)
+++ Components/experimental/paletComp/src/worker.h 2009-08-31 19:12:43 UTC (rev 840)
@@ -74,13 +74,18 @@
private:
Bezier bezier;
Balizas B;
- Pale P;
+ Pale *P;
+ Pale *Destino;
+ Pale *Objetivo;
+
float pinz_Pos;
bool posible_pale;
+ bool pale_activo;
bool pale_detectado;
bool pale_cogido;
bool pinza_vacia;
+ bool buscar_activo;
QRect zonaI;
bool Ejecucion;
@@ -88,7 +93,7 @@
QVector <Etapas>Plan;
-
+ QVector <Pale *>Objetivos;
qWorld *world;
@@ -126,17 +131,16 @@
void upPinza();
//Palet
- void buscarPale();
- void identificarPale(color externo,color centro);
+ void identificarObjetivo(Pale *Objetivo);
void drawPale();
void actualizarZonaBusqueda(int x1,int y1,int x2,int y2);
void mostrarBalizas(QList<baliza *>list);
- void actualizarPos(int x,int y,float alfa);
- void actualizarPosHead(int x,int y);
+ int actualizarPosHead(int x,int y);
+
//Movimiento
QPoint bezierPoint(float t);
void generarBezier(QPoint p1,QPoint p2,QPoint p3); //conserva el punto origen
@@ -161,7 +165,8 @@
QTimer tPlan;
QTimer tAproximacion;
- QTimer tIrPale;
+ QTimer tIrObjetivo;
+ QTimer tBuscarObjetivo;
/*******/
tablaColor tc;
@@ -176,8 +181,9 @@
void ejecutarPlan();
void aproximacion();
- void irPale();
+ void irObjetivo();
void irDestino();
+ void buscarObjetivo();
/**
* Slot that can accept signal from image widgets like QAcho, QVision or QVisor
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|