Java RobotMap.MAX_MODULE_ANGLE属性代码示例

public boolean isOverRotated()
{
    return Math.abs(frontModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE 
            || Math.abs(leftModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE 
            || Math.abs(backModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE 
            || Math.abs(rightModule.getModuleAngle()) > RobotMap.MAX_MODULE_ANGLE;
}
 

public void setAngle(double angle){

        if(!unwinding)
        {
            //The following is uses a weighted rating system to decide which direction we rotate the module
            
            //constants for the weighted average
            final double K_DELTA = RobotMap.K_MODULE_ANGLE_DELTA;
            final double K_TWIST = RobotMap.K_MODULE_ANGLE_TWIST;
            final double K_REVERSE = RobotMap.K_MODULE_ANGLE_REVERSE;
            
            //ensure that anglePID is enabled before running
            anglePID.enable();

            //converts the inputed angle into its reference angle
            angle = angle % 360;

            double setPointForward = angle; // angle setpoint if we want the wheel to move forward
            double setPointBackward = angle + 180; // angle setpoint if we want the wheel to move backwards

            //The following code ensures that our 2 potential setpoints are the ones closest to our current angle
            while(Math.abs(setPointForward - moduleEncoder.getDistance()) > 180
                    && Math.abs(setPointForward) < RobotMap.MAX_MODULE_ANGLE - 180) // while setPointForward is not the closest possible angle to moduleEncoder and getting closer would not bring it past MAX_MOUDLE_ROTATIONS
            {
                if(setPointForward - moduleEncoder.getDistance() < 0) setPointForward += 360; //if we need to add 360 to get closer to moduleEncoder, do so
                else setPointForward -= 360; //else subtract 360
            }

            while(Math.abs(setPointBackward - moduleEncoder.getDistance()) > 180
                    && Math.abs(setPointBackward) < RobotMap.MAX_MODULE_ANGLE - 180) // while setPointBackward is not the closest possible angle to moduleEncoder and getting closer would not bring it past MAX_MOUDLE_ROTATIONS
            {
                if(setPointBackward - moduleEncoder.getDistance() < 0) setPointBackward += 360; //if we need to add 360 to get closer to moduleEncoder, do so
                else setPointBackward -= 360; //else subtract 360
            }

            //rating for how desirable each setpoint is. Higher numbers are better
            double forwardsRating = 0;
            double backwardsRating = 0;

            //Rating for the distance between where the module is currently pointing and each of the setpoints
            forwardsRating -= K_DELTA*Math.abs(setPointForward - moduleEncoder.getDistance());
            backwardsRating -= K_DELTA*Math.abs(setPointBackward - moduleEncoder.getDistance());

            //Rating boost if this setpoint is closer to the 0 (where the wire is completely untwisted) that the current module angle
            if(setPointForward > 0){
                forwardsRating += (moduleEncoder.getDistance() - setPointForward)*K_TWIST; // positive => we are unwinding (moving closer to zero)
            } else {
                forwardsRating += (setPointForward - moduleEncoder.getDistance())*K_TWIST; // negative => we are winding up (moving farther from zero)
            }

            if(setPointBackward > 0){
                backwardsRating += (moduleEncoder.getDistance() - setPointBackward)*K_TWIST; // positive => we are unwinding (moving closer to zero)
            } else {
                backwardsRating += (setPointBackward - moduleEncoder.getDistance())*K_TWIST; // negative => we are winding up (moving farther from zero)
            }

            //Rating for if the how much the velocity will need to change in order the make the wheel go further. Forwards rating gets a positive boost if wheel is already moving forwards, if the wheel is currently moving backwards it gets a deduction.
            forwardsRating += K_REVERSE * wheelEncoder.getRate();

            //Decision making time
            if(forwardsRating > backwardsRating)
            {
                anglePID.setSetpoint(setPointForward);
                invertSpeed = false;
            }
            else
            {
                anglePID.setSetpoint(setPointBackward);
                invertSpeed = true;
            }
        }
    }