Java MathUtils類代碼示例

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
/**
    * Configure the duty-cycle of the PWM generator
    * @param pwmGenerator The generator index reserved by allocateDO_PWM()
    * @param dutyCycle The percent duty cycle to output [0..1].
    */
   public void setDO_PWMDutyCycle(int pwmGenerator, double dutyCycle) {
if (pwmGenerator == ~0) return;
       if (dutyCycle > 1.0) {
           dutyCycle = 1.0;
       }
       if (dutyCycle < 0.0) {
           dutyCycle = 0.0;
       }
       double rawDutyCycle = 256.0 * dutyCycle;
       if (rawDutyCycle > 255.5) {
           rawDutyCycle = 255.5;
       }
       byte pwmPeriodPower = m_fpgaDIO.readDO_PWMConfig_PeriodPower();
       if (pwmPeriodPower < 4) {
           // The resolution of the duty cycle drops close to the highest frequencies.
           rawDutyCycle = rawDutyCycle / MathUtils.pow(2.0, 4 - pwmPeriodPower);
       }
       m_fpgaDIO.writeDO_PWMDutyCycle(pwmGenerator, (byte)rawDutyCycle);
   }
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
protected void execute() {
    jx = getJoystickX();
    jy = -getJoystickY();
    jz = getJoystickZ();
    leftOffCenter = (Math.abs(jx) > deadBand || Math.abs(jy) > deadBand);
    rightOffCenter = (Math.abs(jz) > deadBand);
    if(leftOffCenter){     //if outside deadband
        magnitude = Math.sqrt(MathUtils.pow(jx,2) + MathUtils.pow(jy,2));
        findBearing();
        quadrantCompensation();
        //SmartDashboard.putNumber("X Value: ", jx);
        //SmartDashboard.putNumber("Y Value: ", jy);
        SmartDashboard.putNumber("Bearing: ", bearing);
        SmartDashboard.putNumber("Magnitude: ", magnitude);
        mecanumDriveTrain.translate(magnitude, bearing, jz);
        return;
    }
    if(rightOffCenter){
        mecanumDriveTrain.turn(0.3*getJoystickZ());
    }
    else{
        mecanumDriveTrain.stop();
    }
  
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
public void mecanumDrive(double x, double y, double rotation, double gyro) {

        SmartDashboard.putNumber("[DT] X", x);
        SmartDashboard.putNumber("[DT] Y", y);
        SmartDashboard.putNumber("[DT] Theta", rotation);
        SmartDashboard.putNumber("[DT] Gyro", gyro);
        if(DriveTrain.polarMode == true){
            SmartDashboard.putString("polarMode", "Polar Mode");
        }else{
            SmartDashboard.putString("polarMode", "Field Oriented");
        }

        if (polarMode == true) {
            drive.mecanumDrive_Polar(Math.sqrt(x * x + y * y), (Math.toDegrees(MathUtils.atan2(y, x)) - 90), rotation);
        } else {
            drive.mecanumDrive_Cartesian(.7 * x, .7 * y, .7 * rotation, gyro);
        }

    }
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
/**
 * Returns the closest Target object to the given point and with the given
 * type. The distance from the point to the center of all Target objects of the proper
 * type is computed and the target with the smallest such distance is returned.
 * @param x The x-coordinate of the point to which the distance is computed.
 * @param y The y-coordinate of the point to which the distance is computed.
 * @param centerTarget The type of target to return. {@code true} indicates
 * a high, center target and {@code false} indicates a low, non-center target.
 * @return The target with the smallest distance to the specified point
 * or {@code null} if no such target exists.
 */
public Target getClosestTarget(double x, double y, boolean centerTarget){
    double minDistance = Double.MAX_VALUE;
    Target foundTarget = null;
    for(int i = 0; i < targets.size(); i++){
        Target target = (Target)targets.elementAt(i);
        if(target.isCenter() == centerTarget){
            double sqDistance = MathUtils.pow(target.getX() - x, 2)+MathUtils.pow(target.getY() - y, 2);
            if(sqDistance < minDistance){
                minDistance = sqDistance;
                foundTarget = target;
            }
        }
    }
    return foundTarget;
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
/**
     * getDegreesToTarget()
     *
     * This method returns the angle to the target.
     *
     * @return the angle to the target, negative th robot needs to turn left,
     * positive, right
     */
    public double getDegreesToTarget() throws NoTargetFoundException
    {
        double offset;
        if (target == null)
        {
            throw new NoTargetFoundException("No target found.");
        } else
        {
            offset = target.center_mass_x - (IMAGE_WIDTH / 2.0);
            offset = offset * (TARGET_WIDTH / target.target_width);
            offset = MathUtils.atan(offset / getDistanceToTarget());
            return offset;
        }
//        else
//        {
//            offset = ((double) r.center_mass_x) - (IMAGE_WIDTH / 2.0);
//            offset = offset * (24.0 / ((double) r.boundingRectWidth));
//            offset = MathUtils.atan(offset / getDistanceWCamera());
//            return Math.toDegrees(offset);
//        }
    }
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
public double getMecanumMagnitude() {
    double x = driverController.getRawAxis(XBoxControllerMap.LEFT_X_AXIS);
    double y = driverController.getRawAxis(XBoxControllerMap.LEFT_Y_AXIS);
    double returnMagnitude = 0.0;
    double squareMagnitude = Math.sqrt(MathUtils.pow(x, 2.0) + MathUtils.pow(y, 2.0));
    if(x != 0.0 && y != 0.0){
        double borderX;
        double borderY;
        if(Math.abs(x) > Math.abs(y)){ //scale x to 1.0
            borderX = x / Math.abs(x);
            borderY = y / Math.abs(x);
        } else { //scale y to 1.0
            borderX = x / Math.abs(y);
            borderY = y / Math.abs(y);
        }
        double borderMagnitude = Math.sqrt(MathUtils.pow(borderX, 2.0) + MathUtils.pow(borderY, 2.0));
        returnMagnitude = squareMagnitude / borderMagnitude;
    } else {
        returnMagnitude = squareMagnitude;
    }
    
    if (returnMagnitude < RobotMap.DEADZONE_RADIUS) {
        returnMagnitude = 0.0;
    }        
    return returnMagnitude;
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
public double getMecanumDirection(){
    double direction;
    double x = driverController.getRawAxis(LEFT_X_AXIS);
    double y = driverController.getRawAxis(LEFT_Y_AXIS);
    //y *= -1.0; <-- Created a flip problem
    double magnitude = getMecanumMagnitude();
    
    if(magnitude < DEADZONE_RADIUS){
        direction = 0.0;
    }
    else{
        direction = MathUtils.atan2(y, x);
    }
    direction -= Math.PI/2;
    
    
    return direction;
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
public static double mapJoystickToPowerOutput(double input)
   {
       if(Math.abs(input) < 0.05)
{
	// Stop if joystick is near zero
	return 0.0;
}
else
{
           double mapping;

           if(Math.abs(input) <= 0.75)
           {
                   mapping = 0.95 * ((0.5 * MathUtils.pow(Math.abs(input), 2.0)) + 0.2);
                   mapping = (input >= 0) ? mapping : -mapping; // Change to negative if the input was negative
                   return mapping;
           }
           else
           {
                   mapping = 2.16 * Math.abs(input) - 1.16;
                   mapping = (input >= 0) ? mapping : -mapping; // Change to negative if the input was negative
                   return mapping;
           }
}
   }
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
protected void execute() {
    // Using inverse kinematics from Ether at ChiefDelphi (http://www.chiefdelphi.com/media/papers/2426) //
    double targetAngularSpeed = -1 * oi.getRightStickX(); //Right stick X controls rotation speed; positive = right = clockwise
    double targetVelocityX = -1 * oi.getLeftStick().getX();
    double targetVelocityY = oi.getLeftStick().getY();
    
    // Calculation factors //
    double A = targetVelocityX - (.5 * targetAngularSpeed * ROBOT_LENGTH);
    double B = targetVelocityX + (.5 * targetAngularSpeed * ROBOT_LENGTH);
    double C = targetVelocityY - (.5 * targetAngularSpeed * ROBOT_LENGTH);
    double D = targetVelocityY + (.5 * targetAngularSpeed * ROBOT_LENGTH);

    double wheel1Speed = Math.sqrt((B * B) + (D * D));
    double wheel2Speed = Math.sqrt((B * B) + (C * C));
    double wheel3Speed = Math.sqrt((A * A) + (C * C));
    double wheel4Speed = Math.sqrt((A * A) + (D * D));
    
    double maxSpeed = Math.max(Math.max(wheel1Speed, wheel2Speed), Math.max(wheel3Speed, wheel4Speed));
    
    if (maxSpeed > 1.0) {
        wheel1Speed = wheel1Speed / maxSpeed;
        wheel2Speed = wheel2Speed / maxSpeed;
        wheel3Speed = wheel3Speed / maxSpeed;
        wheel4Speed = wheel4Speed / maxSpeed;
    }
    
    double wheel1Angle = MathUtils.atan2(B, D) * 180.0 / Math.PI;
    double wheel2Angle = MathUtils.atan2(B, C) * 180.0 / Math.PI;
    double wheel3Angle = MathUtils.atan2(A, C) * 180.0 / Math.PI;
    double wheel4Angle = MathUtils.atan2(A, D) * 180.0 / Math.PI;
    
    if (maxSpeed > 0.05) {
        swervedrive.setWheelAngles(wheel1Angle, wheel2Angle, wheel3Angle, wheel4Angle);
        swervedrive.setWheelPowers(wheel1Speed, wheel2Speed, wheel3Speed, wheel4Speed);
    } else {
        swervedrive.setAllWheelAngles(180);
        swervedrive.setAllWheelPowers(0);
    }
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
private void GetAccelerationFromJoyStick(double x, double y) {
    // use particle simulation to estimate acceleration from left joystick input
    double xVelocity = prevXVelocity + x;  // update velocity with accel from joystick
    double yVelocity = prevYVelocity + y;
    xVelocity = xVelocity * 0.995;  // viscous drag
    yVelocity = yVelocity * 0.995;
    double xAccel = xVelocity - prevXVelocity;  // compute new accel
    double yAccel = yVelocity - prevYVelocity;
    if (Math.abs(xAccel)>0.05 || Math.abs(yAccel)>0.05) {
       accelerometerAngle = MathUtils.atan2(-xAccel, yAccel);
       SmartDashboard.putNumber("accel angle", -accelerometerAngle);
    }
    prevXVelocity = xVelocity;
    prevYVelocity = yVelocity;
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
/**
 * Calculates exact decimal number of target clicks. Used in isAbsoluteWound
 * method to drive robot until an 'acceptable' error is attained.
 *
 * @return Exact decimal number of target clicks.
 */
private double calculateExactTargetClick() {
    double distance;
    
    if (cameraInput)
        distance = camera.getDistance();
    else
        distance = rangefinder.getRangeInches();
    
    return windQuadratic * MathUtils.pow(distance, 2)
            + windSlope * distance + windIntercept;
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
/**
 * Controls the drive train when using the mecanum wheels
 */
public void mecanum()
{
    // Gets the latest values of the right joysticks 
    double u = storage.con.RIGHT_STICK_LEFT_RIGHT.getVal();
    double v = storage.con.RIGHT_STICK_UP_DOWN.getVal();
    
    // Calculates the ideal speed, angle, and rotation
    double speed = Math.sqrt(u * u + v * v);
    double angle = MathUtils.atan(v / u);
    
    // Rotation outputs a value from -1.0 to 1.0
    // -1.0 is turning left at the fastest speed possible, 1.0 is right at the fastest speed possible
    double rotation = storage.con.LEFT_STICK_LEFT_RIGHT.getVal();
    
    // Determines what to set the motor, see mecanum PDF for more info
    double fl = speed * Math.sin(angle + Math.PI / 4) + rotation;
    double fr = speed * Math.cos(angle + Math.PI / 4) - rotation;
    double bl = speed * Math.cos(angle + Math.PI / 4) + rotation;
    double br = speed * Math.sin(angle + Math.PI / 4) - rotation;
    
    // Max value a motor can take (1.0 is fastest speed)
    double max = 1;
    
    // If one of the motor speeds is going to be > 1.0, that means we need to scale
    // all of the motors down so the fastest is at 1.0
    if(Math.abs(fl) > 1 || Math.abs(fr) > 1 || Math.abs(bl) > 1 || Math.abs(br) > 1)
    {
        max = Math.max(Math.abs(fl), Math.abs(fr));
        max = Math.max(max, Math.abs(bl));
        max = Math.max(max, Math.abs(br));
    }
    
    // Sets the motors' speeds, and scales if any value is outside of the range [-1,1]
    storage.data.MOTOR_FL.setX(fl / max);
    storage.data.MOTOR_FR.setX(fr / max);
    storage.data.MOTOR_BL.setX(bl / max);
    storage.data.MOTOR_BR.setX(br / max);
    
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
public void mecanum()
{
    double lr = storage.con.RIGHT_STICK_LEFT_RIGHT.getVal();
    double ud = storage.con.RIGHT_STICK_UP_DOWN.getVal();
           
    double mag = Math.sqrt(lr * lr + ud * ud);
    double angle = MathUtils.atan(ud / lr);
    double rotation = storage.con.LEFT_STICK_LEFT_RIGHT.getVal();
    
    double fl = mag * Math.sin(angle + Math.PI / 4) + rotation;
    double fr = mag * Math.cos(angle + Math.PI / 4) - rotation;
    double bl = mag * Math.cos(angle + Math.PI / 4) + rotation;
    double br = mag * Math.sin(angle + Math.PI / 4) - rotation;
    
    double max = 1;
    
    if(Math.abs(fl) > 1 || Math.abs(fr) > 1 || Math.abs(bl) > 1 || Math.abs(br) > 1)
    {
        max = Math.max(Math.abs(fl),Math.abs(fr));
        max = Math.max(max, Math.abs(bl));
        max = Math.max(max, Math.abs(br));
    }
    
    storage.data.MOTOR_FL.setX(fl / max);
    storage.data.MOTOR_FR.setX(fr / max);
    storage.data.MOTOR_BL.setX(bl / max);
    storage.data.MOTOR_BR.setX(br / max);
    
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
/**
 * This method does exponent math without changing the original sign
 * (+ or -). For example, Math.pow(-2, 4) evaluates to 16 while
 * monsieurKevin(-2, 4) evaluates to -16. (The method name is an inside
 * joke).
 * @param base is the base
 * @param exp is the exponent
 * @return @param base^@param exp while keeping the sign of @param base
 */
public double monsieurKevin(double base, int exp){
    double multiplier = 1;
    
    if(base < 0){
        multiplier *= -1;
    }
    
    double ans = MathUtils.pow(base, exp);
    ans *= multiplier;
    
    return ans;
}
 

import com.sun.squawk.util.MathUtils; //導入依賴的package包/類
private void findBearing(){
    if(jy == 0){
        if(jx > 0){
            bearing = 90;
        }
        if(jx < 0){
            bearing = -90;
        }
    }
    else{
        bearing = Math.toDegrees(MathUtils.atan(jx / jy));
    }
}