Adding MecanumDrivetrain and some other bits and bobs
This commit is contained in:
parent
e636f02e47
commit
546fc06aed
@ -4,15 +4,41 @@
|
||||
|
||||
package frc.robot;
|
||||
|
||||
import edu.wpi.first.epilogue.Logged;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.Commands;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.constants.OIConstants;
|
||||
import frc.robot.interfaces.IDrivetrain;
|
||||
import frc.robot.subsystems.DifferentialDrivetrain;
|
||||
|
||||
public class RobotContainer {
|
||||
// TODO Not sure if the polymorphism will work correctly here when logging
|
||||
@Logged(name = "Drivetrain")
|
||||
private IDrivetrain drivetrain;
|
||||
|
||||
private CommandXboxController driver;
|
||||
|
||||
public RobotContainer() {
|
||||
drivetrain = new DifferentialDrivetrain(
|
||||
new Pose2d(),
|
||||
true,
|
||||
false
|
||||
);
|
||||
|
||||
driver = new CommandXboxController(OIConstants.kDriverUSB);
|
||||
|
||||
configureBindings();
|
||||
}
|
||||
|
||||
private void configureBindings() {}
|
||||
private void configureBindings() {
|
||||
if (drivetrain instanceof DifferentialDrivetrain) {
|
||||
((DifferentialDrivetrain) drivetrain).setDefaultCommand(
|
||||
drivetrain.drive(driver)
|
||||
);
|
||||
}
|
||||
}
|
||||
|
||||
public Command getAutonomousCommand() {
|
||||
return Commands.print("No autonomous command configured");
|
||||
|
||||
52
src/main/java/frc/robot/constants/MecanumDriveConstants.java
Normal file
52
src/main/java/frc/robot/constants/MecanumDriveConstants.java
Normal file
@ -0,0 +1,52 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveKinematics;
|
||||
|
||||
public class MecanumDriveConstants {
|
||||
public static final int kFrontLeftPWM = 0;
|
||||
public static final int kRearLeftPWM = 1;
|
||||
public static final int kFrontRightPWM = 2;
|
||||
public static final int kRearRightPWM = 3;
|
||||
|
||||
// TODO Are these the right values
|
||||
public static final int kFrontLeftEncoderChannelA = 0;
|
||||
public static final int kFrontLeftEncoderChannelB = 1;
|
||||
public static final int kRearLeftEncoderChannelA = 2;
|
||||
public static final int kRearLeftEncoderChannelB = 3;
|
||||
public static final int kFrontRightEncoderChannelA = 4;
|
||||
public static final int kFrontRightEncoderChannelB = 5;
|
||||
public static final int kRearRightEncoderChannelA = 6;
|
||||
public static final int kRearRightEncoderChannelB = 7;
|
||||
|
||||
// TODO Set this to a real value
|
||||
public static final double kDistancePerPulseMeters = 0;
|
||||
|
||||
public static final double kSlowModeMultiplier = .75;
|
||||
|
||||
// TODO Set these to a real value
|
||||
public static final Translation2d kFrontLeftTranslation =
|
||||
new Translation2d(0, 0);
|
||||
public static final Translation2d kRearLeftTranslation =
|
||||
new Translation2d(0, 0);
|
||||
public static final Translation2d kFrontRightTranslation =
|
||||
new Translation2d(0, 0);
|
||||
public static final Translation2d kRearRightTranslation =
|
||||
new Translation2d(0, 0);
|
||||
|
||||
public static final double kMaxSpeedMetersPerSecond = 0;
|
||||
public static final double kMaxAngularSpeedRadiansPerSecond = 0;
|
||||
|
||||
public static final boolean kInvertForwardBackward = true;
|
||||
public static final boolean kInvertLeftRight = true;
|
||||
public static final boolean kInvertRotation = true;
|
||||
public static final boolean kInvertGyro = true;
|
||||
public static final boolean kInvertEncoders = false;
|
||||
|
||||
public static final MecanumDriveKinematics kKinematics = new MecanumDriveKinematics(
|
||||
kFrontLeftTranslation,
|
||||
kFrontRightTranslation,
|
||||
kRearLeftTranslation,
|
||||
kRearRightTranslation
|
||||
);
|
||||
}
|
||||
5
src/main/java/frc/robot/constants/OIConstants.java
Normal file
5
src/main/java/frc/robot/constants/OIConstants.java
Normal file
@ -0,0 +1,5 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
public class OIConstants {
|
||||
public static final int kDriverUSB = 0;
|
||||
}
|
||||
@ -36,6 +36,9 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
|
||||
private boolean slowMode;
|
||||
private boolean fieldOriented;
|
||||
|
||||
@Logged(name = "Drivebase Type", importance = Importance.INFO)
|
||||
public static final String kDrivebaseType = "Differential";
|
||||
|
||||
public DifferentialDrivetrain(Pose2d initialPose, boolean slowModeAtStartup, boolean fieldOrientedAtStartup) {
|
||||
super();
|
||||
|
||||
@ -96,7 +99,7 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
|
||||
}
|
||||
|
||||
@Override
|
||||
@Logged(name = "DD Pose", importance = Importance.INFO)
|
||||
@Logged(name = "Pose", importance = Importance.INFO)
|
||||
public Pose2d getPose() {
|
||||
return poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
@ -188,27 +191,27 @@ public class DifferentialDrivetrain extends SubsystemBase implements IDrivetrain
|
||||
});
|
||||
}
|
||||
|
||||
@Logged(name = "DD Gyro Angle", importance = Importance.DEBUG)
|
||||
@Logged(name = "Gyro Angle Degrees", importance = Importance.DEBUG)
|
||||
public double getGyroAngle() {
|
||||
return gyro.getAngle() * (DifferentialDriveConstants.kInvertGyro ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "DD Left Encoder Meters", importance = Importance.DEBUG)
|
||||
@Logged(name = "Left Encoder Meters", importance = Importance.DEBUG)
|
||||
public double getLeftEncoderMeters() {
|
||||
return leftEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "DD Right Encoder Meters", importance = Importance.DEBUG)
|
||||
@Logged(name = "Right Encoder Meters", importance = Importance.DEBUG)
|
||||
public double getRightEncoderMeters() {
|
||||
return rightEncoder.getDistance() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "DD Left Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
@Logged(name = "Left Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
public double getLeftEncoderMetersPerSecond() {
|
||||
return leftEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "DD Right Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
@Logged(name = "Right Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
public double getRightEncoderMetersPerSecond() {
|
||||
return rightEncoder.getRate() * (DifferentialDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
266
src/main/java/frc/robot/subsystems/MecanumDrivetrain.java
Normal file
266
src/main/java/frc/robot/subsystems/MecanumDrivetrain.java
Normal file
@ -0,0 +1,266 @@
|
||||
package frc.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.epilogue.Logged;
|
||||
import edu.wpi.first.epilogue.Logged.Importance;
|
||||
import edu.wpi.first.math.estimator.MecanumDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
|
||||
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.drive.MecanumDrive;
|
||||
import edu.wpi.first.wpilibj.motorcontrol.VictorSP;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.PrintCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
|
||||
import frc.robot.constants.MecanumDriveConstants;
|
||||
import frc.robot.interfaces.IDrivetrain;
|
||||
|
||||
public class MecanumDrivetrain extends SubsystemBase implements IDrivetrain {
|
||||
private VictorSP frontLeft;
|
||||
private VictorSP rearLeft;
|
||||
private VictorSP frontRight;
|
||||
private VictorSP rearRight;
|
||||
|
||||
private MecanumDrive drive;
|
||||
|
||||
private Encoder frontLeftEncoder;
|
||||
private Encoder rearLeftEncoder;
|
||||
private Encoder frontRightEncoder;
|
||||
private Encoder rearRightEncoder;
|
||||
|
||||
private ADXRS450_Gyro gyro;
|
||||
|
||||
private MecanumDrivePoseEstimator poseEstimator;
|
||||
|
||||
private boolean slowMode;
|
||||
private boolean fieldOriented;
|
||||
|
||||
@Logged(name = "Drivebase Type", importance = Importance.INFO)
|
||||
public static final String kDrivebaseType = "Mecanum";
|
||||
|
||||
public MecanumDrivetrain(Pose2d initialPose, boolean slowModeAtStartup, boolean fieldOrientedAtStartup) {
|
||||
super();
|
||||
|
||||
frontLeft = new VictorSP(MecanumDriveConstants.kFrontLeftPWM);
|
||||
rearLeft = new VictorSP(MecanumDriveConstants.kRearLeftPWM);
|
||||
frontRight = new VictorSP(MecanumDriveConstants.kFrontRightPWM);
|
||||
rearRight = new VictorSP(MecanumDriveConstants.kRearRightPWM);
|
||||
|
||||
drive = new MecanumDrive(frontLeft, rearLeft, frontRight, rearRight);
|
||||
|
||||
frontLeftEncoder = new Encoder(
|
||||
MecanumDriveConstants.kFrontLeftEncoderChannelA,
|
||||
MecanumDriveConstants.kFrontLeftEncoderChannelB
|
||||
);
|
||||
frontLeftEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
|
||||
|
||||
rearLeftEncoder = new Encoder(
|
||||
MecanumDriveConstants.kRearLeftEncoderChannelA,
|
||||
MecanumDriveConstants.kRearLeftEncoderChannelB
|
||||
);
|
||||
rearLeftEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
|
||||
|
||||
frontRightEncoder = new Encoder(
|
||||
MecanumDriveConstants.kFrontRightEncoderChannelA,
|
||||
MecanumDriveConstants.kFrontRightEncoderChannelB
|
||||
);
|
||||
frontRightEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
|
||||
|
||||
rearRightEncoder = new Encoder(
|
||||
MecanumDriveConstants.kRearRightEncoderChannelA,
|
||||
MecanumDriveConstants.kRearRightEncoderChannelB
|
||||
);
|
||||
rearRightEncoder.setDistancePerPulse(MecanumDriveConstants.kDistancePerPulseMeters);
|
||||
|
||||
gyro = new ADXRS450_Gyro();
|
||||
|
||||
poseEstimator = new MecanumDrivePoseEstimator(
|
||||
MecanumDriveConstants.kKinematics,
|
||||
Rotation2d.fromDegrees(getGyroAngle()),
|
||||
new MecanumDriveWheelPositions(
|
||||
getFrontLeftEncoderMeters(),
|
||||
getFrontRightEncoderMeters(),
|
||||
getRearLeftEncoderMeters(),
|
||||
getRearRightEncoderMeters()
|
||||
),
|
||||
initialPose
|
||||
);
|
||||
|
||||
slowMode = slowModeAtStartup;
|
||||
fieldOriented = fieldOrientedAtStartup;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
poseEstimator.update(
|
||||
Rotation2d.fromDegrees(getGyroAngle()),
|
||||
new MecanumDriveWheelPositions(
|
||||
getFrontLeftEncoderMeters(),
|
||||
getFrontRightEncoderMeters(),
|
||||
getRearLeftEncoderMeters(),
|
||||
getRearRightEncoderMeters()
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void resetPose(Pose2d pose) {
|
||||
poseEstimator.resetPosition(
|
||||
Rotation2d.fromDegrees(getGyroAngle()),
|
||||
new MecanumDriveWheelPositions(
|
||||
getFrontLeftEncoderMeters(),
|
||||
getFrontRightEncoderMeters(),
|
||||
getRearLeftEncoderMeters(),
|
||||
getRearRightEncoderMeters()
|
||||
),
|
||||
pose
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
@Logged(name = "Pose", importance = Importance.INFO)
|
||||
public Pose2d getPose() {
|
||||
return poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isSlowMode() {
|
||||
return slowMode;
|
||||
}
|
||||
|
||||
@Override
|
||||
public boolean isFieldOriented() {
|
||||
return fieldOriented;
|
||||
}
|
||||
|
||||
@Override
|
||||
public ChassisSpeeds getChassisSpeeds() {
|
||||
return MecanumDriveConstants.kKinematics.toChassisSpeeds(
|
||||
new MecanumDriveWheelSpeeds(
|
||||
getFrontLeftEncoderMetersPerSecond(),
|
||||
getFrontRightEncoderMetersPerSecond(),
|
||||
getRearLeftEncoderMetersPerSecond(),
|
||||
getFrontRightEncoderMetersPerSecond()
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command toggleSlowMode() {
|
||||
return runOnce(() -> {
|
||||
slowMode = !slowMode;
|
||||
});
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command toggleFieldOriented() {
|
||||
return runOnce(() -> {
|
||||
fieldOriented = !fieldOriented;
|
||||
});
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command drive(CommandXboxController controller) {
|
||||
return run(() -> {
|
||||
double xSpeedDelivered =
|
||||
controller.getLeftY()
|
||||
* (slowMode ? MecanumDriveConstants.kSlowModeMultiplier : 1)
|
||||
* (MecanumDriveConstants.kInvertForwardBackward ? -1 : 1)
|
||||
* MecanumDriveConstants.kMaxSpeedMetersPerSecond;
|
||||
|
||||
double ySpeedDelivered =
|
||||
controller.getLeftX()
|
||||
* (slowMode ? MecanumDriveConstants.kSlowModeMultiplier : 1)
|
||||
* (MecanumDriveConstants.kInvertLeftRight ? -1 : 1)
|
||||
* MecanumDriveConstants.kMaxSpeedMetersPerSecond;
|
||||
|
||||
double rotDelivered =
|
||||
controller.getRightX()
|
||||
* (slowMode ? MecanumDriveConstants.kSlowModeMultiplier : 1)
|
||||
* (MecanumDriveConstants.kInvertRotation ? -1 : 1)
|
||||
* MecanumDriveConstants.kMaxAngularSpeedRadiansPerSecond;
|
||||
|
||||
ChassisSpeeds speeds = fieldOriented ?
|
||||
ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered, Rotation2d.fromDegrees(getGyroAngle())) :
|
||||
new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered);
|
||||
|
||||
drive.driveCartesian(
|
||||
speeds.vxMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond,
|
||||
speeds.vyMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond,
|
||||
speeds.omegaRadiansPerSecond / MecanumDriveConstants.kMaxAngularSpeedRadiansPerSecond
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command drive(ChassisSpeeds speeds) {
|
||||
return run(() -> {
|
||||
drive.driveCartesian(
|
||||
speeds.vxMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond,
|
||||
speeds.vyMetersPerSecond / MecanumDriveConstants.kMaxSpeedMetersPerSecond,
|
||||
speeds.omegaRadiansPerSecond / MecanumDriveConstants.kMaxAngularSpeedRadiansPerSecond
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command drive(Pose2d targetPose) {
|
||||
return new PrintCommand("Mecanum Drivetrain Drive to Pose Not Implemented");
|
||||
}
|
||||
|
||||
@Override
|
||||
public Command stop() {
|
||||
return run(() -> {
|
||||
drive.driveCartesian(0, 0, 0);
|
||||
});
|
||||
}
|
||||
|
||||
@Logged(name = "Gyro Angle Degrees", importance = Importance.DEBUG)
|
||||
public double getGyroAngle() {
|
||||
return gyro.getAngle() * (MecanumDriveConstants.kInvertGyro ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Front Left Encoder Meters", importance = Importance.DEBUG)
|
||||
public double getFrontLeftEncoderMeters() {
|
||||
return frontLeftEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Rear Left Encoder Meters", importance = Importance.DEBUG)
|
||||
public double getRearLeftEncoderMeters() {
|
||||
return rearLeftEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Front Right Encoder Meters", importance = Importance.DEBUG)
|
||||
public double getFrontRightEncoderMeters() {
|
||||
return frontRightEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Rear Right Encoder Meters", importance = Importance.DEBUG)
|
||||
public double getRearRightEncoderMeters() {
|
||||
return rearRightEncoder.getDistance() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Front Left Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
public double getFrontLeftEncoderMetersPerSecond() {
|
||||
return frontLeftEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Rear Left Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
public double getRearLeftEncoderMetersPerSecond() {
|
||||
return rearLeftEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1: 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Front Right Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
public double getFrontRightEncoderMetersPerSecond() {
|
||||
return frontRightEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
|
||||
@Logged(name = "Rear Right Encoder Meters Per Second", importance = Importance.DEBUG)
|
||||
public double getRearRightEncoderMetersPerSecond() {
|
||||
return rearRightEncoder.getRate() * (MecanumDriveConstants.kInvertEncoders ? -1 : 1);
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user