diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 178ebb9..dee8bd9 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -19,7 +19,9 @@ import frc.robot.SubsystemChecker.SubsystemType; import frc.robot.commands.BrakeModeDisabled; import frc.robot.commands.CheckArmSetpoints; +import frc.robot.commands.SetBlingCommand; import frc.robot.commands.SyncArmEncoders; +import frc.robot.commands.SyncArmEncodersV2; import frc.robot.commands.SyncSteerEncoders; import frc.robot.config.Config; import frc.robot.robotcontainers.ArmBotContainer; @@ -30,6 +32,7 @@ import frc.robot.robotcontainers.MiniNeoDiffContainer; import frc.robot.robotcontainers.MiniSwerveContainer; import frc.robot.robotcontainers.RobotContainer; +import frc.robot.subsystems.BlingSubsystem; import frc.robot.subsystems.DiffNeoSubsystem; import frc.robot.subsystems.DiffTalonSubsystem; @@ -46,6 +49,8 @@ public class Robot extends TimedRobot { Compressor pcmCompressor = new Compressor(1, PneumaticsModuleType.CTREPCM); + private boolean m_firstDisableAtBootup = true; + @@ -97,8 +102,9 @@ public void robotInit() { } if (SubsystemChecker.canSubsystemConstruct(SubsystemType.SwerveSubsystem)) { + BlingSubsystem.getINSTANCE().setRed(); new SyncSteerEncoders().schedule(); - new WaitCommand(3).andThen(new SyncArmEncoders()).schedule(); + // new SyncArmEncodersV2().schedule(); // With new encoder wiring, no longer need to sync arms } // Add CommandScheduler to shuffleboard so we can display what commands are scheduled @@ -131,7 +137,15 @@ public void robotPeriodic() { @Override public void disabledInit() { if (brakeModeDisabledCommand != null) - brakeModeDisabledCommand.schedule(); + brakeModeDisabledCommand.schedule(); + + if (SubsystemChecker.canSubsystemConstruct(SubsystemType.BlingSubsystem)) { + if (m_firstDisableAtBootup) { + m_firstDisableAtBootup = false; + } else { + BlingSubsystem.getINSTANCE().setDisabled(); + } + } } @Override diff --git a/src/main/java/frc/robot/SubsystemChecker.java b/src/main/java/frc/robot/SubsystemChecker.java index 408719b..1cc96d1 100644 --- a/src/main/java/frc/robot/SubsystemChecker.java +++ b/src/main/java/frc/robot/SubsystemChecker.java @@ -34,11 +34,12 @@ public enum SubsystemType { */ // RobotID: 0, 2023 Competition robot, unnamed private static SubsystemType[] compBotId0 = new SubsystemType[] { - SubsystemType.SwerveSubsystem, // Chassis unknown + SubsystemType.SwerveSubsystem, SubsystemType.ArmSubsystem, SubsystemType.RelaySubsystem, SubsystemType.VisionNTSubsystem, SubsystemType.GripperSubsystem, + SubsystemType.BlingSubsystem, }; // RobotID: 1, 2022 robot, RapidReact, Clutch diff --git a/src/main/java/frc/robot/commands/SyncArmEncodersV2.java b/src/main/java/frc/robot/commands/SyncArmEncodersV2.java new file mode 100644 index 0000000..3df9a70 --- /dev/null +++ b/src/main/java/frc/robot/commands/SyncArmEncodersV2.java @@ -0,0 +1,150 @@ +// 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; + +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.CommandBase; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import frc.robot.config.ArmConfig; +import frc.robot.subsystems.ArmSubsystem; +import frc.robot.subsystems.BlingSubsystem; + +public class SyncArmEncodersV2 extends CommandBase { + private Timer m_smallTimer = new Timer(); + private Timer m_permanantTimer = new Timer(); + private int m_commandState = 0; + private int m_numSamples = 0; + private double m_sumBotSamples = 0; + private double m_sumTopSamples = 0; + + /** Creates a new SyncSteerEncoders. */ + public SyncArmEncodersV2() { + addRequirements(ArmSubsystem.getInstance()); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_smallTimer.restart(); + m_permanantTimer.restart(); + m_numSamples = 0; + m_sumBotSamples = 0; + m_sumTopSamples = 0; + m_commandState = 0; + + BlingSubsystem.getINSTANCE().noEncodersSynced(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (m_smallTimer.get() > ArmConfig.ENCODER_SYNCING_PERIOD_V2) { + m_smallTimer.reset(); + + if (m_commandState == 0 && m_permanantTimer.hasElapsed(1)) { + DriverStation.reportWarning( + String.format("Arm encoders are not synced, collecting arm encoder samples... (%.1fs)", m_permanantTimer.get()), + false); + + m_commandState = 1; + + } else if (m_commandState == 1) { + m_sumBotSamples += ArmSubsystem.getInstance().getAbsoluteBottom(); + m_sumTopSamples += ArmSubsystem.getInstance().getAbsoluteTop(); + m_numSamples++; + + if (m_numSamples >= ArmConfig.NUM_SYNCING_SAMPLES) { + m_commandState = 2; + } + } else if (m_commandState == 2) { + ArmSubsystem.getInstance().resetEncoder( + m_sumBotSamples / m_numSamples, + m_sumTopSamples / m_numSamples + ); + + if (ArmSubsystem.getInstance().areEncodersSynced() == false) { + DriverStation.reportWarning( + String.format("Arm encoders are not synced, attempting to sync them... (%.1fs)", m_permanantTimer.get()), + false); + + } else { + m_commandState = 99; // End the command, the encoders are synced + } + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (m_commandState == 99) { + DriverStation.reportWarning( + String.format("Arm encoders are synced (%.1f) \n", m_permanantTimer.get()), + false); + + BlingSubsystem.getINSTANCE().armEncodersSynced(); + + new WaitCommand(1).andThen( + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().armEncodersSynced()), + new WaitCommand(1), + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().armEncodersSynced()) + ).schedule(); + + ArmSubsystem.getInstance().sparkMaxBurnFlash(); + } + + m_permanantTimer.stop(); + m_smallTimer.stop(); + + if ( + /** Temporary checks for current encoder setup. TODO: Remove once encoder wrapping is solved.*/ + ArmSubsystem.getInstance().getBottomPosition() < Math.toRadians(5) || + ArmSubsystem.getInstance().getBottomPosition() > Math.toRadians(170) || + ArmSubsystem.getInstance().getTopPosition() < Math.toRadians(5) || + ArmSubsystem.getInstance().getTopPosition() > Math.toRadians(70) || + ArmSubsystem.getInstance().areEncodersSynced() == false || + m_commandState != 99 + ) { + + DriverStation.reportError("Arm encoders were reset outside the proper range. " + + " Disabling the arm to prevent damage." + + " Make sure the code boots up with the arm in the reset position.", true); + + // This line below will prevent any commands from being scheduled to the ArmSubsystem. + // This line is temporary while we haven't properly setup the absolute encoders properly yet. + // If it's blocking you from using the arm, you just need to put the arm in the reset position + // and reboot the code (which will properly reset the NEO encoders the absolute encoders). + Commands.run(() -> ArmSubsystem.getInstance(), ArmSubsystem.getInstance()).withInterruptBehavior(InterruptionBehavior.kCancelIncoming).ignoringDisable(true).schedule(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + if (m_permanantTimer.get() > ArmConfig.ENCODER_SYNCING_TIMEOUT) { + DriverStation.reportError( + String.format("Arm encoders are not synced. SyncArmEncodersV2 spent %.1fs trying to sync them and has timed out", + m_permanantTimer.get()), + false); + + return true; + } + return m_commandState == 99; + } + + @Override + public boolean runsWhenDisabled() { + return true; + } + + @Override + public InterruptionBehavior getInterruptionBehavior() { + DriverStation.reportWarning("Another ArmSubsystem command was scheduled while " + + "SyncArmEncodersV2 is still running. Cancelling the incoming command.", false); + return InterruptionBehavior.kCancelIncoming; + } +} diff --git a/src/main/java/frc/robot/commands/SyncSteerEncoders.java b/src/main/java/frc/robot/commands/SyncSteerEncoders.java index 295f471..1a457e7 100644 --- a/src/main/java/frc/robot/commands/SyncSteerEncoders.java +++ b/src/main/java/frc/robot/commands/SyncSteerEncoders.java @@ -43,7 +43,7 @@ public void initialize() { @Override public void execute() { SmartDashboard.putNumber("SyncSteerState", state); - if (state == 0 && m_permanantTimer.hasElapsed(6)) { + if (state == 0 && m_permanantTimer.hasElapsed(1)) { state = 1; DriverStation.reportWarning( String.format("Starting to sync steer encoders (%.1fs)", m_permanantTimer.get()), false); @@ -57,14 +57,16 @@ public void execute() { false); SwerveSubsystem.getInstance().resetEncodersFromCanCoder(); } else { - state = 2; + state = 99; m_smallTimer.restart(); } } - if (state == 2 && m_smallTimer.hasElapsed(5)) { - SwerveSubsystem.getInstance().resetEncodersFromCanCoder(); - state = 99; - } + // if (state == 2 && m_smallTimer.hasElapsed(1)) { + // if (SwerveSubsystem.getInstance().checkSteeringEncoders() == false) { + // SwerveSubsystem.getInstance().resetEncodersFromCanCoder(); + // } + // state = 99; + // } SmartDashboard.putNumber("SyncSteerState", state); } @@ -77,7 +79,11 @@ public void end(boolean interrupted) { false); BlingSubsystem.getINSTANCE().steerEncodersSynced(); - new WaitCommand(1).andThen(Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced())).schedule(); + new WaitCommand(1).andThen( + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced()), + new WaitCommand(1), + Commands.runOnce(() -> BlingSubsystem.getINSTANCE().steerEncodersSynced()) + ).schedule(); } m_permanantTimer.stop(); diff --git a/src/main/java/frc/robot/config/ArmConfig.java b/src/main/java/frc/robot/config/ArmConfig.java index f13c982..7034beb 100644 --- a/src/main/java/frc/robot/config/ArmConfig.java +++ b/src/main/java/frc/robot/config/ArmConfig.java @@ -94,8 +94,8 @@ public enum ArmPosition { GAME_PIECE_BOTTOM } - public static final double TOP_NEO_GEAR_RATIO = Config.robotSpecific(23.5, 0.0, 0.0, 0.0, 0.0, 60.0, 60.0); //comp --> 23.5 - public static final double BOTTOM_NEO_GEAR_RATIO = 62.5; + public static final double TOP_NEO_GEAR_RATIO = Config.robotSpecific(1.0, 0.0, 0.0, 0.0, 0.0, 60.0, 60.0); // NEO Encoder gear ratio is 23.5; + public static final double BOTTOM_NEO_GEAR_RATIO = 1.0; // NEO Encoder gear ratio is 62.5; public static final double L1 = 27.75; //length of arm 1 in inches public static final double L2 = 38.6; //length of arm 2 in inches public static final double LENGTH_BOTTOM_ARM_TO_COG = 14.56; @@ -198,12 +198,16 @@ public enum ArmPosition { public static final int top_duty_cycle_channel = 9; public static final int bottom_duty_cycle_channel = 7; - // arm offsets - public static final double top_arm_offset = -282.000000; - public static final double bottom_arm_offset = 307.800000; + // arm offsets - Must be [0, 360] if using setZeroOffset on a REV AbsoluteEncoder object + public static final double top_arm_offset = 285; // Old rio wiring: -282.000000; + public static final double bottom_arm_offset = 50.2; // Old rio wiring: 307.800000; + + public static final boolean botEncoderInverted = true; + public static final boolean topEncoderInverted = false; // Syncing encoders public static double ENCODER_SYNCING_PERIOD = 0.4; // seconds + public static double ENCODER_SYNCING_PERIOD_V2 = 0.05; // seconds public static int ENCODER_SYNCING_TIMEOUT = 20; // seconds public static double ENCODER_SYNCING_TOLERANCE = 0.01; // radians public static int NUM_SYNCING_SAMPLES = 20; // num of samples needed to average diff --git a/src/main/java/frc/robot/config/Config.java b/src/main/java/frc/robot/config/Config.java index 051b90b..55efa04 100644 --- a/src/main/java/frc/robot/config/Config.java +++ b/src/main/java/frc/robot/config/Config.java @@ -208,13 +208,16 @@ public static class Swerve{ // ~ Teleop speeds. Speeds should be portional to kMaxAttainableAngularSpeed. Angular speeds should be specified. + // REDUCING THE SPEED OF THE ROBOT SO WE DON"T DAMAGE SPONSER'S CARPET + public static final double OUTREACH_SPEED_REDUCTION = 0.4; + // Default Speeds - public static final double teleopDefaultSpeed = kMaxAttainableWheelSpeed;// Full speed - public static final double teleopDefaultAngularSpeed = Math.PI*3.0; // Old value: Math.PI*2.0 + public static final double teleopDefaultSpeed = kMaxAttainableWheelSpeed * OUTREACH_SPEED_REDUCTION;// Full speed + public static final double teleopDefaultAngularSpeed = Math.PI*3.0 * OUTREACH_SPEED_REDUCTION; // Old value: Math.PI*2.0 // Left Bumper speeds - public static final double teleopLeftBumperSpeed = kMaxAttainableWheelSpeed / 3.0;// A third of full speed - public static final double teleopLeftBumperAngularSpeed = Math.PI; // Can be a slower angular speed if wanted + public static final double teleopLeftBumperSpeed = kMaxAttainableWheelSpeed / 3.0 * OUTREACH_SPEED_REDUCTION;// A third of full speed + public static final double teleopLeftBumperAngularSpeed = Math.PI * OUTREACH_SPEED_REDUCTION; // Can be a slower angular speed if wanted // Right bumper speeds public static final double teleopRightBumperSpeed = 0.3; diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java index 3109d3f..70a8570 100644 --- a/src/main/java/frc/robot/subsystems/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration; import com.ctre.phoenix.sensors.SensorInitializationStrategy; import com.ctre.phoenix.sensors.SensorTimeBase; +import com.revrobotics.AbsoluteEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; @@ -43,14 +44,16 @@ public class ArmSubsystem extends SubsystemBase { + // for top arm + private final String m_tuningTableTop = "Arm/TopArmTuning"; + private final String m_dataTableTop = "Arm/TopArmData"; + + // for bottom arm + private final String m_tuningTableBottom = "Arm/BottomArmTuning"; + private final String m_dataTableBottom = "Arm/BottomArmData"; private ArmDisplay armDisplay; - private static final MotorType motorType = MotorType.kBrushless; - private static final SparkMaxAbsoluteEncoder.Type encAbsType = SparkMaxAbsoluteEncoder.Type.kDutyCycle; - - public CANCoder m_absoluteTopArmEncoder; - public CANCoder m_absoluteBottomArmEncoder; private static ArmSubsystem instance = null; public final CANSparkMax m_topArm; public final CANSparkMax m_bottomArm; @@ -58,19 +61,8 @@ public class ArmSubsystem extends SubsystemBase { public SparkMaxPIDController m_pidControllerBottomArm; public ProfileExternalPIDController m_topPID; public ProfileExternalPIDController m_bottomPID; - private RelativeEncoder m_bottomEncoder; - private RelativeEncoder m_topEncoder; - - private double m_topArmEncoderOffset; - private double m_bottomArmEncoderOffset; - - // for top arm - private final String m_tuningTableTop = "Arm/TopArmTuning"; - private final String m_dataTableTop = "Arm/TopArmData"; - - //for bottom arm - private final String m_tuningTableBottom = "Arm/BottomArmTuning"; - private final String m_dataTableBottom = "Arm/BottomArmData"; + private AbsoluteEncoder m_bottomAbsEncoder; + private AbsoluteEncoder m_topAbsEncoder; // network table entries for top arm private DoubleEntry m_topArmPSubs; @@ -109,10 +101,6 @@ public class ArmSubsystem extends SubsystemBase { DoubleSolenoid brakeSolenoidLow; DoubleSolenoid brakeSolenoidHigh; - // duty cycle encoder - private DutyCycleEncoder m_topDutyCycleEncoder; - private DutyCycleEncoder m_bottomDutyCycleEncoder; - public static ArmSubsystem getInstance() { if (instance == null) { SubsystemChecker.subsystemConstructed(SubsystemType.ArmSubsystem); @@ -123,8 +111,8 @@ public static ArmSubsystem getInstance() { /** Creates a new ArmSubsystem. */ public ArmSubsystem() { - m_topArm = new CANSparkMax(Config.CANID.TOP_ARM_SPARK_CAN_ID, motorType); - m_bottomArm = new CANSparkMax(Config.CANID.BOTTOM_ARM_SPARK_CAN_ID, motorType); + m_topArm = new CANSparkMax(Config.CANID.TOP_ARM_SPARK_CAN_ID, MotorType.kBrushless); + m_bottomArm = new CANSparkMax(Config.CANID.BOTTOM_ARM_SPARK_CAN_ID, MotorType.kBrushless); m_topArm.restoreFactoryDefaults(); m_bottomArm.restoreFactoryDefaults(); // m_topArm.setSmartCurrentLimit(ArmConfig.CURRENT_LIMIT); @@ -141,11 +129,6 @@ public ArmSubsystem() { m_topArm.enableSoftLimit(SoftLimitDirection.kReverse, ArmConfig.TOP_SOFT_LIMIT_ENABLE); m_bottomArm.enableSoftLimit(SoftLimitDirection.kForward, ArmConfig.BOTTOM_SOFT_LIMIT_ENABLE); m_bottomArm.enableSoftLimit(SoftLimitDirection.kReverse, ArmConfig.BOTTOM_SOFT_LIMIT_ENABLE); - - m_topDutyCycleEncoder = new DutyCycleEncoder(ArmConfig.top_duty_cycle_channel); - m_topDutyCycleEncoder.setDistancePerRotation(360); - m_bottomDutyCycleEncoder = new DutyCycleEncoder(ArmConfig.bottom_duty_cycle_channel); - m_bottomDutyCycleEncoder.setDistancePerRotation(360); CANCoderConfiguration config = new CANCoderConfiguration(); config.initializationStrategy = SensorInitializationStrategy.BootToAbsolutePosition; @@ -156,9 +139,6 @@ public ArmSubsystem() { armDisplay = new ArmDisplay(ArmConfig.L1, ArmConfig.L2); - m_absoluteTopArmEncoder = new CANCoder(Config.CANID.TOP_CANCODER_CAN_ID); - m_absoluteBottomArmEncoder = new CANCoder(Config.CANID.BOTTOM_CANCODER_CAN_ID); - brakeSolenoidLow = new DoubleSolenoid(Config.CTRE_PCM_CAN_ID, PneumaticsModuleType.CTREPCM, Config.ARMLOW_PNEUMATIC_FORWARD_CHANNEL, @@ -169,22 +149,27 @@ public ArmSubsystem() { Config.ARMHIGH_PNEUMATIC_FORWARD_CHANNEL, Config.ARMHIGH_PNEUMATIC_REVERSE_CHANNEL); - m_absoluteTopArmEncoder.configAllSettings(config); - m_absoluteBottomArmEncoder.configAllSettings(config); - m_pidControllerTopArm = m_topArm.getPIDController(); m_pidControllerBottomArm = m_bottomArm.getPIDController(); m_topPID = new ProfileExternalPIDController(new Constraints(ArmConfig.TOP_MAX_VEL, ArmConfig.TOP_MAX_ACCEL)); m_bottomPID = new ProfileExternalPIDController(new Constraints(ArmConfig.BOTTOM_MAX_VEL, ArmConfig.BOTTOM_MAX_ACCEL)); - m_bottomEncoder = m_bottomArm.getEncoder(); - m_topEncoder = m_topArm.getEncoder(); + m_bottomAbsEncoder = m_bottomArm.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); + m_topAbsEncoder = m_topArm.getAbsoluteEncoder(SparkMaxAbsoluteEncoder.Type.kDutyCycle); - m_bottomEncoder.setPositionConversionFactor(ArmConfig.bottomArmPositionConversionFactor); - m_topEncoder.setPositionConversionFactor(ArmConfig.topArmPositionConversionFactor); + m_bottomAbsEncoder.setPositionConversionFactor(ArmConfig.bottomArmPositionConversionFactor); + m_topAbsEncoder.setPositionConversionFactor(ArmConfig.topArmPositionConversionFactor); + m_bottomAbsEncoder.setVelocityConversionFactor(ArmConfig.bottomArmVelocityConversionFactor); + m_topAbsEncoder.setVelocityConversionFactor(ArmConfig.topArmVelocityConversionFactor); + + m_bottomAbsEncoder.setInverted(ArmConfig.botEncoderInverted); + m_topAbsEncoder.setInverted(ArmConfig.topEncoderInverted); + + m_bottomAbsEncoder.setZeroOffset(Math.toRadians(ArmConfig.bottom_arm_offset)); + m_topAbsEncoder.setZeroOffset(Math.toRadians(ArmConfig.top_arm_offset)); - m_topEncoder.setVelocityConversionFactor(ArmConfig.topArmVelocityConversionFactor); - m_bottomEncoder.setVelocityConversionFactor(ArmConfig.bottomArmVelocityConversionFactor); + m_pidControllerBottomArm.setFeedbackDevice(m_bottomAbsEncoder); + m_pidControllerTopArm.setFeedbackDevice(m_topAbsEncoder); NetworkTable topArmTuningTable = NetworkTableInstance.getDefault().getTable(m_tuningTableTop); m_topArmPSubs = topArmTuningTable.getDoubleTopic("P").getEntry(ArmConfig.top_arm_kP); @@ -242,8 +227,19 @@ public ArmSubsystem() { m_bottomArmFFTestingVolts = bottomArmDataTable.getDoubleTopic("VoltageSetInFFTesting").publish(); updatePIDSettings(); - updateFromAbsoluteTop(); - updateFromAbsoluteBottom(); + + // Must burn flash last + burnFlash(); + } + + public void burnFlash() { + // Sleep 0.2 seconds to make sure all configurations are completed on the can bus. + try { + Thread.sleep(200); + } catch (Exception e) {} + + m_bottomArm.burnFlash(); + m_topArm.burnFlash(); } public void updatePIDSettings() { @@ -274,17 +270,12 @@ public void updatePIDSettings() { PowerDistribution pdp = new PowerDistribution(); @Override public void periodic() { - double topPosition = m_topEncoder.getPosition(); - double bottomPosition = m_bottomEncoder.getPosition(); - - m_topArmPosPub.accept(Math.toDegrees(topPosition)); - m_topArmVelPub.accept(m_topEncoder.getVelocity()); - m_bottomArmPosPub.accept(Math.toDegrees(bottomPosition)); - m_bottomArmVelPub.accept(m_bottomEncoder.getVelocity()); - m_topAbsoluteEncoder.accept(Math.toDegrees(getAbsoluteTop())); - m_bottomAbsoluteEncoder.accept(Math.toDegrees(getAbsoluteBottom())); + m_topArmPosPub.accept(Math.toDegrees(getTopPosition())); + m_topArmVelPub.accept(getTopVel()); + m_bottomArmPosPub.accept(Math.toDegrees(getBottomPosition())); + m_bottomArmVelPub.accept(getTopVel()); - armDisplay.updateMeasurementDisplay(bottomPosition, topPosition); + armDisplay.updateMeasurementDisplay(getBottomPosition(), getTopPosition()); // pdp.getCurrent(12); @@ -298,8 +289,8 @@ public void periodic() { } public void resetMotionProfile() { - m_topPID.reset(m_topEncoder.getPosition(), m_topEncoder.getVelocity()); - m_bottomPID.reset(m_bottomEncoder.getPosition(), m_bottomEncoder.getVelocity()); + m_topPID.reset(getTopPosition(), getTopVel()); + m_bottomPID.reset(getBottomPosition(), getBottomVel()); } public double[] inverseKinematics(double L1, double L2, double x, double z) { @@ -321,7 +312,7 @@ public void setBottomJoint(double angle_bottom, double angle_top, double vel) { angle_bottom = Math.toRadians(95); } double pidSetpoint = m_bottomPID.getPIDSetpoint(new TrapezoidProfile.State(angle_bottom, vel)); - m_pidControllerBottomArm.setReference(pidSetpoint, ControlType.kPosition, 0, calculateFFBottom(m_bottomEncoder.getPosition(), m_topEncoder.getPosition(), m_hasCone)); + m_pidControllerBottomArm.setReference(pidSetpoint, ControlType.kPosition, 0, calculateFFBottom(getBottomPosition(), getTopPosition(), m_hasCone)); m_bottomArmSetpointPub.accept(Math.toDegrees(angle_bottom)); } public void setTopJoint(double angle) { @@ -375,8 +366,8 @@ public void setTopJoint(double angle, double vel) { } public void resetEncoder(double bottom_position, double top_position) { - m_topArm.getEncoder().setPosition(top_position); - m_bottomArm.getEncoder().setPosition(bottom_position); + // m_topArm.getEncoder().setPosition(top_position); + // m_bottomArm.getEncoder().setPosition(bottom_position); } private double calculateFFTop(boolean haveCone, double topSetpoint) { @@ -420,7 +411,7 @@ public void testFeedForwardTop(double additionalVoltage) { } public void testFeedForwardBottom(double additionalVoltage) { - double voltage = additionalVoltage + calculateFFBottom(m_bottomEncoder.getPosition(), m_topEncoder.getPosition(), m_hasCone); + double voltage = additionalVoltage + calculateFFBottom(getBottomPosition(), getTopPosition(), m_hasCone); m_pidControllerBottomArm.setReference(voltage, ControlType.kVoltage); m_bottomArmFFTestingVolts.accept(voltage); } @@ -428,22 +419,6 @@ public void testFeedForwardBottom(double additionalVoltage) { public void stopMotors() { m_topArm.stopMotor(); m_bottomArm.stopMotor(); -} - - public double getAbsoluteTop() { - return Math.toRadians(m_topDutyCycleEncoder.getAbsolutePosition() * 360 + m_topArmOffset.get()); - } - - public double getAbsoluteBottom() { - return Math.toRadians(m_bottomDutyCycleEncoder.getAbsolutePosition() * -360 + m_bottomArmOffset.get()); - } - - public void updateFromAbsoluteTop() { - errREV(m_topEncoder.setPosition(getAbsoluteTop())); - } - - public void updateFromAbsoluteBottom() { - errREV(m_bottomEncoder.setPosition(getAbsoluteBottom())); } public void controlBottomArmBrake( boolean bBrakeOn) { @@ -475,19 +450,19 @@ public void updateSetpointDisplay(double setpoint1, double setpoint2) { } public double getTopPosition() { - return m_topEncoder.getPosition(); + return m_topAbsEncoder.getPosition(); // + Math.toRadians(m_topArmOffset.get()); } public double getBottomPosition() { - return m_bottomEncoder.getPosition(); + return m_bottomAbsEncoder.getPosition(); // + Math.toRadians(m_bottomArmOffset.get()); } public double getTopVel() { - return m_topEncoder.getVelocity(); + return m_topAbsEncoder.getVelocity(); } public double getBottomVel() { - return m_bottomEncoder.getVelocity(); + return m_bottomAbsEncoder.getVelocity(); } public void setHasCone(boolean hasCone) { @@ -502,17 +477,6 @@ public void setHasCone(boolean hasCone) { m_hasCone = hasCone; } - /** - * Checks if the Neo encoder is synced with the Absolute Encoder - * - * @return Whether the encoders are synced or not - */ - public boolean areEncodersSynced() { - return Math.abs(getAbsoluteBottom() - getBottomPosition()) < ArmConfig.ENCODER_SYNCING_TOLERANCE && - Math.abs(getAbsoluteTop() - getTopPosition()) < ArmConfig.ENCODER_SYNCING_TOLERANCE; - - } - public void setTopConstraints(double maxVelocity, double maxAccel) { m_topPID.setConstraints(new Constraints(maxVelocity, maxAccel)); } @@ -520,4 +484,13 @@ public void setTopConstraints(double maxVelocity, double maxAccel) { public void setTopVoltage(double topVoltage) { m_topArm.set(topVoltage); } + + /* Artifacts from old wiring but kept to make old syncing files not have errors */ + public boolean areEncodersSynced() {return false;} + public void updateFromAbsoluteBottom() {} + public void updateFromAbsoluteTop() {} + public double getAbsoluteBottom() {return 0.0;} + public double getAbsoluteTop() {return 0.0;} + public void sparkMaxBurnFlash() {} + } diff --git a/src/main/java/frc/robot/subsystems/BlingSubsystem.java b/src/main/java/frc/robot/subsystems/BlingSubsystem.java index aa9b1dd..ff20ef0 100644 --- a/src/main/java/frc/robot/subsystems/BlingSubsystem.java +++ b/src/main/java/frc/robot/subsystems/BlingSubsystem.java @@ -21,7 +21,7 @@ public class BlingSubsystem extends SubsystemBase { private static BlingSubsystem INSTANCE = null; private boolean steerEncodersSynced = false; - private boolean armEncodersSynced = false; + private boolean armEncodersSynced = true; // No longer need arm encoder syncing /** * Creates a new Bling. */ diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java index 487ea98..0420433 100644 --- a/src/main/java/frc/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/SwerveModule.java @@ -67,6 +67,24 @@ public class SwerveModule { */ public SwerveModule(int driveCanID, boolean driveInverted, int turningCanID, boolean turningInverted, int encoderCanID, double encoderOffset, String ModuleName) { + CANCoderConfiguration encoderConfiguration = new CANCoderConfiguration(); + encoderConfiguration.initializationStrategy = + SensorInitializationStrategy.BootToAbsolutePosition; + encoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond; + encoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; + encoderConfiguration.magnetOffsetDegrees = 0; + encoderConfiguration.sensorDirection = direction == Direction.CLOCKWISE; + + encoder = new CANCoder(encoderCanID); + + errCTRE(encoder.configFactoryDefault()); + CANCoderUtil.setCANCoderBusUsage(encoder, CCUsage.kMinimal); + errCTRE(encoder.configAllSettings(encoderConfiguration)); + + String tableName = "SwerveChassis/SwerveModule" + ModuleName; + swerveModuleTable = NetworkTableInstance.getDefault().getTable(tableName); + sub_offset = swerveModuleTable.getDoubleTopic("offset").subscribe(encoderOffset); + // CODE: Construct both CANSparkMax objects and set all the nessecary settings (CONSTANTS from Config or from the parameters of the constructor) m_driveMotor = new CANSparkMax(driveCanID, MotorType.kBrushless); @@ -109,25 +127,12 @@ public SwerveModule(int driveCanID, boolean driveInverted, int turningCanID, boo errREV(m_turningPIDController.setIZone(Config.Swerve.sub_steering_kIZone.get())); errREV(m_turningPIDController.setFF(Config.Swerve.sub_steering_kFF.get())); + updateSteeringFromCanCoder(); + resetLastAngle(); + errREV(m_driveMotor.burnFlash()); errREV(m_turningMotor.burnFlash()); - String tableName = "SwerveChassis/SwerveModule" + ModuleName; - swerveModuleTable = NetworkTableInstance.getDefault().getTable(tableName); - - CANCoderConfiguration encoderConfiguration = new CANCoderConfiguration(); - encoderConfiguration.initializationStrategy = - SensorInitializationStrategy.BootToAbsolutePosition; - encoderConfiguration.sensorTimeBase = SensorTimeBase.PerSecond; - encoderConfiguration.absoluteSensorRange = AbsoluteSensorRange.Unsigned_0_to_360; - encoderConfiguration.magnetOffsetDegrees = 0; - encoderConfiguration.sensorDirection = direction == Direction.CLOCKWISE; - - encoder = new CANCoder(encoderCanID); - - errCTRE(encoder.configFactoryDefault()); - CANCoderUtil.setCANCoderBusUsage(encoder, CCUsage.kMinimal); - errCTRE(encoder.configAllSettings(encoderConfiguration)); desiredSpeedEntry = swerveModuleTable.getDoubleTopic("Desired speed (mps)").publish(); desiredAngleEntry = swerveModuleTable.getDoubleTopic("Desired angle (deg)").publish(); @@ -138,11 +143,10 @@ public SwerveModule(int driveCanID, boolean driveInverted, int turningCanID, boo canCoderEntry = swerveModuleTable.getDoubleTopic("CanCoder").publish(); desiredAngle360Range = swerveModuleTable.getDoubleTopic("Desired angle 360 range").publish(); currentAngle360Range = swerveModuleTable.getDoubleTopic("Current angle 360 range").publish(); - sub_offset = swerveModuleTable.getDoubleTopic("offset").subscribe(encoderOffset); - updateSteeringFromCanCoder(); - resetLastAngle(); + + } /**