Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code
This commit is contained in:
commit
adb17f9cea
@ -8,7 +8,7 @@ import frc.robot.constants.OIConstants;
|
||||
import frc.robot.subsystems.Arm;
|
||||
import frc.robot.subsystems.ClimberPivot;
|
||||
import frc.robot.subsystems.ClimberRollers;
|
||||
import frc.robot.subsystems.DriveSubsystem;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
import frc.robot.subsystems.Elevator;
|
||||
import frc.robot.subsystems.Indexer;
|
||||
import frc.robot.subsystems.Manipulator;
|
||||
@ -23,7 +23,7 @@ public class RobotContainer {
|
||||
|
||||
private ClimberRollers climberRollers;
|
||||
|
||||
private DriveSubsystem drivetrain;
|
||||
private Drivetrain drivetrain;
|
||||
|
||||
private Elevator elevator;
|
||||
|
||||
@ -41,7 +41,7 @@ public class RobotContainer {
|
||||
|
||||
climberRollers = new ClimberRollers();
|
||||
|
||||
drivetrain = new DriveSubsystem();
|
||||
drivetrain = new Drivetrain();
|
||||
|
||||
elevator = new Elevator();
|
||||
|
||||
|
||||
@ -5,6 +5,9 @@ public class ArmConstants {
|
||||
public static final int kCANcoderID = 0;
|
||||
|
||||
public static final double kEncoderConversionFactor = 0;
|
||||
<<<<<<< HEAD
|
||||
|
||||
public static final double kArmMaxVelocity = 0;
|
||||
=======
|
||||
>>>>>>> f4908e4051cd2c1ce41df73debe5e7e2cc506c4a
|
||||
}
|
||||
|
||||
@ -1,10 +1,16 @@
|
||||
package frc.robot.constants;
|
||||
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
import static edu.wpi.first.units.Units.Second;
|
||||
import static edu.wpi.first.units.Units.Seconds;
|
||||
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Config;
|
||||
|
||||
public class DriveConstants {
|
||||
public class DrivetrainConstants {
|
||||
// Driving Parameters - Note that these are not the maximum capable speeds of
|
||||
// the robot, rather the allowed maximum speeds
|
||||
public static final double kMaxSpeedMetersPerSecond = 4.8;
|
||||
@ -14,12 +20,6 @@ public class DriveConstants {
|
||||
public static final double kTrackWidth = Units.inchesToMeters(26.5);
|
||||
// Distance between centers of right and left wheels on robot
|
||||
public static final double kWheelBase = Units.inchesToMeters(26.5);
|
||||
// Distance between front and back wheels on robot
|
||||
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
|
||||
|
||||
// Angular offsets of the modules relative to the chassis in radians
|
||||
public static final double kFrontLeftChassisAngularOffset = -Math.PI / 2;
|
||||
@ -27,6 +27,16 @@ public class DriveConstants {
|
||||
public static final double kBackLeftChassisAngularOffset = Math.PI;
|
||||
public static final double kBackRightChassisAngularOffset = Math.PI / 2;
|
||||
|
||||
// 1, 7, 10 is the default for these three values
|
||||
public static final double kSysIDDrivingRampRate = 1;
|
||||
public static final double kSysIDDrivingStepVolts = 7;
|
||||
public static final double kSysIDDrivingTimeout = 10;
|
||||
|
||||
// 1, 7, 10 is the default for these three values
|
||||
public static final double kSysIDTurningRampRate = 1;
|
||||
public static final double kSysIDTurningStepVolts = 7;
|
||||
public static final double kSysIDTurningTimeout = 10;
|
||||
|
||||
// SPARK MAX CAN IDs
|
||||
public static final int kFrontLeftDrivingCanId = 11;
|
||||
public static final int kRearLeftDrivingCanId = 13;
|
||||
@ -39,4 +49,25 @@ public class DriveConstants {
|
||||
public static final int kRearRightTurningCanId = 16;
|
||||
|
||||
public static final boolean kGyroReversed = false;
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
// Distance between front and back wheels on robot
|
||||
public static final SwerveDriveKinematics kDriveKinematics = new SwerveDriveKinematics(
|
||||
new Translation2d(kWheelBase / 2, kTrackWidth / 2),
|
||||
new Translation2d(kWheelBase / 2, -kTrackWidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, kTrackWidth / 2),
|
||||
new Translation2d(-kWheelBase / 2, -kTrackWidth / 2));
|
||||
|
||||
public static final SysIdRoutine.Config kSysIDDrivingConfig = new Config(
|
||||
Volts.of(kSysIDDrivingRampRate).per(Second),
|
||||
Volts.of(kSysIDDrivingStepVolts),
|
||||
Seconds.of(kSysIDDrivingTimeout)
|
||||
);
|
||||
|
||||
public static final SysIdRoutine.Config kSysIDTurningConfig = new Config(
|
||||
Volts.of(kSysIDTurningRampRate).per(Second),
|
||||
Volts.of(kSysIDTurningStepVolts),
|
||||
Seconds.of(kSysIDTurningTimeout)
|
||||
);
|
||||
}
|
||||
@ -33,15 +33,12 @@ public class ElevatorConstants {
|
||||
public static final double kFeedForwardG = 0;
|
||||
public static final double kFeedForwardV = 0;
|
||||
|
||||
public static final double kElevatorMaxVelocity = 0;
|
||||
|
||||
// 1, 7, 10 are the defaults for these, change as necessary
|
||||
public static final double kSysIDRampRate = 1;
|
||||
public static final double kSysIDStepVolts = 7;
|
||||
public static final double kSysIDTimeout = 10;
|
||||
|
||||
// This should be kCoast for SysID, any other time it can be kBrake if you want
|
||||
public static final IdleMode kIdleMode = IdleMode.kCoast;
|
||||
public static final IdleMode kIdleMode = IdleMode.kBrake;
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
|
||||
@ -20,6 +20,11 @@ public class ModuleConstants {
|
||||
public static final double kDriveWheelFreeSpeedRps = (kDrivingMotorFreeSpeedRps * kWheelCircumferenceMeters)
|
||||
/ kDrivingMotorReduction;
|
||||
|
||||
public static final int kDriveMotorCurrentLimit = 40;
|
||||
public static final int kTurnMotorCurrentLimit = 20;
|
||||
|
||||
// YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A NEW CONFIGURATION ITEM
|
||||
|
||||
public static final SparkMaxConfig drivingConfig = new SparkMaxConfig();
|
||||
public static final SparkMaxConfig turningConfig = new SparkMaxConfig();
|
||||
|
||||
@ -31,7 +36,7 @@ public class ModuleConstants {
|
||||
|
||||
drivingConfig
|
||||
.idleMode(IdleMode.kBrake)
|
||||
.smartCurrentLimit(50);
|
||||
.smartCurrentLimit(kDriveMotorCurrentLimit);
|
||||
drivingConfig.encoder
|
||||
.positionConversionFactor(drivingFactor) // meters
|
||||
.velocityConversionFactor(drivingFactor / 60.0); // meters per second
|
||||
|
||||
@ -19,15 +19,15 @@ import edu.wpi.first.wpilibj.ADIS16470_IMU;
|
||||
import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc.robot.constants.DriveConstants;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.constants.OIConstants;
|
||||
|
||||
public class DriveSubsystem extends SubsystemBase {
|
||||
public class Drivetrain extends SubsystemBase {
|
||||
// Create MAXSwerveModules
|
||||
private MAXSwerveModule m_frontLeft;
|
||||
private MAXSwerveModule m_frontRight;
|
||||
private MAXSwerveModule m_rearLeft;
|
||||
private MAXSwerveModule m_rearRight;
|
||||
protected MAXSwerveModule m_frontLeft;
|
||||
protected MAXSwerveModule m_frontRight;
|
||||
protected MAXSwerveModule m_rearLeft;
|
||||
protected MAXSwerveModule m_rearRight;
|
||||
|
||||
// The gyro sensor
|
||||
private ADIS16470_IMU m_gyro;
|
||||
@ -36,35 +36,35 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
private SwerveDriveOdometry m_odometry;
|
||||
|
||||
/** Creates a new DriveSubsystem. */
|
||||
public DriveSubsystem() {
|
||||
public Drivetrain() {
|
||||
m_frontLeft = new MAXSwerveModule(
|
||||
DriveConstants.kFrontLeftDrivingCanId,
|
||||
DriveConstants.kFrontLeftTurningCanId,
|
||||
DriveConstants.kFrontLeftChassisAngularOffset
|
||||
DrivetrainConstants.kFrontLeftDrivingCanId,
|
||||
DrivetrainConstants.kFrontLeftTurningCanId,
|
||||
DrivetrainConstants.kFrontLeftChassisAngularOffset
|
||||
);
|
||||
|
||||
m_frontRight = new MAXSwerveModule(
|
||||
DriveConstants.kFrontRightDrivingCanId,
|
||||
DriveConstants.kFrontRightTurningCanId,
|
||||
DriveConstants.kFrontRightChassisAngularOffset
|
||||
DrivetrainConstants.kFrontRightDrivingCanId,
|
||||
DrivetrainConstants.kFrontRightTurningCanId,
|
||||
DrivetrainConstants.kFrontRightChassisAngularOffset
|
||||
);
|
||||
|
||||
m_rearLeft = new MAXSwerveModule(
|
||||
DriveConstants.kRearLeftDrivingCanId,
|
||||
DriveConstants.kRearLeftTurningCanId,
|
||||
DriveConstants.kBackLeftChassisAngularOffset
|
||||
DrivetrainConstants.kRearLeftDrivingCanId,
|
||||
DrivetrainConstants.kRearLeftTurningCanId,
|
||||
DrivetrainConstants.kBackLeftChassisAngularOffset
|
||||
);
|
||||
|
||||
m_rearRight = new MAXSwerveModule(
|
||||
DriveConstants.kRearRightDrivingCanId,
|
||||
DriveConstants.kRearRightTurningCanId,
|
||||
DriveConstants.kBackRightChassisAngularOffset
|
||||
DrivetrainConstants.kRearRightDrivingCanId,
|
||||
DrivetrainConstants.kRearRightTurningCanId,
|
||||
DrivetrainConstants.kBackRightChassisAngularOffset
|
||||
);
|
||||
|
||||
m_gyro = new ADIS16470_IMU();
|
||||
|
||||
m_odometry = new SwerveDriveOdometry(
|
||||
DriveConstants.kDriveKinematics,
|
||||
DrivetrainConstants.kDriveKinematics,
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)),
|
||||
new SwerveModulePosition[] {
|
||||
m_frontLeft.getPosition(),
|
||||
@ -137,17 +137,17 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
*/
|
||||
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
|
||||
// Convert the commanded speeds into the correct units for the drivetrain
|
||||
double xSpeedDelivered = xSpeed * DriveConstants.kMaxSpeedMetersPerSecond;
|
||||
double ySpeedDelivered = ySpeed * DriveConstants.kMaxSpeedMetersPerSecond;
|
||||
double rotDelivered = rot * DriveConstants.kMaxAngularSpeed;
|
||||
double xSpeedDelivered = xSpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
|
||||
double ySpeedDelivered = ySpeed * DrivetrainConstants.kMaxSpeedMetersPerSecond;
|
||||
double rotDelivered = rot * DrivetrainConstants.kMaxAngularSpeed;
|
||||
|
||||
var swerveModuleStates = DriveConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
var swerveModuleStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered,
|
||||
Rotation2d.fromDegrees(m_gyro.getAngle(IMUAxis.kZ)))
|
||||
: new ChassisSpeeds(xSpeedDelivered, ySpeedDelivered, rotDelivered));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
swerveModuleStates, DriveConstants.kMaxSpeedMetersPerSecond);
|
||||
swerveModuleStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(swerveModuleStates[0]);
|
||||
m_frontRight.setDesiredState(swerveModuleStates[1]);
|
||||
m_rearLeft.setDesiredState(swerveModuleStates[2]);
|
||||
@ -177,7 +177,7 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
*/
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(
|
||||
desiredStates, DriveConstants.kMaxSpeedMetersPerSecond);
|
||||
desiredStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
|
||||
m_frontLeft.setDesiredState(desiredStates[0]);
|
||||
m_frontRight.setDesiredState(desiredStates[1]);
|
||||
m_rearLeft.setDesiredState(desiredStates[2]);
|
||||
@ -212,6 +212,6 @@ public class DriveSubsystem extends SubsystemBase {
|
||||
* @return The turn rate of the robot, in degrees per second
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
return m_gyro.getRate(IMUAxis.kZ) * (DriveConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
return m_gyro.getRate(IMUAxis.kZ) * (DrivetrainConstants.kGyroReversed ? -1.0 : 1.0);
|
||||
}
|
||||
}
|
||||
@ -6,6 +6,8 @@ import com.revrobotics.RelativeEncoder;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkBase.PersistMode;
|
||||
import com.revrobotics.spark.SparkBase.ResetMode;
|
||||
import com.revrobotics.spark.SparkLowLevel.MotorType;
|
||||
|
||||
import edu.wpi.first.math.controller.ElevatorFeedforward;
|
||||
|
||||
@ -7,6 +7,7 @@ package frc.robot.subsystems;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModulePosition;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.wpilibj.RobotController;
|
||||
|
||||
import com.revrobotics.spark.SparkClosedLoopController;
|
||||
import com.revrobotics.spark.SparkMax;
|
||||
@ -107,6 +108,22 @@ public class MAXSwerveModule {
|
||||
m_desiredState = desiredState;
|
||||
}
|
||||
|
||||
public void setVoltageDrive(double voltage){
|
||||
m_drivingSpark.setVoltage(voltage);
|
||||
}
|
||||
|
||||
public void setVoltageTurn(double voltage) {
|
||||
m_turningSpark.setVoltage(voltage);
|
||||
}
|
||||
|
||||
public double getVoltageDrive() {
|
||||
return m_drivingSpark.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
|
||||
public double getVoltageTurn() {
|
||||
return m_turningSpark.get() * RobotController.getBatteryVoltage();
|
||||
}
|
||||
|
||||
/** Zeroes all the SwerveModule encoders. */
|
||||
public void resetEncoders() {
|
||||
m_drivingEncoder.setPosition(0);
|
||||
|
||||
151
src/main/java/frc/robot/subsystems/sysid/DrivetrainSysID.java
Normal file
151
src/main/java/frc/robot/subsystems/sysid/DrivetrainSysID.java
Normal file
@ -0,0 +1,151 @@
|
||||
package frc.robot.subsystems.sysid;
|
||||
|
||||
import static edu.wpi.first.units.Units.Volts;
|
||||
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
|
||||
import static edu.wpi.first.units.Units.Meters;
|
||||
import static edu.wpi.first.units.Units.Radians;
|
||||
import static edu.wpi.first.units.Units.MetersPerSecond;
|
||||
|
||||
import edu.wpi.first.units.measure.MutAngle;
|
||||
import edu.wpi.first.units.measure.MutDistance;
|
||||
import edu.wpi.first.units.measure.MutLinearVelocity;
|
||||
import edu.wpi.first.units.measure.MutVoltage;
|
||||
import edu.wpi.first.units.measure.Voltage;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
|
||||
import frc.robot.constants.DrivetrainConstants;
|
||||
import frc.robot.subsystems.Drivetrain;
|
||||
|
||||
public class DrivetrainSysID extends Drivetrain {
|
||||
public enum RoutineType {
|
||||
kDriving,
|
||||
kTurning
|
||||
}
|
||||
|
||||
private MutVoltage drivingAppliedVoltage;
|
||||
private MutVoltage turningAppliedVoltage;
|
||||
|
||||
private MutDistance drivingPosition;
|
||||
|
||||
private MutAngle turningPosition;
|
||||
|
||||
private MutLinearVelocity drivingVelocity;
|
||||
|
||||
private SysIdRoutine drivingRoutine;
|
||||
private SysIdRoutine turningRoutine;
|
||||
|
||||
public DrivetrainSysID() {
|
||||
super();
|
||||
|
||||
drivingAppliedVoltage = Volts.mutable(0);
|
||||
turningAppliedVoltage = Volts.mutable(0);
|
||||
|
||||
drivingPosition = Meters.mutable(0);
|
||||
|
||||
turningPosition = Radians.mutable(0);
|
||||
|
||||
drivingVelocity = MetersPerSecond.mutable(0);
|
||||
|
||||
// TODO Does there need to be a mutable for each metric for each motor, or are reusing the three OK
|
||||
drivingRoutine = new SysIdRoutine(
|
||||
DrivetrainConstants.kSysIDDrivingConfig,
|
||||
new SysIdRoutine.Mechanism(
|
||||
this::setDriveVoltageAll,
|
||||
(log) -> {
|
||||
log.motor("frontLeftDrive")
|
||||
.voltage(drivingAppliedVoltage.mut_replace(m_frontLeft.getVoltageDrive(), Volts))
|
||||
.linearPosition(drivingPosition.mut_replace(m_frontLeft.getPosition().distanceMeters, Meters))
|
||||
.linearVelocity(drivingVelocity.mut_replace(m_frontLeft.getState().speedMetersPerSecond, MetersPerSecond));
|
||||
log.motor("rearLeftDrive")
|
||||
.voltage(drivingAppliedVoltage.mut_replace(m_rearLeft.getVoltageDrive(), Volts))
|
||||
.linearPosition(drivingPosition.mut_replace(m_rearLeft.getPosition().distanceMeters, Meters))
|
||||
.linearVelocity(drivingVelocity.mut_replace(m_rearLeft.getState().speedMetersPerSecond, MetersPerSecond));
|
||||
log.motor("frontRightDrive")
|
||||
.voltage(drivingAppliedVoltage.mut_replace(m_frontRight.getVoltageDrive(), Volts))
|
||||
.linearPosition(drivingPosition.mut_replace(m_frontRight.getPosition().distanceMeters, Meters))
|
||||
.linearVelocity(drivingVelocity.mut_replace(m_frontRight.getState().speedMetersPerSecond, MetersPerSecond));
|
||||
log.motor("rearRightDrive")
|
||||
.voltage(drivingAppliedVoltage.mut_replace(m_rearRight.getVoltageDrive(), Volts))
|
||||
.linearPosition(drivingPosition.mut_replace(m_rearRight.getPosition().distanceMeters, Meters))
|
||||
.linearVelocity(drivingVelocity.mut_replace(m_rearRight.getState().speedMetersPerSecond, MetersPerSecond));
|
||||
},
|
||||
this
|
||||
)
|
||||
);
|
||||
|
||||
// TODO Does there need to be a mutable for each metric for each motor, or are reusing the two OK
|
||||
// TODO Angular velocity is not tracked because it's not easy to get by default, is this OK
|
||||
turningRoutine = new SysIdRoutine(
|
||||
DrivetrainConstants.kSysIDTurningConfig,
|
||||
new SysIdRoutine.Mechanism(
|
||||
this::setTurnVoltageAll,
|
||||
(log) -> {
|
||||
log.motor("frontLeftTurn")
|
||||
.voltage(turningAppliedVoltage.mut_replace(m_frontLeft.getVoltageTurn(), Volts))
|
||||
.angularPosition(turningPosition.mut_replace(m_frontLeft.getState().angle.getRadians(), Radians));
|
||||
log.motor("rearLeftTurn")
|
||||
.voltage(turningAppliedVoltage.mut_replace(m_rearLeft.getVoltageTurn(), Volts))
|
||||
.angularPosition(turningPosition.mut_replace(m_rearLeft.getState().angle.getRadians(), Radians));
|
||||
log.motor("frontRightTurn")
|
||||
.voltage(turningAppliedVoltage.mut_replace(m_frontRight.getVoltageTurn(), Volts))
|
||||
.angularPosition(turningPosition.mut_replace(m_frontRight.getState().angle.getRadians(), Radians));
|
||||
log.motor("rearRightTurn")
|
||||
.voltage(turningAppliedVoltage.mut_replace(m_rearRight.getVoltageTurn(), Volts))
|
||||
.angularPosition(turningPosition.mut_replace(m_rearRight.getState().angle.getRadians(), Radians));
|
||||
},
|
||||
this
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
public Command sysIdQuasistatic(SysIdRoutine.Direction direction, RoutineType type) {
|
||||
switch(type) {
|
||||
case kDriving:
|
||||
return makeDriveStraight().andThen(drivingRoutine.quasistatic(direction));
|
||||
case kTurning:
|
||||
default:
|
||||
return turningRoutine.quasistatic(direction);
|
||||
}
|
||||
}
|
||||
|
||||
public Command sysIdDynamic(SysIdRoutine.Direction direction, RoutineType type) {
|
||||
switch(type) {
|
||||
case kDriving:
|
||||
return makeDriveStraight().andThen(drivingRoutine.dynamic(direction));
|
||||
case kTurning:
|
||||
default:
|
||||
return turningRoutine.dynamic(direction);
|
||||
}
|
||||
}
|
||||
|
||||
public void setDriveVoltageAll(Voltage voltage) {
|
||||
m_frontLeft.setVoltageDrive(voltage.magnitude());
|
||||
m_rearLeft.setVoltageDrive(voltage.magnitude());
|
||||
m_frontRight.setVoltageDrive(voltage.magnitude());
|
||||
m_rearRight.setVoltageDrive(voltage.magnitude());
|
||||
}
|
||||
|
||||
public void setTurnVoltageAll(Voltage voltage) {
|
||||
m_frontLeft.setVoltageTurn(voltage.magnitude());
|
||||
m_rearLeft.setVoltageTurn(voltage.magnitude());
|
||||
m_frontRight.setVoltageTurn(voltage.magnitude());
|
||||
m_rearRight.setVoltageTurn(voltage.magnitude());
|
||||
}
|
||||
|
||||
// TODO Does this nonsense actually work
|
||||
private Command makeDriveStraight() {
|
||||
return runOnce(() -> {
|
||||
SwerveModuleState state = new SwerveModuleState(0, Rotation2d.fromDegrees(0));
|
||||
|
||||
this.setModuleStates(new SwerveModuleState[] {
|
||||
state,
|
||||
state,
|
||||
state,
|
||||
state
|
||||
});
|
||||
});
|
||||
}
|
||||
}
|
||||
@ -59,4 +59,4 @@ public class ElevatorSysID extends Elevator {
|
||||
public Command sysIdDynamic(SysIdRoutine.Direction direction) {
|
||||
return routine.dynamic(direction);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user