Example #1
0
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;
        

}