|
|
|
@ -1,8 +1,5 @@
|
|
|
|
|
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 com.acmerobotics.dashboard.canvas.Canvas;
|
|
|
|
@ -61,24 +58,18 @@ public final class MecanumDrive {
|
|
|
|
|
// TODO: fill in these values based on
|
|
|
|
|
// see https://ftc-docs.firstinspires.org/en/latest/programming_resources/imu/imu.html?highlight=imu#physical-hub-mounting
|
|
|
|
|
public RevHubOrientationOnRobot.LogoFacingDirection logoFacingDirection =
|
|
|
|
|
RevHubOrientationOnRobot.LogoFacingDirection.DOWN;
|
|
|
|
|
RevHubOrientationOnRobot.LogoFacingDirection.UP;
|
|
|
|
|
public RevHubOrientationOnRobot.UsbFacingDirection usbFacingDirection =
|
|
|
|
|
RevHubOrientationOnRobot.UsbFacingDirection.RIGHT;
|
|
|
|
|
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
|
|
|
|
|
|
|
|
|
|
// drive model parameters
|
|
|
|
|
public double inPerTick = 125.5/42126.8333333;
|
|
|
|
|
public double lateralInPerTick = 0.0026542073080477006;
|
|
|
|
|
public double trackWidthTicks = 4012.7667474312616;
|
|
|
|
|
|
|
|
|
|
public double par0Yticks = -2097.77;
|
|
|
|
|
|
|
|
|
|
public double par1ticks = -2097.77;
|
|
|
|
|
|
|
|
|
|
public double perpXTicks = -2497.77;
|
|
|
|
|
public double inPerTick = 1;
|
|
|
|
|
public double lateralInPerTick = inPerTick;
|
|
|
|
|
public double trackWidthTicks = 0;
|
|
|
|
|
|
|
|
|
|
// feedforward parameters (in tick units)
|
|
|
|
|
public double kS = 0.7579547290234911;
|
|
|
|
|
public double kV = 0.0005724562271687877;
|
|
|
|
|
public double kS = 0;
|
|
|
|
|
public double kV = 0;
|
|
|
|
|
public double kA = 0;
|
|
|
|
|
|
|
|
|
|
// path profile parameters (in inches)
|
|
|
|
@ -238,17 +229,15 @@ public final class MecanumDrive {
|
|
|
|
|
|
|
|
|
|
// TODO: reverse motor directions if needed
|
|
|
|
|
// leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
|
|
|
|
|
leftFront.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)
|
|
|
|
|
// 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));
|
|
|
|
|
|
|
|
|
|
voltageSensor = hardwareMap.voltageSensor.iterator().next();
|
|
|
|
|
|
|
|
|
|
localizer =new ThreeDeadWheelLocalizer(hardwareMap, PARAMS.inPerTick);
|
|
|
|
|
localizer = new DriveLocalizer();
|
|
|
|
|
|
|
|
|
|
FlightRecorder.write("MECANUM_PARAMS", PARAMS);
|
|
|
|
|
}
|
|
|
|
|