diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..0ad17cb --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,10 @@ + + + + + + + + + \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java new file mode 100644 index 0000000..217ffda --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.roadrunner.drive; + +import static org.firstinspires.ftc.teamcode.constants.ControlBoard.ODO_LEFT_ENCODER; +import static org.firstinspires.ftc.teamcode.constants.ControlBoard.ODO_RIGHT_ENCODER; +import static org.firstinspires.ftc.teamcode.constants.ControlBoard.ODO_STRAFE_ENCODER; + +import androidx.annotation.NonNull; + +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.roadrunner.geometry.Pose2d; +import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder; + +import java.util.Arrays; +import java.util.List; + +/* + * Sample tracking wheel localizer implementation assuming the standard configuration: + * + * /--------------\ + * | ____ | + * | ---- | + * | || || | + * | || || | + * | | + * | | + * \--------------/ + * + */ +@Config +public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer { + public static double TICKS_PER_REV = 8192; + public static double WHEEL_RADIUS = 0.6889764; // in + public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed + + public static double LATERAL_DISTANCE = 12.3206682876705018; // in; distance between the left and right wheels + public static double FORWARD_OFFSET = 1.28125; // in; offset of the lateral wheel + // 2.6 in + + public static double X_MULTIPLIER = 1.10506990189; // Multiplier in the X direction + public static double Y_MULTIPLIER = 1.09955932763; // Multiplier in the Y direction + + private final Encoder leftEncoder; + private final Encoder rightEncoder; + private final Encoder strafeEncoder; + + public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) { + super(Arrays.asList( + new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left + new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right + new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front + )); + + leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, ODO_LEFT_ENCODER.getDeviceName())); + rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, ODO_RIGHT_ENCODER.getDeviceName())); + strafeEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, ODO_STRAFE_ENCODER.getDeviceName())); + + strafeEncoder.setDirection(Encoder.Direction.REVERSE); + } + + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; + } + + @NonNull + @Override + public List getWheelPositions() { + return Arrays.asList( + encoderTicksToInches(leftEncoder.getCurrentPosition()) * X_MULTIPLIER, + encoderTicksToInches(rightEncoder.getCurrentPosition()) * X_MULTIPLIER, + encoderTicksToInches(strafeEncoder.getCurrentPosition()) * Y_MULTIPLIER + ); + } + + @NonNull + @Override + public List getWheelVelocities() { + return Arrays.asList( + encoderTicksToInches(leftEncoder.getCorrectedVelocity()) * X_MULTIPLIER, + encoderTicksToInches(rightEncoder.getCorrectedVelocity()) * X_MULTIPLIER, + encoderTicksToInches(strafeEncoder.getCorrectedVelocity()) * Y_MULTIPLIER + ); + } +} diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSlideSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSlideSubsystem.kt new file mode 100644 index 0000000..fa2af43 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSlideSubsystem.kt @@ -0,0 +1,7 @@ +package org.firstinspires.ftc.teamcode.subsystems.arm + +import com.arcrobotics.ftclib.command.SubsystemBase + +class ArmSlideSubsystem : SubsystemBase() { + +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt index cbb3589..73236ca 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt @@ -1,7 +1,7 @@ -package org.firstinspires.ftc.teamcode.utils.roadrunner.drive +package org.firstinspires.ftc.teamcode.roadrunner.drive; -import com.acmerobotics.dashboard.config.Config -import com.qualcomm.robotcore.hardware.PIDFCoefficients +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; /* * Constants shared between multiple drive types. @@ -17,12 +17,13 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients * and op modes themselves. */ @Config -object DriveConstants { +public class DriveConstants { + /* * These are motor constants that should be listed online for your motors. */ - const val TICKS_PER_REV = 383.6 - const val MAX_RPM = 435.0 + public static final double TICKS_PER_REV = 383.6; + public static final double MAX_RPM = 435; /* * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders. @@ -32,13 +33,9 @@ object DriveConstants { * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients * from DriveVelocityPIDTuner. */ - const val RUN_USING_ENCODER = false - - @JvmField - var MOTOR_VELO_PID = PIDFCoefficients( - 0.0, 0.0, 0.0, - getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV) - ) + public static final boolean RUN_USING_ENCODER = false; + public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0, + getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV)); /* * These are physical constants that can be determined from your robot (including the track @@ -48,14 +45,9 @@ object DriveConstants { * angular distances although most angular parameters are wrapped in Math.toRadians() for * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO. */ - @JvmField - var WHEEL_RADIUS = 1.8898 // in - - @JvmField - var GEAR_RATIO = 0.5 // output (wheel) speed / input (motor) speed - - @JvmField - var TRACK_WIDTH = 15.75 // in + public static double WHEEL_RADIUS = 1.88976; // in` + public static double GEAR_RATIO = 0.5; // output (wheel) speed / input (motor) speed + public static double TRACK_WIDTH = 13.1; // in /* * These are the feedforward parameters used to model the drive motor behavior. If you are using @@ -63,14 +55,9 @@ object DriveConstants { * motor encoders or have elected not to use them for velocity control, these values should be * empirically tuned. */ - @JvmField - var kV = 0.025 - - @JvmField - var kA = 0.005 - - @JvmField - var kStatic = 0.01 + public static double kV = 0.024; + public static double kA = 0.003; + public static double kStatic = 0.01; /* * These values are used to generate the trajectories for you robot. To ensure proper operation, @@ -83,48 +70,39 @@ object DriveConstants { * Note from LearnRoadRunner.com: * The velocity and acceleration constraints were calculated based on the following equation: * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85 - * Resulting in 36.58665032249647 in/s. + * Resulting in 73.17330064499293 in/s. * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above. * This is capped at 85% because there are a number of variables that will prevent your bot from actually * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc. * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically - * max velocity. The theoretically maximum velocity is 43.04311802646643 in/s. + * max velocity. The theoretically maximum velocity is 86.08623605293286 in/s. * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally * affected if it is aiming for a velocity not actually possible. * * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on * actual testing. Just set it at a reasonable value and keep increasing until your path following starts - * to degrade. As of now, it simply mirrors the velocity, resulting in 36.58665032249647 in/s/s + * to degrade. As of now, it simply mirrors the velocity, resulting in 73.17330064499293 in/s/s * * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s. * You are free to raise this on your own if you would like. It is best determined through experimentation. */ - @JvmField - var MAX_VEL = 36.58665032249647 - - @JvmField - var MAX_ACCEL = 36.58665032249647 - - @JvmField - var MAX_ANG_VEL = Math.toRadians(163.028007) + public static double MAX_VEL = 38.110287416570166; + public static double MAX_ACCEL = 38.110287416570166; + public static double MAX_ANG_VEL = Math.toRadians(355.94452299662714); + public static double MAX_ANG_ACCEL = Math.toRadians(172.2983035714285); - @JvmField - var MAX_ANG_ACCEL = Math.toRadians(123.30945) - @JvmStatic - fun encoderTicksToInches(ticks: Double): Double { - return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV + public static double encoderTicksToInches(double ticks) { + return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV; } - @JvmStatic - fun rpmToVelocity(rpm: Double): Double { - return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0 + public static double rpmToVelocity(double rpm) { + return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0; } - @JvmStatic - fun getMotorVelocityF(ticksPerSecond: Double): Double { - // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx - return 32767 / ticksPerSecond + public static double getMotorVelocityF(double ticksPerSecond) { + // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx + return 32767 / ticksPerSecond; } } \ No newline at end of file