bool Angle::operator <(Angle ang)
{
if (this->Value < ang.getValue()) return true;
else return false;
}
bool Angle::IsBetween(Angle lim_inf, Angle lim_sup)
{
return this->IsBetween(lim_inf.getValue(), lim_sup.getValue());
}
Angle Angle::operator -(Angle ang)
{
return Angle(Normalize(Value - ang.getValue()));
}
void ControlReactivo::Compute() {
/**************************************
*
* HACER FUNCIÓN DE IMPRESIÓN EN TXT ESTES DONDE ESTES
*/
//**************** No hay object
if(pointsObjectFrontalDanger.size() <= 0 && pointsObjectDanger.size() <= 0)
{
outputGiro = vg;
outputAvance = va;
}
//****** 1) Velocidad de avance disminuye cuando se acerca a objeto
else
{
float auxrangeMin = rangeActionFrontal;
float distmax=rangeActionFrontal;
float distmin=0.3;
if (pointsObjectFrontalDanger.size() > 0) {
for (int i = 0; i < rangeObjectFront.size(); i++) {
if (rangeObjectFront[i] < auxrangeMin){
auxrangeMin = rangeObjectFront[i];
}
}
float error = (auxrangeMin-distmin)/(distmax-distmin);
outputAvance = kreactivoAvance * error *va;
}
//****** 2) Velocidad de giro
Angle auxangleMin;
auxangleMin.setValue(0.0);
float auxrangeGiroMin = rangeAction;
int angdch=0;
int angizq=0;
if (pointsObjectDanger.size() > 0) {
for (int i = 0; i < rangeObject.size(); i++) {
if (rangeObject[i] < auxrangeGiroMin){
auxrangeGiroMin=rangeObject[i];
auxangleMin=angleObject[i];
if(angleObject[i]<=0)
angdch++;
else
angizq++;
}
}
float errorg=PI/2+0.01-fabs(auxangleMin.getValue());
if(angizq<angdch) //IZQ
outputGiro=kreactivoGiro*errorg;
else //DCH
outputGiro=-kreactivoGiro*errorg;
if(disttray<0.2)//No hagas caso a react y sigue tray!
outputGiro=vg;
// //Que no solo tenga prioridad esquivar obstaculos, tb distancia a tray
// cout<<"dist "<<disttray<<endl;
// double distPreferenciaReactivo=0.70;
// if(angizq<angdch)
// {
// if(disttray>distPreferenciaReactivo)
// {
// //Derecha obligada
// outputGiro=-kreactivoGiro*errorg;
// cout<<"dch obl"<<endl;
// }
// else
// {
// //Izquierda
// outputGiro=kreactivoGiro*errorg;
// cout<<"izq"<<endl;
// }
// }
//
// else
// {
// if(disttray>distPreferenciaReactivo)
// {
// //Izquierda obligada
// outputGiro=kreactivoGiro*errorg;
// cout<<"izq obl"<<endl;
// }
// else
// {
// //Derecha
// outputGiro=-kreactivoGiro*errorg;
// cout<<"dch"<<endl;
// }
// }
}
}
//SATURACION
if(outputGiro>2.0)
outputGiro=2.0;
if(outputAvance>2.0)
outputAvance=2.0;
}