VoltageTest.java

package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;

/// Voltage testing
import java.io.File;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import com.qualcomm.robotcore.util.ReadWriteFile;
import org.firstinspires.ftc.robotcore.external.navigation.CurrentUnit;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;

@TeleOp
public class VoltageTest extends LinearOpMode{

///     This file is a modified version of BasicMovement2

        DcMotorEx motorRight = null;
        DcMotorEx motorRightback = null;
        DcMotorEx motorLeft = null;
        DcMotorEx motorLeftback = null;

/// Voltage testing
ElapsedTime updateTimer = new ElapsedTime();
public double rampIncrement = 0.15;
public boolean rampUpPower = false;
private VoltageSensor controlHubVoltageSensor = null;
double lfMaxCurrent = 0.0;
double rfMaxCurrent = 0.0;
double lbMaxCurrent = 0.0;
double rbMaxCurrent = 0.0;
double minVoltage = 99.9;


    @Override
    public void runOpMode() throws InterruptedException {

        motorRight = hardwareMap.get(DcMotorEx.class, "motorRight"); //
        motorRightback = hardwareMap.get(DcMotorEx.class, "motorRightback");   //
        motorLeft = hardwareMap.get(DcMotorEx.class, "motorLeft"); //
        motorLeftback = hardwareMap.get(DcMotorEx.class, "motorLeftback");   //
        DcMotor motorWheel = hardwareMap.get(DcMotor.class, "motorWheel");  //
        DcMotor motorWheelred = hardwareMap.get(DcMotor.class, "motorWheelred");
        Servo pusherahhting = hardwareMap.get(Servo.class, "pusherahhting");
        

        motorLeft.setDirection(DcMotor.Direction.REVERSE);
        motorLeftback.setDirection(DcMotor.Direction.REVERSE);

///        waitForStart();
/// Voltage Test
controlHubVoltageSensor = hardwareMap.get(VoltageSensor.class, "Control Hub");
checkInputsAndWaitForStart();

        //Stick power
        while (opModeIsActive()) {
            double y = gamepad1.left_stick_y; // Remember, this is reversed!
            double x = -gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
            double rx = -gamepad1.right_stick_x;
            
            //Maths
            double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
            double LeftPower = (Math.pow((y + x + rx), 3) * 0.8) / denominator;
            double LeftbackPower = (Math.pow((y - x + rx), 3) * 0.8) / denominator;
            double RightPower = (Math.pow((y - x - rx), 3) * 0.8) / denominator;
            double RightbackPower = (Math.pow((y + x - rx), 3) * 0.8) / denominator;

/// Voltage Test
printVoltageData ();
if (rampUpPower) {
    if (updateTimer.seconds() > 0.1)
    {
        // increase slowly
        if (LeftPower > 0) LeftPower = Math.min(LeftPower, motorLeft.getPower () + rampIncrement);
        else if (LeftPower < 0) LeftPower = Math.max(LeftPower, motorLeft.getPower () - rampIncrement);
        if (LeftbackPower > 0) LeftbackPower = Math.min(LeftbackPower, motorLeftback.getPower () + rampIncrement);
        else if (LeftbackPower < 0) LeftbackPower = Math.max(LeftbackPower, motorLeftback.getPower () - rampIncrement);
        if (RightPower > 0) RightPower = Math.min(RightPower, motorRight.getPower () + rampIncrement);
        else if (RightPower < 0) RightPower = Math.max(RightPower, motorRight.getPower () - rampIncrement);
        if (RightbackPower > 0) RightbackPower = Math.min(RightbackPower, motorRightback.getPower () + rampIncrement);
        else if (RightbackPower < 0) RightbackPower = Math.max(RightbackPower, motorRightback.getPower () - rampIncrement);
                
        updateTimer.reset ();
    }
    else
    {
        // keep as is
        LeftPower = motorLeft.getPower ();
        LeftbackPower = motorLeftback.getPower ();
        RightPower = motorRight.getPower ();
        RightbackPower = motorRightback.getPower ();
    }
}

            motorLeft.setPower(LeftPower);
            motorLeftback.setPower(LeftbackPower);
            motorRight.setPower(RightPower);
            motorRightback.setPower(RightbackPower);
            
             //shooting
            if (gamepad1.left_trigger>0){
                motorWheel.setPower(.55);
                motorWheelred.setPower(-.55);
            }
            if (gamepad1.left_trigger == 0){
                motorWheel.setPower(0);
                motorWheelred.setPower(0);
            }
            //Servo
            if (gamepad1.right_trigger>0){
                pusherahhting.setPosition(.13);
            }
            if (gamepad1.right_trigger == 0){
                pusherahhting.setPosition(.03);
                
            }
            
            
            
        }
    }

/// Voltage Test
private void printVoltageData () {

    double lfCurrent = motorLeft.getCurrent(CurrentUnit.AMPS);
    double lbCurrent = motorLeftback.getCurrent(CurrentUnit.AMPS);
    double rfCurrent = motorRight.getCurrent(CurrentUnit.AMPS);
    double rbCurrent = motorRightback.getCurrent(CurrentUnit.AMPS);
    double voltage = controlHubVoltageSensor.getVoltage();

    lfMaxCurrent = Math.max(lfMaxCurrent, lfCurrent);
    lbMaxCurrent = Math.max(lbMaxCurrent, lbCurrent);
    rfMaxCurrent = Math.max(rfMaxCurrent, rfCurrent);
    rbMaxCurrent = Math.max(rbMaxCurrent, rbCurrent);
    minVoltage = Math.min (minVoltage, voltage);

    telemetry.addLine().addData("LeftFront ", lfCurrent).addData (" Max", lfMaxCurrent);
    telemetry.addLine().addData("LeftBack  ", lbCurrent).addData (" Max", lbMaxCurrent);
    telemetry.addLine().addData("RightFront", rfCurrent).addData (" Max", rfMaxCurrent);
    telemetry.addLine().addData("RightBack ", rbCurrent).addData (" Max", rbMaxCurrent);
    telemetry.addLine().addData("   Voltage", voltage).addData (" Min", minVoltage);
    telemetry.update();
}

private void checkInputsAndWaitForStart() {

String rampSetting = "0";
File myFileName = AppUtil.getInstance().getSettingsFile("voltageSetting.txt");
rampSetting = ReadWriteFile.readFile(myFileName);

telemetry.addData("Init Setting", "'" + rampSetting + "'"); 
if (rampSetting.equals (""))
{
    rampSetting = "0";
    rampUpPower = false;
}
telemetry.addData("Setting", "'" + rampSetting + "'"); 
telemetry.update();
sleep(1000);

while (!opModeIsActive() && !isStopRequested())
{
    telemetry.addData(":", "Press A to cycle through options");
    telemetry.addLine("");
    if (rampSetting.equals ("0")) telemetry.addLine("No ramp up");
    if (rampSetting.equals ("1")) telemetry.addLine("Ramp up 0.20");
    if (rampSetting.equals ("2")) telemetry.addLine("Ramp up 0.15");
    if (rampSetting.equals ("3")) telemetry.addLine("Ramp up 0.10");
    telemetry.update();

        if (rampSetting.equals ("0"))
        {
            rampUpPower = false;
        }
        else if (rampSetting.equals ("1"))
        {
            rampUpPower = true;
            rampIncrement = 0.20;
        }
        else if (rampSetting.equals ("2"))
        {
            rampUpPower = true;
            rampIncrement = 0.15;
        }
        else if (rampSetting.equals ("3"))
        {
            rampUpPower = true;
            rampIncrement = 0.10;
        }

    if (gamepad1.aWasPressed ())
    {
        if (rampSetting.equals ("0"))
        {
            rampSetting = "1";
        }
        else if (rampSetting.equals ("1"))
        {
            rampSetting = "2";
        }
        else if (rampSetting.equals ("2"))
        {
            rampSetting = "3";
        }
        else if (rampSetting.equals ("3"))
        {
            rampSetting = "0";
        }
        else
        {
            telemetry.addData("Unknown", "'" + rampSetting + "'"); 
            telemetry.update();
            sleep(1000);
        }
        ReadWriteFile.writeFile(myFileName, rampSetting);
    }

}
}

    }