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; }