|
|
@ -1,8 +1,5 @@
|
|
|
|
package org.firstinspires.ftc.teamcode;
|
|
|
|
package org.firstinspires.ftc.teamcode;
|
|
|
|
|
|
|
|
|
|
|
|
import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap;
|
|
|
|
|
|
|
|
import static org.firstinspires.ftc.teamcode.TankDrive.PARAMS;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
import androidx.annotation.NonNull;
|
|
|
|
import androidx.annotation.NonNull;
|
|
|
|
|
|
|
|
|
|
|
|
import com.acmerobotics.dashboard.canvas.Canvas;
|
|
|
|
import com.acmerobotics.dashboard.canvas.Canvas;
|
|
|
@ -63,22 +60,17 @@ public final class MecanumDrive {
|
|
|
|
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
|
|
|
|
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
|
|
|
|
RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
|
|
|
|
RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
|
|
|
|
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
|
|
|
|
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
|
|
|
|
RevHubOrientationOnRobot.UsbFacingDirection.RIGHT;
|
|
|
|
RevHubOrientationOnRobot.UsbFacingDirection.LEFT;
|
|
|
|
|
|
|
|
|
|
|
|
// drive model parameters
|
|
|
|
// drive model parameters
|
|
|
|
public double inPerTick = 125.5/42126.8333333;
|
|
|
|
//public double inPerTick = 37619.0/112;
|
|
|
|
public double lateralInPerTick = 0.0026542073080477006;
|
|
|
|
public double inPerTick = 18760.5/56;
|
|
|
|
public double trackWidthTicks = 4012.7667474312616;
|
|
|
|
public double lateralInPerTick = 293.2046574609792;
|
|
|
|
|
|
|
|
public double trackWidthTicks = 3968.437903995162;
|
|
|
|
public double par0Yticks = -2097.77;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
public double par1ticks = -2097.77;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
public double perpXTicks = -2497.77;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// feedforward parameters (in tick units)
|
|
|
|
// feedforward parameters (in tick units)
|
|
|
|
public double kS = 0.7579547290234911;
|
|
|
|
public double kS = 0.7748584778215575;
|
|
|
|
public double kV = 0.0005724562271687877;
|
|
|
|
public double kV = 0.0005849868585467051;
|
|
|
|
public double kA = 0;
|
|
|
|
public double kA = 0;
|
|
|
|
|
|
|
|
|
|
|
|
// path profile parameters (in inches)
|
|
|
|
// path profile parameters (in inches)
|
|
|
@ -147,8 +139,6 @@ public final class MecanumDrive {
|
|
|
|
|
|
|
|
|
|
|
|
imu = lazyImu.get();
|
|
|
|
imu = lazyImu.get();
|
|
|
|
|
|
|
|
|
|
|
|
// TODO: reverse encoders if needed
|
|
|
|
|
|
|
|
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
@Override
|
|
|
|
@Override
|
|
|
@ -238,12 +228,15 @@ public final class MecanumDrive {
|
|
|
|
|
|
|
|
|
|
|
|
// TODO: reverse motor directions if needed
|
|
|
|
// TODO: reverse motor directions if needed
|
|
|
|
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
|
|
|
|
rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
|
|
|
|
|
|
|
|
rightBack.setDirection(DcMotorSimple.Direction.FORWARD);
|
|
|
|
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
leftBack.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
|
|
|
|
// TODO: make sure your config has an IMU with this name (can be BNO or BHI)
|
|
|
|
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
|
|
|
// see https://ftc-docs.firstinspires.org/en/latest/hardware_and_software_configuration/configuring/index.html
|
|
|
|
lazyImu = new LazyImu(hardwareMap, "adaimu", new RevHubOrientationOnRobot(
|
|
|
|
lazyImu = new LazyImu(hardwareMap, "imu", new RevHubOrientationOnRobot(
|
|
|
|
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
|
|
|
|
PARAMS.logoFacingDirection, PARAMS.usbFacingDirection));
|
|
|
|
|
|
|
|
|
|
|
|
voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
|
|
|
voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
|
|
|