diff --git a/src/main/deploy/YAGSL/modules/backright.json b/src/main/deploy/YAGSL/modules/backright.json index 824b0e6..bf49ebe 100644 --- a/src/main/deploy/YAGSL/modules/backright.json +++ b/src/main/deploy/YAGSL/modules/backright.json @@ -18,7 +18,7 @@ "drive": false, "angle": true }, - "absoluteEncoderOffset": -108.896, + "absoluteEncoderOffset": 107.22, "location": { "front": -12, "left": -12 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index d88c015..3ac6b6f 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.RobotBase; /** @@ -75,5 +76,7 @@ public enum Mode { public static final int DRIVE_JOYSTICK_PORT = 0; public static final int STEER_JOYSTICK_PORT = 1; + + } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0f6f319..e1b7242 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,12 +7,16 @@ import java.io.File; import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.button.CommandJoystick; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; +import frc.robot.commands.drive.Drive; import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.roller.SpinRoller; import frc.robot.commands.tilt.TiltDown; @@ -99,6 +103,10 @@ private void configureBindings() { // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); + + //basic drive command + Command drive = new Drive(drivebase, 0.1,0.1,2, true, 1); + driveJoystick.button(1).whileTrue(drive); } public void putShuffleboardCommands() { if (Constants.DEBUG) { @@ -132,3 +140,4 @@ public RobotVisualizer getRobotVisualizer() { return robotVisualizer; } } + diff --git a/src/main/java/frc/robot/commands/drive/Drive.java b/src/main/java/frc/robot/commands/drive/Drive.java new file mode 100644 index 0000000..cb43168 --- /dev/null +++ b/src/main/java/frc/robot/commands/drive/Drive.java @@ -0,0 +1,67 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.drive; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; +/** + * Drives the robot + * @param speedX Speed in the x direction in meters per second + * @param speedY Speed in the Y direction in meters per second + * @param radians Rotational speed in radians per second + * @param fieldRelative True for field-relative, false for robot-relative. + * @param time Amount of time before the command ends + */ +public class Drive extends Command { + + private final SwerveSubsystem drivebase; + private final double speedX; + private final double speedY; + private final double radians; + private final boolean fieldRelative; + private final double time; + private Timer timer; + public Drive(SwerveSubsystem drivebase, double speedX, double speedY,double radians, boolean fieldRelative, double time) { + timer = new Timer(); + this.time = time; + this.drivebase = drivebase; + this.speedX = speedX; + this.speedY = speedY; + this.radians = radians; + this.fieldRelative = fieldRelative; + addRequirements(drivebase); + } + + + @Override + public void initialize() { + timer.restart(); + } + + + @Override + public void execute() { + drivebase.drive(new Translation2d(speedX,speedY),radians, fieldRelative); + + } + + @Override + public void end(boolean interrupted) { + + } + + + @Override + public boolean isFinished() { + if (timer.hasElapsed(time)) { + return true; + } else{ + return false; + } + } +}