From a93ffb300adb7f74c93fdd25b09d42bbddad4796 Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Tue, 9 Dec 2025 19:10:02 -0800 Subject: [PATCH 01/10] Began simplemechs --- .../SimpleMechs/Arm/ArmConfig.java | 0 .../SimpleMechs/Arm/SimpleArm.java | 17 + .../Drivetrain/SimpleDrivetrain.java | 1346 +++++++++++++++++ .../SimpleMechs/Elevator/ElevatorConfig | 8 + .../SimpleMechs/Elevator/SimpleElevator.java | 17 + .../SimpleMechs/Spinner/SimpleSpinner.java | 17 + .../SimpleMechs/Spinner/SpinnerConfig.java | 8 + 7 files changed, 1413 insertions(+) create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Arm/ArmConfig.java create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java create mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/ArmConfig.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java new file mode 100644 index 0000000..33d5d87 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java @@ -0,0 +1,17 @@ +// 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 org.carlmontrobotics.SimpleMechs.Arm; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SimpleArm extends SubsystemBase { + /** Creates a new SimpleArm. */ + public SimpleArm() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java new file mode 100644 index 0000000..320caa3 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java @@ -0,0 +1,1346 @@ +package org.carlmontrobotics.SimpleMechs.Drivetrain; +import java.util.Arrays; +import java.util.Map; +import java.util.function.Supplier; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SensorFactory; +import org.carlmontrobotics.lib199.swerve.SwerveConfig; +import org.carlmontrobotics.lib199.swerve.SwerveModule; + +import com.ctre.phoenix6.SignalLogger; +import com.ctre.phoenix6.hardware.CANcoder; +import com.studica.frc.AHRS; +import com.studica.frc.AHRS.NavXComType; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.controllers.PPHolonomicDriveController; +import com.pathplanner.lib.util.PathPlannerLogging; +import com.pathplanner.lib.config.PIDConstants; +import com.pathplanner.lib.config.RobotConfig; + +import org.carlmontrobotics.lib199.swerve.SwerveModuleSim; + +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.ControlType; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.ClosedLoopConfig; +import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.hal.SimDouble; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Twist2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.kinematics.SwerveDriveKinematics; +import edu.wpi.first.math.kinematics.SwerveDriveOdometry; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.Angle; +import edu.wpi.first.units.measure.AngularVelocity; +import edu.wpi.first.units.measure.Distance; +import edu.wpi.first.units.measure.LinearVelocity; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +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; +import edu.wpi.first.units.MutableMeasure; +import edu.wpi.first.units.measure.Velocity; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.SendableBuilder; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.SelectCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.wpilibj.RobotBase; +import edu.wpi.first.wpilibj.simulation.SimDeviceSim; + +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Rotation; +import static edu.wpi.first.units.Units.RotationsPerSecond; +import static edu.wpi.first.units.Units.Seconds; +import static edu.wpi.first.units.Units.Volt; +import static edu.wpi.first.units.Units.Meter; +import static edu.wpi.first.units.Units.Meters; + +public class SimpleDrivetrain extends SubsystemBase { + + private final AHRS gyro = new AHRS(NavXComType.kMXP_SPI); + private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); + // ^used by PathPlanner for chaining paths + private SwerveDriveKinematics kinematics = null; + // private SwerveDriveOdometry odometry = null; + private SwerveDrivePoseEstimator poseEstimator = null; + + private SwerveModule modules[]; + private boolean fieldOriented = true; + private double fieldOffset = 0; + + private SparkFlex[] driveMotors = new SparkFlex[] { null, null, null, null }; + private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; + private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; + private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; + public final float initPitch; + public final float initRoll; + + // debug purposes + private SwerveConfig swerveConfig; + private SwerveModule moduleFL; + private SwerveModule moduleFR; + private SwerveModule moduleBL; + private SwerveModule moduleBR; + + private final Field2d field = new Field2d(); + private final Field2d odometryField = new Field2d(); + private final Field2d poseWithLimelightField = new Field2d(); + + public double ppKpDrive = 5.0; + public double ppKiDrive = 0; + public double ppKdDrive = 0; + + public double ppKpTurn = 3; + public double ppKiTurn = 0; + public double ppKdTurn = 0; + + double accelX; + double accelY; + double accelXY; + + private SwerveModuleSim[] moduleSims; + private SimDouble gyroYawSim; + private Timer simTimer = new Timer(); + public double extraSpeedMult = 0; + + private double lastSetX = 0, lastSetY = 0, lastSetTheta = 0; + double kP = 0; + double kI = 0; + double kD = 0; + public enum Mode { + coast, + brake, + toggle + } + + //Constants + double g = 9.8; //m/s^2 + double mu = 1; + double autoCentripetalAccel = mu * g * 2; + + /** + * + * @param wheelBase in meters + * @param trackWidth in meters + */ + public SimpleDrivetrain( + double wheelBase, + double trackWidth, + double wheelDiameterMeters, + double driveGearing, + double kForwardVolts, + double kBackwardVolts, + double kForwardVels, + double kForwardAccels, + double kBackwardVels, + double kBackwardAccels, + double[] drivePID, // [kP, kI, kD] + double[] turnPID, // [kP, kI, kD] + double[] turnFeedForward,//[kS, kV, kA] + double[] turnZeroDeg, //(FL, FR, BL, BR) + boolean[] driveInversion, //(FL, FR, BL, BR) + boolean[] reversed, //FIXME what does this do (FL, FR, BL, BR) + double driveModifier, //Max speed applier + boolean[] turnInversion //(FL, FR, BL, BR) + ) + { + swerveConfig = new SwerveConfig( + wheelDiameterMeters, + driveGearing, + mu, + autoCentripetalAccel, + kForwardVolts, + kForwardVels, + kForwardAccels, + kBackwardVolts, + kBackwardVels, + kBackwardAccels, + drivePID[0], + drivePID[1], + drivePID[2], + turnPID[0], + turnPID[1], + turnPID[2], + turnFeedForward[0], + turnFeedForward[1], + turnFeedForward[2], + turnZeroDeg, + driveInversion, + reversed, + driveModifier, + turnInversion); + + AutoBuilder(); + + double initTimestamp = Timer.getFPGATimestamp(); + double currentTimestamp = initTimestamp; + while (gyro.isCalibrating() && currentTimestamp - initTimestamp < 10) { + currentTimestamp = Timer.getFPGATimestamp(); + try { + Thread.sleep(1000);// 1 second + } catch (InterruptedException e) { + e.printStackTrace(); + break; + } + System.out.println("Calibrating the gyro..."); + } + gyro.reset(); + // this.resetFieldOrientation(); + System.out.println("NavX-MXP firmware version: " + gyro.getFirmwareVersion()); + System.out.println("Magnetometer is calibrated: " + gyro.isMagnetometerCalibrated()); + + // Setup Kinematics + { + // Define the corners of the robot relative to the center of the robot using + // Translation2d objects. + // Positive x-values represent moving toward the front of the robot whereas + // positive y-values represent moving toward the left of the robot. + Translation2d locationFL = new Translation2d(wheelBase / 2, trackWidth / 2); + Translation2d locationFR = new Translation2d(wheelBase / 2, -trackWidth / 2); + Translation2d locationBL = new Translation2d(-wheelBase / 2, trackWidth / 2); + Translation2d locationBR = new Translation2d(-wheelBase / 2, -trackWidth / 2); + + kinematics = new SwerveDriveKinematics(locationFL, locationFR, locationBL, locationBR); + } + + // Initialize modules + { + // initPitch = 0; + // initRoll = 0; + Supplier pitchSupplier = () -> 0F; + Supplier rollSupplier = () -> 0F; + initPitch = gyro.getPitch(); + initRoll = gyro.getRoll(); + // Supplier pitchSupplier = () -> gyro.getPitch(); + // Supplier rollSupplier = () -> gyro.getRoll(); + + + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + driveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), + turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + //SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, + driveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), + turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, + driveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), + turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, + driveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), + turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + turnPidControllers[0] = turnMotors[0].getClosedLoopController(); + turnPidControllers[1] = turnMotors[1].getClosedLoopController(); + turnPidControllers[2] = turnMotors[2].getClosedLoopController(); + turnPidControllers[3] = turnMotors[3].getClosedLoopController(); + if (RobotBase.isSimulation()) { + moduleSims = new SwerveModuleSim[] { + moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() + }; + gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); + } + SmartDashboard.putData("Module FL",moduleFL); + SmartDashboard.putData("Module FR",moduleFR); + SmartDashboard.putData("Module BL",moduleBL); + SmartDashboard.putData("Module BR",moduleBR); + + SmartDashboard.putNumber("bigoal", 0); + + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkFlex driveMotor : driveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : turnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + } + + for (CANcoder coder : turnEncoders) { + coder.getAbsolutePosition().setUpdateFrequency(500); + coder.getPosition().setUpdateFrequency(500); + coder.getVelocity().setUpdateFrequency(500); + } + + //SmartDashboard.putData("Field", field); + //SmartDashboard.putData("Odometry Field", odometryField); + //martDashboard.putData("Pose with Limelight Field", poseWithLimelightField); + + accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis + accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis + accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); + + // for(SparkMax driveMotor : driveMotors) + // driveMotor.setSmartCurrentLimit(80); + + // Must call this method for SysId to run + if (CONFIG.isSysIdTesting()) { + sysIdSetup(); + } + } + + // odometry = new SwerveDriveOdometry(kinematics, + // Rotation2d.fromDegrees(getHeading()), getModulePositions(), + // new Pose2d()); + + poseEstimator = new SwerveDrivePoseEstimator( + getKinematics(), + Rotation2d.fromDegrees(getHeading()), + getModulePositions(), + new Pose2d()); + + // Setup autopath builder + //configurePPLAutoBuilder(); + // SmartDashboard.putNumber("chassis speeds x", 0); + // SmartDashboard.putNumber("chassis speeds y", 0); + + // SmartDashboard.putNumber("chassis speeds theta", 0); + SmartDashboard.putData(this); // For seeing drivetrain data in SmartDashboard + + } + + + + public boolean isAtAngle(double desiredAngleDeg, double toleranceDeg){ + for (SwerveModule module : modules) { + if (!(Math.abs(MathUtil.inputModulus(module.getModuleAngle() - desiredAngleDeg, -90, 90)) < toleranceDeg)) + return false; + } + return true; + } + + @Override + public void simulationPeriodic() { + for (var moduleSim : moduleSims) { + moduleSim.update(); + } + SwerveModuleState[] measuredStates = + new SwerveModuleState[] { + moduleFL.getCurrentState(), moduleFR.getCurrentState(), moduleBL.getCurrentState(), moduleBR.getCurrentState() + }; + ChassisSpeeds speeds = kinematics.toChassisSpeeds(measuredStates); + + double dtSecs = simTimer.get(); + simTimer.restart(); + + Pose2d simPose = field.getRobotPose(); + simPose = simPose.exp( + new Twist2d( + speeds.vxMetersPerSecond * dtSecs, + speeds.vyMetersPerSecond * dtSecs, + speeds.omegaRadiansPerSecond * dtSecs)); + double newAngleDeg = simPose.getRotation().getDegrees(); + // Subtract the offset computed the last time setPose() was called because odometry.update() adds it back. + newAngleDeg -= simGyroOffset.getDegrees(); + newAngleDeg *= (isGyroReversed ? -1.0 : 1.0); + gyroYawSim.set(newAngleDeg); + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + /** + * Sets swerveModules IdleMode both turn and drive + * @param brake boolean for braking, if false then coast + */ + public void setDrivingIdleMode(boolean brake) { + IdleMode mode; + if (brake) { + mode = IdleMode.kBrake; + } + else { + mode = IdleMode.kCoast; + } + for (SparkMax turnMotor : turnMotors) { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(mode); + turnMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + for (SparkFlex driveMotor : driveMotors) { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(mode); + driveMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + @Override + public void periodic() { + detectCollision(); //This does nothing + PathPlannerLogging.logCurrentPose(getPose()); + + //maybe add the field with the position of the robot with only limelight and the field with the position of the robot with only odometry? + //We can compare the two fields to see if odometry is causing the pose to be inaccurate when it hits the reef. + + // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); + // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); + // double goal = SmartDashboard.getNumber("GoalPos", 0); + // PIDController pid = new PIDController(kP, kI, kD); + // kP = SmartDashboard.getNumber("kP", 0); + // kI = SmartDashboard.getNumber("kI", 0); + // kD = SmartDashboard.getNumber("kD", 0); + //pid.setIZone(20); + //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); + // SparkMaxConfig config = new SparkMaxConfig(); + + //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); + // System.out.println(kP); + // config.closedLoop.pid(kP ,kI,kD); + // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); + // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + // //moduleFL.move(0.0000001, 180); + //moduleFL.move(0.01, 180); + // moduleFR.move(0.000000001, 0); + // moduleBR.move(0.0000001, 0); + // moduleFL.move(0.000001, 0); + // moduleBL.move(0.000001, 0); + // turnPidControllers[0].setReference(goal + + // , ControlType.kPosition, ClosedLoopSlot.kSlot0); + + + // 167 -> -200 + // 138 -> 360 + // for (CANcoder coder : turnEncoders) { + // SignalLogger.writeDouble("Regular position " + coder.toString(), + // coder.getPosition().getValue().baseUnitMagnitude()); + // SignalLogger.writeDouble("Velocity " + coder.toString(), + // coder.getVelocity().getValue().baseUnitMagnitude()); + // SignalLogger.writeDouble("Absolute position " + coder.toString(), + // coder.getAbsolutePosition().getValue().baseUnitMagnitude()); + // } + // String out=""; int i=0; + // for (CANcoder coder : turnEncoders) { + // out+=String.format("[i] Abs Pos: %.3f Goal Pos: %.3f ", coder.getAbsolutePosition().getValue().baseUnitMagnitude(),0); + // i++; + // } + // lobotomized to prevent ucontrollabe swerve behavior + // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); + // moduleFL.periodic(); + // moduleFR.periodic(); + // moduleBL.periodic(); + // moduleBR.periodic(); + double goal = SmartDashboard.getNumber("bigoal", 0); + for (SwerveModule module : modules) { + // module.turnPeriodic(); + // module.turnPeriodic(); + module.move(0.00000000001, goal); + module.periodic(); + } + + // field.setRobotPose(odometry.getPoseMeters()); + + + + // odometry.update(gyro.getRotation2d(), getModulePositions()); + + // poseEstimator.update(gyro.getRotation2d(), getModulePositions()); + + //odometry.update(Rotation2d.fromDegrees(getHeading()), getModulePositions()); + + // updateMT2PoseEstimator(); + + // double currSetX = + // SmartDashboard.getNumber("Pose Estimator set x (m)", lastSetX); + // double currSetY = + // SmartDashboard.getNumber("Pose Estimator set y (m)", lastSetY); + // double currSetTheta = SmartDashboard + // .getNumber("Pose Estimator set rotation (deg)", lastSetTheta); + + // if (lastSetX != currSetX || lastSetY != currSetY + // || lastSetTheta != currSetTheta) { + // setPose(new Pose2d(currSetX, currSetY, + // Rotation2d.fromDegrees(currSetTheta))); + // } + + // setPose(new Pose2d(getPose().getTranslation().getX(), + // getPose().getTranslation().getY(), + // Rotation2d.fromDegrees(getHeading()))); + + + // SmartDashboard.putNumber("X position with limelight", getPoseWithLimelight().getX()); + // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY()); + SmartDashboard.putNumber("X position with gyro", getPose().getX()); + SmartDashboard.putNumber("Y position with gyro", getPose().getY()); + SmartDashboard.putData(CONFIG); + + //For finding acceleration of drivetrain for collision detector + SmartDashboard.putNumber("Accel X", accelX); + SmartDashboard.putNumber("Accel Y", accelY); + SmartDashboard.putNumber("2D Acceleration ", accelXY); + + // // // SmartDashboard.putNumber("Pitch", gyro.getPitch()); + // // // SmartDashboard.putNumber("Roll", gyro.getRoll()); + // SmartDashboard.putNumber("Raw gyro angle", gyro.getAngle()); + // SmartDashboard.putNumber("Robot Heading", getHeading()); + // // // SmartDashboard.putNumber("AdjRoll", gyro.getPitch() - initPitch); + // // // SmartDashboard.putNumber("AdjPitch", gyro.getRoll() - initRoll); + // SmartDashboard.putBoolean("Field Oriented", fieldOriented); + // SmartDashboard.putNumber("Gyro Compass Heading", gyro.getCompassHeading()); + // SmartDashboard.putNumber("Compass Offset", compassOffset); + // SmartDashboard.putBoolean("Current Magnetic Field Disturbance", gyro.isMagneticDisturbance()); + SmartDashboard.putNumber("front left encoder", moduleFL.getModuleAngle()); + SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); + SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); + SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + } + + @Override + public void initSendable(SendableBuilder builder) { + super.initSendable(builder); + + for (SwerveModule module : modules) + SendableRegistry.addChild(this, module); + + builder.addBooleanProperty("Magnetic Field Disturbance", + gyro::isMagneticDisturbance, null); + builder.addBooleanProperty("Gyro Calibrating", gyro::isCalibrating, null); + builder.addBooleanProperty("Field Oriented", () -> fieldOriented, + fieldOriented -> this.fieldOriented = fieldOriented); + builder.addDoubleProperty("Pose Estimator X", () -> getPose().getX(), + null); + builder.addDoubleProperty("Pose Estimator Y", () -> getPose().getY(), + null); + builder.addDoubleProperty("Pose Estimator Theta", + () -> + getPose().getRotation().getDegrees(), null); + builder.addDoubleProperty("Robot Heading", () -> getHeading(), null); + builder.addDoubleProperty("Raw Gyro Angle", gyro::getAngle, null); + builder.addDoubleProperty("Pitch", gyro::getPitch, null); + builder.addDoubleProperty("Roll", gyro::getRoll, null); + builder.addDoubleProperty("Field Offset", () -> fieldOffset, fieldOffset -> + this.fieldOffset = fieldOffset); + builder.addDoubleProperty("FL Turn Encoder (Deg)", + () -> moduleFL.getModuleAngle(), null); + builder.addDoubleProperty("FR Turn Encoder (Deg)", + () -> moduleFR.getModuleAngle(), null); + builder.addDoubleProperty("BL Turn Encoder (Deg)", + () -> moduleBL.getModuleAngle(), null); + builder.addDoubleProperty("BR Turn Encoder (Deg)", + () -> moduleBR.getModuleAngle(), null); + } + + + // #region Drive Methods + + /** + * Drives the robot using the given x, y, and rotation speed + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive + */ + public void setExtraSpeedMult(double set) { + extraSpeedMult=set; + } + + /** + * Calculates and implements the required SwerveStates for all 4 modules to get the wanted outcome + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is positive. + */ + public void drive(double forward, double strafe, double rotation) { + drive(getSwerveStates(forward, strafe, rotation)); + } + + /** + * Implements the provided SwerveStates for all 4 modules to get the wanted outcome + * @param moduleStates SwerveModuleState[] + */ + public void drive(SwerveModuleState[] moduleStates) { + //Max speed override + double max = maxSpeed; + SwerveDriveKinematics.desaturateWheelSpeeds(moduleStates, max); + for (int i = 0; i < 4; i++) { + // SmartDashboard.putNumber("moduleIn" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + moduleStates[i].optimize(Rotation2d.fromDegrees(modules[i].getModuleAngle())); + // SmartDashboard.putNumber("moduleOT" + Integer.toString(i), moduleStates[i].angle.getDegrees()); + modules[i].move(moduleStates[i].speedMetersPerSecond, moduleStates[i].angle.getDegrees()); + } + } + + /** + * Configures PathPlanner AutoBuilder + */ + public void AutoBuilder() { + RobotConfig config = Constants.Drivetrainc.Autoc.robotConfig; + AutoBuilder.configure( + //Supplier poseSupplier, + this::getPose, // Robot pose supplier + //Consumer resetPose, + this::setPoseWithLimelight, // Method to reset odometry (will be called if your auto has a starting pose) + //Supplier robotRelativeSpeedsSupplier, + this::getSpeeds, // ChassisSpeeds supplier. MUST BE ROBOT RELATIVE + //BiConsumer output, + (speeds, feedforwards) -> drive(kinematics.toSwerveModuleStates(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds. Also optionally outputs individual module feedforwards + //PathFollowingController controller, + new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for holonomic drive trains + new PIDConstants(4 + , ppKiDrive, ppKdDrive), // Translation PID constants + new PIDConstants(1, ppKiTurn, ppKdTurn) + ), + //RobotConfig robotConfig, + config, // The robot configuration + //BooleanSupplier shouldFlipPath, + () -> { + // Boolean supplier that controls when the path will be mirrored for the red alliance + // This will flip the path being followed to the red side of the field. + // THE ORIGIN WILL REMAIN ON THE BLUE SIDE + var alliance = DriverStation.getAlliance(); + if (alliance.isPresent()) { + return alliance.get() == DriverStation.Alliance.Red; + } + return false; + }, + //Subsystem... driveRequirements + this // Reference to this subsystem to set requirements + ); + } + +//---------------------------------------------------------- + + public void autoCancelDtCommand() { + if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; + + // Use hasDriverInput to get around acceleration limiting on slowdown + if (((TeleopDrive) getDefaultCommand()).hasDriverInput()) { + Command currentDtCommand = getCurrentCommand(); + if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof RotateToFieldRelativeAngle) + && currentDtCommand != null) { + currentDtCommand.cancel(); + } + } + } + + public void stop() { + for (SwerveModule module : modules) + module.move(0, 0); + } + + public boolean isStopped() { + return Math.abs(getSpeeds().vxMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().vyMetersPerSecond) < 0.1 && + Math.abs(getSpeeds().omegaRadiansPerSecond) < 0.1; + } + + /** + * Constructs and returns a ChassisSpeeds objects using forward, strafe, and + * rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A ChassisSpeeds object. + */ + private ChassisSpeeds getChassisSpeeds(double forward, double strafe, double rotation) { + ChassisSpeeds speeds; + if (fieldOriented) { + speeds = ChassisSpeeds.fromFieldRelativeSpeeds(forward, strafe, rotation, + Rotation2d.fromDegrees(getHeading())); + } else { + speeds = new ChassisSpeeds(forward, strafe, rotation); + } + return speeds; + } + + /** + * Constructs and returns four SwerveModuleState objects, one for each side, + * using forward, strafe, and rotation values. + * + * @param forward The desired forward speed, in m/s. Forward is positive. + * @param strafe The desired strafe speed, in m/s. Left is positive. + * @param rotation The desired rotation speed, in rad/s. Counter clockwise is + * positive. + * @return A SwerveModuleState array, one for each side of the drivetrain (FL, + * FR, etc.). + */ + private SwerveModuleState[] getSwerveStates(double forward, double strafe, double rotation) { + return kinematics.toSwerveModuleStates(getChassisSpeeds(forward, -strafe, rotation)); + } + + public SwerveModuleState[] getSwerveStates(ChassisSpeeds speeds) { + return kinematics.toSwerveModuleStates(speeds); + } + + // #endregion + + // #region Getters and Setters + + /** + * @return the heading in degrees, normalized to the range -180 to 180 + */ + public double getHeading() { + double x = gyro.getAngle(); + if (fieldOriented) + x -= fieldOffset; + return Math.IEEEremainder(x * (isGyroReversed ? -1.0 : 1.0), 360); + } + + public SwerveModulePosition[] getModulePositions() { + return Arrays.stream(modules).map(SwerveModule::getCurrentPosition).toArray(SwerveModulePosition[]::new); + } + + public SwerveDrivePoseEstimator getPoseEstimator() { + return poseEstimator; + } + + /** + * Gets pose from {@link #poseEstimator} + * @return Pose2D + */ + public Pose2d getPose() { + // return odometry.getPoseMeters(); + return poseEstimator.getEstimatedPosition(); + } + + private Rotation2d simGyroOffset = new Rotation2d(); + public void setPose(Pose2d initialPose) { + Rotation2d gyroRotation = gyro.getRotation2d(); + // odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); + + poseEstimator.resetPosition(gyroRotation, getModulePositions(), initialPose); + // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future + // We need the offset so that we can compensate for it during simulationPeriodic(). + simGyroOffset = initialPose.getRotation().minus(gyroRotation); + //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); + } + + //This method will set the pose using limelight if it sees a tag and if not it is supposed to run like setPose() + public void setPoseWithLimelight(Pose2d backupPose){ //the pose will be set to backupPose if no tag is seen + // Rotation2d gyroRotation = gyro.getRotation2d(); + // Pose2d pose; + + // if (LimelightHelpers.getTV(REEF_LL)) { + + // pose = LimelightHelpers.getBotPose2d_wpiBlue(REEF_LL); + + // } else if (LimelightHelpers.getTV(CORAL_LL)) { + + // pose = LimelightHelpers.getBotPose2d_wpiBlue(CORAL_LL); + // } + // else { + // pose = backupPose; + // } + + // poseEstimator.resetPosition(gyroRotation, getModulePositions(), pose); + // simGyroOffset = pose.getRotation().minus(gyroRotation); + + } + /** + * Detects if the robot has experienced a collision based on acceleration thresholds. + * @return true if a 2D acceleration greater than the {@link #COLLISION_ACCELERATION_THRESHOLD} false otherwise. + */ + public boolean detectCollision(){ //We can implement this method into updatePoseWithLimelight so that if there is a collision it stops using odometry + accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis + accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis + accelXY = Math.sqrt(accelX * accelX + accelY * accelY); // 2D Acceleration + return accelXY > COLLISION_ACCELERATION_THRESHOLD; // return true if collision detected + } + + /** + * @deprecated Use {@link #resetFieldOrientation()} instead + */ + @Deprecated + public void resetHeading() { + gyro.reset(); + + } + + public double getPitch() { + return gyro.getPitch(); + } + + public double getRoll() { + return gyro.getRoll(); + } + /** + * true stands for fieldOriented, false stands for robotOriented + * @return boolean + */ + public boolean getFieldOriented() { + return fieldOriented; + } + + /** + * True sets fieldOriented, false sets robotOriented + * @param fieldOriented boolean + */ + public void setFieldOriented(boolean fieldOriented) { + this.fieldOriented = fieldOriented; + } + /** + * Sets the current direction the robot is facing is to be 0 + */ + public void resetFieldOrientation() { + fieldOffset = gyro.getAngle(); + } + /** + * Sets the current direction the robot is facing is to be 180 + */ + public void resetFieldOrientationBackwards() { + fieldOffset = 180 + gyro.getAngle(); + } + /** + * Sets the current direction the robot is facing is plus @param angle to be 0 + * @param angle in degrees + */ + public void resetFieldOrientationWithAngle(double angle) { + fieldOffset = angle + gyro.getAngle(); + } + public void resetPoseEstimator() { + // odometry.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + + poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + gyro.reset(); + } + + public SwerveDriveKinematics getKinematics() { + return kinematics; + } + + public ChassisSpeeds getSpeeds() { + return kinematics.toChassisSpeeds(Arrays.stream(modules).map(SwerveModule::getCurrentState) + .toArray(SwerveModuleState[]::new)); + } + + public void setMode(Mode mode) { + for (SwerveModule module : modules){ + switch (mode) { + case coast: + module.coast(); + break; + case brake: + module.brake(); + break; + case toggle: + module.toggleMode(); + break; + } + } + } + /** + * @deprecated Use {@link #setMode(Mode)} instead + * Changes between IdleModes + */ + @Deprecated + public void toggleMode() { + for (SwerveModule module : modules) + module.toggleMode(); + } + /** + * @deprecated Use {@link #setMode(Mode)} instead + */ + @Deprecated + public void brake() { + for (SwerveModule module : modules) + module.brake(); + } + /** + * @deprecated Use {@link #setMode(Mode)} instead + */ + @Deprecated + public void coast() { + for (SwerveModule module : modules) + module.coast(); + } + + + + // #region SysId Code + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage[] m_appliedVoltage = new MutVoltage[8]; + + // Mutable holder for unit-safe linear distance values, persisted to avoid + // reallocation. + private final MutDistance[] m_distance = new MutDistance[4]; + // Mutable holder for unit-safe linear velocity values, persisted to avoid + // reallocation. + private final MutLinearVelocity[] m_velocity = new MutLinearVelocity[4]; + // edu.wpi.first.math.util.Units.Rotations beans; + private final MutAngle[] m_revs = new MutAngle[4]; + private final MutAngularVelocity[] m_revs_vel = new MutAngularVelocity[4]; + + private enum SysIdTest { + FRONT_DRIVE, + BACK_DRIVE, + ALL_DRIVE, + // FLBR_TURN, + // FRBL_TURN, + // ALL_TURN + FL_ROT, + FR_ROT, + BL_ROT, + BR_ROT + } + + private SendableChooser sysIdChooser = new SendableChooser<>(); + + // ROUTINES FOR SYSID + // private SysIdRoutine.Config defaultSysIdConfig = new + // SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(5)); + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config(Volts.of(1).per(Seconds), + Volts.of(2.891), Seconds.of(10)); + + // DRIVE + private void motorLogShort_drive(SysIdRoutineLog log, int id) { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + driveMotors[id].getBusVoltage() * driveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(driveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(driveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } + + // Create a new SysId routine for characterizing the drive. + private SysIdRoutine frontOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + // Tell SysId how to give the driving voltage to the motors. + (Voltage volts) -> { + driveMotors[0].setVoltage(volts.in(Volts)); + driveMotors[1].setVoltage(volts.in(Volts)); + modules[2].coast(); + modules[3].coast(); + }, + log -> {// FRONT + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + }, + this)); + + private SysIdRoutine backOnlyDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + modules[0].coast(); + modules[1].coast(); + modules[2].brake(); + modules[3].brake(); + driveMotors[2].setVoltage(volts.in(Volts)); + driveMotors[3].setVoltage(volts.in(Volts)); + }, + log -> {// BACK + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine allWheelsDriveRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + (Voltage volts) -> { + for (SparkFlex dm : driveMotors) { + dm.setVoltage(volts.in(Volts)); + } + }, + log -> { + motorLogShort_drive(log, 0);// fl named automatically + motorLogShort_drive(log, 1);// fr + motorLogShort_drive(log, 2);// bl + motorLogShort_drive(log, 3);// br + }, + this)); + + private SysIdRoutine sysidroutshort_turn(int id, String logname) { + return new SysIdRoutine( + defaultSysIdConfig, + // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), + // Seconds.of(3)), + new SysIdRoutine.Mechanism( + (Voltage volts) -> turnMotors[id].setVoltage(volts.in(Volts)), + log -> log.motor(logname + "_turn") + .voltage(m_appliedVoltage[id + 4].mut_replace( + // ^because drivemotors take up the first 4 slots of the unit holders + turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) + .angularPosition( + m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) + .angularVelocity(m_revs_vel[id].mut_replace( + turnEncoders[id].getVelocity().getValueAsDouble(), RotationsPerSecond)), + this)); + } + + // as always, fl/fr/bl/br + private SysIdRoutine[] rotateRoutine = new SysIdRoutine[] { + sysidroutshort_turn(0, "fl"), // woaw, readable code??? + sysidroutshort_turn(1, "fr"), + sysidroutshort_turn(2, "bl"), + sysidroutshort_turn(3, "br") + }; + + //TODO: migrate to elastic + //private ShuffleboardTab sysIdTab = Shuffleboard.getTab("Drivetrain SysID"); + + // void sysidtabshorthand(String name, SysIdRoutine.Direction dir, int width, + // int height){ + // sysIdTab.add(name, dir).withSize(width, height); + // } + void sysidtabshorthand_qsi(String name, SysIdRoutine.Direction dir) { + //TODO: migrate to elastic + //sysIdTab.add(name, sysIdQuasistatic(dir)).withSize(2, 1); + } + + // void sysidtabshorthand_dyn(String name, SysIdRoutine.Direction dir) { + // sysIdTab.add(name, sysIdDynamic(dir)).withSize(2, 1); + // } + + private void sysIdSetup() { + // SysId Setup + { + Supplier stopNwait = () -> new SequentialCommandGroup( + new InstantCommand(this::stop), new WaitCommand(2)); + + /* + * Alex's old sysId tests + * sysIdTab.add("All sysid tests", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,2), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,2), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - FRONT wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,0), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,0), + * (Command)stopNwait.get()) + * )); + * sysIdTab.add("All sysid tests - BACK wheels", new SequentialCommandGroup( + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new + * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,1), + * (Command)stopNwait.get()), + * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,1), + * (Command)stopNwait.get()) + * )); + */ + + // sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); + // sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); + // sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); + // sysidtabshorthand_dyn("Dynamic Backward", SysIdRoutine.Direction.kReverse); + + //TODO: migrate to elastic + //sysIdTab + //.add(sysIdChooser) + //.withSize(2, 1); + + sysIdChooser.addOption("Front Only Drive", SysIdTest.FRONT_DRIVE); + sysIdChooser.addOption("Back Only Drive", SysIdTest.BACK_DRIVE); + sysIdChooser.addOption("All Drive", SysIdTest.ALL_DRIVE); + // sysIdChooser.addOption("fl-br Turn", SysIdTest.FLBR_TURN); + // sysIdChooser.addOption("fr-bl Turn", SysIdTest.FRBL_TURN); + // sysIdChooser.addOption("All Turn", SysIdTest.ALL_TURN); + sysIdChooser.addOption("FL Rotate", SysIdTest.FL_ROT); + sysIdChooser.addOption("FR Rotate", SysIdTest.FR_ROT); + sysIdChooser.addOption("BL Rotate", SysIdTest.BL_ROT); + sysIdChooser.addOption("BR Rotate", SysIdTest.BR_ROT); + + //TODO: migrate to elastic + //sysIdTab.add("ALL THE SYSID TESTS", allTheSYSID())// is this legal?? + //.withSize(2, 1); + + // sysIdTab.add("Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)).withSize(2, 1); + // sysIdTab.add("Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)).withSize(2, 1); + // SmartDashboard.putData("Quackson Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); + // SmartDashboard.putData("Quackson Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); + + // SmartDashboard.putData("Dyanmic forward", sysIdDynamic(SysIdRoutine.Direction.kForward));//.withSize(2, 1); + // SmartDashboard.putData("Dyanmic backward", sysIdDynamic(SysIdRoutine.Direction.kReverse));//.withSize(2, 1); + //sysIdTab.add(this); + + for (int i = 0; i < 8; i++) {// first four are drive, next 4 are turn motors + m_appliedVoltage[i] = Volt.mutable(0); + } + for (int i = 0; i < 4; i++) { + m_distance[i] = Meter.mutable(0); + m_velocity[i] = MetersPerSecond.mutable(0); + + m_revs[i] = Rotation.mutable(0); + m_revs_vel[i] = RotationsPerSecond.mutable(0); + } + + // SmartDashboard.putNumber("Desired Angle", 0); + + // SmartDashboard.putNumber("kS", 0); + } + } + + // public Command sysIdQuasistatic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyRoutine.quasistatic(direction); + // case 1: + // return backOnlyRoutine.quasistatic(direction); + // case 2: + // return allWheelsRoutine.quasistatic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + + private SysIdTest selector() { + //SysIdTest test = sysIdChooser.getSelected(); + SysIdTest test = SysIdTest.FRONT_DRIVE; + System.out.println("Test Selected: " + test); + return test; + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return new SelectCommand<>( + Map.ofEntries( + // DRIVE + Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running front only quasistatic forward") + : new PrintCommand("Running front only quasistatic backward"), + frontOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running back only quasistatic forward") + : new PrintCommand("Running back only quasistatic backward"), + backOnlyDriveRoutine.quasistatic(direction))), + Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running all drive quasistatic forward") + : new PrintCommand("Running all drive quasistatic backward"), + allWheelsDriveRoutine.quasistatic(direction))), + // ROTATE + Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FL rotate quasistatic forward") + : new PrintCommand("Running FL rotate quasistatic backward"), + rotateRoutine[0].quasistatic(direction))), + Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running FR rotate quasistatic forward") + : new PrintCommand("Running FR rotate quasistatic backward"), + rotateRoutine[1].quasistatic(direction))), + Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BL rotate quasistatic forward") + : new PrintCommand("Running BL rotate quasistatic backward"), + rotateRoutine[2].quasistatic(direction))), + Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + direction == SysIdRoutine.Direction.kForward + ? new PrintCommand("Running BR rotate quasistatic forward") + : new PrintCommand("Running BR rotate quasistatic backward"), + rotateRoutine[3].quasistatic(direction))) + + // //TURN + // Map.entry(SysIdTest.FLBR_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fL-bR turn quasistatic forward") : + // new PrintCommand("Running fL-bR turn quasistatic backward"), + // flbrTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.FRBL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running fR-bL turn quasistatic forward") : + // new PrintCommand("Running fR-bL turn quasistatic backward"), + // frblTurn.quasistatic(direction) + // )), + // Map.entry(SysIdTest.ALL_TURN, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward ? + // new PrintCommand("Running all turn quasistatic forward") : + // new PrintCommand("Running all turn quasistatic backward"), + // allWheelsTurn.quasistatic(direction) + // )) + ), + this::selector); + } + + // public Command sysIdDynamic(SysIdRoutine.Direction direction, int + // frontorback) { + // switch(frontorback) { + // case 0: + // return frontOnlyDrive.dynamic(direction); + // case 1: + // return backOnlyDrive.dynamic(direction); + // case 2: + // return allWheelsDrive.dynamic(direction); + // } + // return new PrintCommand("Invalid Command"); + // } + private Command allTheSYSID(SysIdRoutine.Direction direction) { + return new SequentialCommandGroup( + frontOnlyDriveRoutine.dynamic(direction), + backOnlyDriveRoutine.dynamic(direction), + allWheelsDriveRoutine.dynamic(direction), + rotateRoutine[0].dynamic(direction), + rotateRoutine[1].dynamic(direction), + rotateRoutine[2].dynamic(direction), + rotateRoutine[3].dynamic(direction), + + frontOnlyDriveRoutine.quasistatic(direction), + backOnlyDriveRoutine.quasistatic(direction), + allWheelsDriveRoutine.quasistatic(direction), + rotateRoutine[0].quasistatic(direction), + rotateRoutine[1].quasistatic(direction), + rotateRoutine[2].quasistatic(direction), + rotateRoutine[3].quasistatic(direction)); + } + + /** + * Makes sysId to run for both directions + * @return Command to run sysId + */ + public Command allTheSYSID() { + return new SequentialCommandGroup( + allTheSYSID(SysIdRoutine.Direction.kForward), + allTheSYSID(SysIdRoutine.Direction.kReverse)); + } + /** + * Makes sysId to find feedforward and pid values for drivetrain + * @param direction SysIdRoutine.Direction.kForward or kReverse + * @return Command to run sysID + */ + // public Command sysIdDynamic(SysIdRoutine.Direction direction) { + // return new SelectCommand<>( + // Map.ofEntries( + // // DRIVE + // Map.entry(SysIdTest.FRONT_DRIVE, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running front only dynamic forward") + // : new PrintCommand("Running front only dynamic backward"), + // frontOnlyDriveRoutine.dynamic(direction))), + // Map.entry(SysIdTest.BACK_DRIVE, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running back only dynamic forward") + // : new PrintCommand("Running back only dynamic backward"), + // backOnlyDriveRoutine.dynamic(direction))), + // Map.entry(SysIdTest.ALL_DRIVE, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running all wheels dynamic forward") + // : new PrintCommand("Running all wheels dynamic backward"), + // allWheelsDriveRoutine.dynamic(direction))), + // // ROTATE + // Map.entry(SysIdTest.FL_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running FL rotate dynamic forward") + // : new PrintCommand("Running FL rotate dynamic backward"), + // rotateRoutine[0].dynamic(direction))), + // Map.entry(SysIdTest.FR_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running FR rotate dynamic forward") + // : new PrintCommand("Running FR rotate dynamic backward"), + // rotateRoutine[1].dynamic(direction))), + // Map.entry(SysIdTest.BL_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running BL rotate dynamic forward") + // : new PrintCommand("Running BL rotate dynamic backward"), + // rotateRoutine[2].dynamic(direction))), + // Map.entry(SysIdTest.BR_ROT, new ParallelCommandGroup( + // direction == SysIdRoutine.Direction.kForward + // ? new PrintCommand("Running BR rotate dynamic forward") + // : new PrintCommand("Running BR rotate dynamic backward"), + // rotateRoutine[3].dynamic(direction)))), + // this::selector); + // } + + // #endregion + + /** + * Sets all SwerveModules to point in a certain angle + * @param angle in degrees + */ + public void keepRotateMotorsAtDegrees(int angle) { + for (SwerveModule module : modules) { + module.turnPeriodic(); + module.move(0, angle); + } + } + + /** + * Gets how fast the robot is spinning from gyro + * @return degrees per second + */ + public double getGyroRate() { + return gyro.getRate(); + } +} diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig b/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig new file mode 100644 index 0000000..a689dfd --- /dev/null +++ b/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig @@ -0,0 +1,8 @@ +package org.carlmontrobotics.lib199.simplemechs.elevator; + +public final class ElevatorConfig { + + public ElevatorConfig() { + + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java new file mode 100644 index 0000000..a69f3f0 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java @@ -0,0 +1,17 @@ +// 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 org.carlmontrobotics.SimpleMechs.Elevator; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SimpleElevator extends SubsystemBase { + /** Creates a new Elevator. */ + public SimpleElevator() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java new file mode 100644 index 0000000..288e31f --- /dev/null +++ b/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java @@ -0,0 +1,17 @@ +// 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 org.carlmontrobotics.SimpleMechs.Spinner; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class SimpleSpinner extends SubsystemBase { + /** Creates a new SimpleSpinner. */ + public SimpleSpinner() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java new file mode 100644 index 0000000..003a707 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java @@ -0,0 +1,8 @@ +package org.carlmontrobotics.SimpleMechs.Spinner; + +public final class SpinnerConfig { + + public SpinnerConfig() { + + } +} From 8b9450e829d6e08ed837a9dcf07d03eac3376695 Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Thu, 11 Dec 2025 22:08:43 -0800 Subject: [PATCH 02/10] Worked on armConfig moved everything to lib199 For some reason all the files were not in lib199 but only in carlmontrobotics --- .../SimpleMechs/Arm/ArmConfig.java | 0 .../lib199/SimpleMechs/Arm/ArmConfig.java | 246 ++++++++++++++++++ .../SimpleMechs/Arm/SimpleArm.java | 2 +- .../Drivetrain/SimpleDrivetrain.java | 47 +--- .../SimpleMechs/Elevator/ElevatorConfig.java} | 2 +- .../SimpleMechs/Elevator/SimpleElevator.java | 2 +- .../SimpleMechs/Spinner/SimpleSpinner.java | 6 +- .../SimpleMechs/Spinner/SpinnerConfig.java | 2 +- 8 files changed, 257 insertions(+), 50 deletions(-) delete mode 100644 src/main/java/org/carlmontrobotics/SimpleMechs/Arm/ArmConfig.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java rename src/main/java/org/carlmontrobotics/{ => lib199}/SimpleMechs/Arm/SimpleArm.java (89%) rename src/main/java/org/carlmontrobotics/{ => lib199}/SimpleMechs/Drivetrain/SimpleDrivetrain.java (97%) rename src/main/java/org/carlmontrobotics/{SimpleMechs/Elevator/ElevatorConfig => lib199/SimpleMechs/Elevator/ElevatorConfig.java} (56%) rename src/main/java/org/carlmontrobotics/{ => lib199}/SimpleMechs/Elevator/SimpleElevator.java (89%) rename src/main/java/org/carlmontrobotics/{ => lib199}/SimpleMechs/Spinner/SimpleSpinner.java (79%) rename src/main/java/org/carlmontrobotics/{ => lib199}/SimpleMechs/Spinner/SpinnerConfig.java (60%) diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/ArmConfig.java deleted file mode 100644 index e69de29..0000000 diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java new file mode 100644 index 0000000..37d8427 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -0,0 +1,246 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Arm; +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DriverStation; + + +public final class ArmConfig { + double armRelativeGearReduction, armAbosoluteGearReduction, armPIDTolerance, armMaxVolts; + int[] armFollowId; + MotorConfig[] armFollowMotorType; + boolean[] armFollowInversed; + + int armMasterMotorId; + MotorConfig armMasterMotorType; + boolean armMasterInversed; + + double [] armKPID, armKFeedForward; + double armKP, armKI, armKD, armKS, armKG, armKV, armKA; + + AbsoluteEncoder armMainAbsoluteEncoder; + RelativeEncoder armMainRelativeEncoder; + RelativeEncoder armBackupRelativeEncoder; + + ArmFeedforward armFeed; + PIDController armPID; + boolean armMainAbsoluteEncoderExists, armMainRelativeEncoderExists, armBackupRelativeEncoderExists, armPIDContinuousInput; + Enum armStates; + + SparkBase armMasterMotor; + + + + + + public ArmConfig( + double armRelativeGearReduction, double armAbosoluteGearReduction, + double armMaxVolts, + int[] armFollowId, MotorConfig[] armFollowMotorType, boolean[] armFollowInversed, + int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, + double[] armKPID, double armPIDTolerance, boolean armPIDContinuousInput, + double[] armKFeedForward, + AbsoluteEncoder armMainAbsoluteEncoder, int armMotorIDOfBackupRelativeEncoder, + Object armStates + ) { + + this.armRelativeGearReduction = armRelativeGearReduction; + this.armAbosoluteGearReduction = armAbosoluteGearReduction; + this.armFollowId = armFollowId; + this.armFollowMotorType = armFollowMotorType; + this.armFollowInversed = armFollowInversed; + this.armMasterMotorId = armMasterMotorId; + this.armMasterMotorType = armMasterMotorType; + this.armMasterInversed = armMasterInversed; + + this.armMainAbsoluteEncoder = armMainAbsoluteEncoder; + this.armPIDTolerance = armPIDTolerance; + this.armPIDContinuousInput = armPIDContinuousInput; + this.armMaxVolts = armMaxVolts; + this.armKPID = armKPID; + this.armKFeedForward = armKFeedForward; + + checkRequirements(armStates, armMotorIDOfBackupRelativeEncoder); + + } + + + private void checkRequirements(Object armStates, int armMotorOfBackupRelativeEncoder) { + matchMotors(); + checkMotorConfigs(); //Prevent future issues with new motors + checkEncoders(armMotorOfBackupRelativeEncoder); + checkPID(); + checkFeedForward(); + checkGearReduction(); + checkEnum(armStates); + } + + private void matchMotors() { + if (armFollowId != null) { + if (armFollowId.length == armFollowInversed.length + && armFollowId.length == armFollowMotorType.length) { + return; + } + throw new IllegalArgumentException("Motors must have matching IDs, MotorTypes and Inverse Configs"); + } + if (armMasterMotorId != -1) { + if (armMasterMotorType != null) { + return; + } + throw new IllegalArgumentException("Master motor needs a MotorConfig"); + } + throw new IllegalArgumentException("Master motor needs to exist"); + } + + private void checkMotorConfigs() { + for (MotorConfig config: armFollowMotorType) { + if (config != MotorConfig.NEO && config != MotorConfig.NEO_VORTEX && config != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + if (armMasterMotorType != MotorConfig.NEO && armMasterMotorType != MotorConfig.NEO_VORTEX && armMasterMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config???"); + } + } + + private void checkEncoders(int armMotorOfBackupRelativeEncoder) { + if (armMainAbsoluteEncoder != null) { + armMainAbsoluteEncoderExists = true; + } + + if (armMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig flexConfig = new SparkFlexConfig(); + this.armMasterMotor = MotorControllerFactory.createSparkFlex(armMasterMotorId); + flexConfig.inverted(armMasterInversed) + .idleMode(IdleMode.kBrake) + .encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second + armMasterMotor.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armMainRelativeEncoder = armMasterMotor.getEncoder(); + } + else { + SparkMaxConfig maxConfig = new SparkMaxConfig(); + armMasterMotor = MotorControllerFactory.createSparkMax(armMasterMotorId, armMasterMotorType); + maxConfig.inverted(armMasterInversed) + .idleMode(IdleMode.kBrake) + .encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second + armMasterMotor.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armMainRelativeEncoder = armMasterMotor.getEncoder(); + } + if (armMotorOfBackupRelativeEncoder != -1) { + for (int i = 0; i < armFollowId.length; i++) { + if (armFollowId[i] == armMotorOfBackupRelativeEncoder) { + if (armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + SparkFlexConfig flexConfig = new SparkFlexConfig(); + SparkFlex dummyFlex = MotorControllerFactory.createSparkFlex(armMasterMotorId); + flexConfig.encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second + dummyFlex.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armBackupRelativeEncoder = dummyFlex.getEncoder(); + } + else { + SparkMaxConfig maxConfig = new SparkMaxConfig(); + SparkMax dummyMax = MotorControllerFactory.createSparkMax(armMasterMotorId, armMasterMotorType); + maxConfig.encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second + dummyMax.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armBackupRelativeEncoder = dummyMax.getEncoder(); + } + return; + } + } + throw new IllegalArgumentException("Backup motor id of " + armMotorOfBackupRelativeEncoder + " is not part of provided motors :("); + } + else { + System.out.println("Arm does not have backup encoder, highly recommended"); + } + + + } + + private void checkPID() { + if (armKPID.length == 3) { + armKP = armKPID[0]; + armKI = armKPID[1]; + armKD = armKPID[2]; + armPID = new PIDController(armKP, armKI, armKD); + armPID.setTolerance(armPIDTolerance); + if (armPIDContinuousInput) { + armPID.enableContinuousInput(-180, 180); + } + return; + } + throw new IllegalArgumentException("Need to have 3 values for PID"); + } + + private void checkFeedForward() { + if (armKFeedForward.length == 4) { + armKS = armKFeedForward[0]; + armKG = armKFeedForward[1]; + armKV = armKFeedForward[2]; + armKA = armKFeedForward[3]; + armFeed = new ArmFeedforward(armKS, armKG, armKV, armKA); + return; + } + throw new IllegalArgumentException("Need to have 4 values for Arm FeedForward: Friction, Gravity, Voltage, and Acceleration"); + } + + private void checkGearReduction() { + if (armRelativeGearReduction == 0) { + throw new IllegalArgumentException("Gear reduction for relative encoders cannot be 0"); + } + if (armMainAbsoluteEncoderExists && armAbosoluteGearReduction == 0) { + throw new IllegalArgumentException("Absolute gear reduction cannot be 0 if absolute encoder exists"); + } + } + + private void checkEnum(Object armStates) { + Class cls = armStates.getClass(); + + if (!cls.isEnum()) { + throw new IllegalArgumentException("Expected an enum"); + } + + java.lang.reflect.Method getValueMethod; + // Ensure a getValue() method exists + try { + getValueMethod = cls.getDeclaredMethod("getValue"); + } catch (NoSuchMethodException e) { + throw new IllegalArgumentException("Enum must have a getValue() method returning the value of each state"); + } + + Object[] values = cls.getEnumConstants(); + + for (Object constant : values) { + try { + Object result = getValueMethod.invoke(constant); + + if (!(result instanceof Number)) { + throw new IllegalArgumentException( + "getValue() must return a numeric type: " + constant); + } + + double value = ((Number) result).doubleValue(); + + } catch (Exception e) { + throw new RuntimeException("Failed to read enum value", e); + } + } + this.armStates = (Enum) armStates; + } + +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java similarity index 89% rename from src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java index 33d5d87..5438a1a 100644 --- a/src/main/java/org/carlmontrobotics/SimpleMechs/Arm/SimpleArm.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java @@ -2,7 +2,7 @@ // 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 org.carlmontrobotics.SimpleMechs.Arm; +package org.carlmontrobotics.lib199.SimpleMechs.Arm; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java similarity index 97% rename from src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java index 320caa3..75791b0 100644 --- a/src/main/java/org/carlmontrobotics/SimpleMechs/Drivetrain/SimpleDrivetrain.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java @@ -1,4 +1,4 @@ -package org.carlmontrobotics.SimpleMechs.Drivetrain; +package org.carlmontrobotics.lib199.SimpleMechs.Drivetrain; import java.util.Arrays; import java.util.Map; import java.util.function.Supplier; @@ -161,51 +161,10 @@ public enum Mode { * @param trackWidth in meters */ public SimpleDrivetrain( - double wheelBase, - double trackWidth, - double wheelDiameterMeters, - double driveGearing, - double kForwardVolts, - double kBackwardVolts, - double kForwardVels, - double kForwardAccels, - double kBackwardVels, - double kBackwardAccels, - double[] drivePID, // [kP, kI, kD] - double[] turnPID, // [kP, kI, kD] - double[] turnFeedForward,//[kS, kV, kA] - double[] turnZeroDeg, //(FL, FR, BL, BR) - boolean[] driveInversion, //(FL, FR, BL, BR) - boolean[] reversed, //FIXME what does this do (FL, FR, BL, BR) - double driveModifier, //Max speed applier - boolean[] turnInversion //(FL, FR, BL, BR) + SwerveConfig swerveconfig ) { - swerveConfig = new SwerveConfig( - wheelDiameterMeters, - driveGearing, - mu, - autoCentripetalAccel, - kForwardVolts, - kForwardVels, - kForwardAccels, - kBackwardVolts, - kBackwardVels, - kBackwardAccels, - drivePID[0], - drivePID[1], - drivePID[2], - turnPID[0], - turnPID[1], - turnPID[2], - turnFeedForward[0], - turnFeedForward[1], - turnFeedForward[2], - turnZeroDeg, - driveInversion, - reversed, - driveModifier, - turnInversion); + this.swerveConfig = swerveConfig; AutoBuilder(); diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java similarity index 56% rename from src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java index a689dfd..c7e1633 100644 --- a/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/ElevatorConfig +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java @@ -1,4 +1,4 @@ -package org.carlmontrobotics.lib199.simplemechs.elevator; +package org.carlmontrobotics.lib199.SimpleMechs.Elevator; public final class ElevatorConfig { diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java similarity index 89% rename from src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java index a69f3f0..94e1c47 100644 --- a/src/main/java/org/carlmontrobotics/SimpleMechs/Elevator/SimpleElevator.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java @@ -2,7 +2,7 @@ // 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 org.carlmontrobotics.SimpleMechs.Elevator; +package org.carlmontrobotics.lib199.SimpleMechs.Elevator; import edu.wpi.first.wpilibj2.command.SubsystemBase; diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java similarity index 79% rename from src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java index 288e31f..5d19480 100644 --- a/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SimpleSpinner.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java @@ -2,13 +2,15 @@ // 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 org.carlmontrobotics.SimpleMechs.Spinner; +package org.carlmontrobotics.lib199.SimpleMechs.Spinner; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SimpleSpinner extends SubsystemBase { /** Creates a new SimpleSpinner. */ - public SimpleSpinner() {} + public SimpleSpinner(SpinnerConfig spinnerConfig) { + + } @Override public void periodic() { diff --git a/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java similarity index 60% rename from src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java rename to src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java index 003a707..22ec30d 100644 --- a/src/main/java/org/carlmontrobotics/SimpleMechs/Spinner/SpinnerConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java @@ -1,4 +1,4 @@ -package org.carlmontrobotics.SimpleMechs.Spinner; +package org.carlmontrobotics.lib199.SimpleMechs.Spinner; public final class SpinnerConfig { From 6e7ea466e31d8334494ccb1caa7efffbbd1d6952 Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Sun, 14 Dec 2025 19:48:10 -0800 Subject: [PATCH 03/10] Arm Complete Removed state enum cause did not able to work nicely how I thought --- .../lib199/SimpleMechs/Arm/ArmConfig.java | 337 ++++++++++++++---- .../lib199/SimpleMechs/Arm/SimpleArm.java | 306 +++++++++++++++- 2 files changed, 574 insertions(+), 69 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java index 37d8427..6f027f9 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -15,11 +15,10 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.wpilibj.DriverStation; public final class ArmConfig { - double armRelativeGearReduction, armAbosoluteGearReduction, armPIDTolerance, armMaxVolts; + double armRelativeGearReduction, armAbsoluteGearReduction, armPIDTolerance, armMaxVolts; int[] armFollowId; MotorConfig[] armFollowMotorType; boolean[] armFollowInversed; @@ -35,30 +34,58 @@ public final class ArmConfig { RelativeEncoder armMainRelativeEncoder; RelativeEncoder armBackupRelativeEncoder; - ArmFeedforward armFeed; + ArmFeedforward armFeedForward; PIDController armPID; - boolean armMainAbsoluteEncoderExists, armMainRelativeEncoderExists, armBackupRelativeEncoderExists, armPIDContinuousInput; - Enum armStates; + boolean armMainAbsoluteEncoderExists, armMainRelativeEncoderExists, armBackupRelativeEncoderExists, armPIDContinuousInput, armPIDExists, armFeedForwardExists; + //Enum armStates; SparkBase armMasterMotor; - + double armAbsoluteZeroOffset; + double bottomLimit; + double topLimit; + + //@param armStates provide an enum State of different positions the arm should be at, null if no state machine + /** + * Creates a ArmConfig for a SimpleArm + * @param armRelativeGearReduction Gear reduction from shaft spinning where motors are to the arm axis + * @param armAbsoluteGearReduction Gear reduction from absolute encoder reading to the arm axis + * @param armMaxVolts Max volts that motors can output on to the arm, 14 if no limit + * @param armFollowId List all motorIds for followers + * @param armFollowMotorType List all motor configs for the followers in the same order as above + * @param armFollowInversed List all inverse configs for the followers in the same order as above + * @param armMasterMotorId Master id + * @param armMasterMotorType Motor Config of Master + * @param armMasterInversed Inverse Config of Master + * @param armKPID (kP, kI, kD) of arm + * @param armPIDTolerance Tolerance (in degrees) of arm + * @param armPIDContinuousInput True if arm can do infinite 360s, keep false otherwise! + * @param armKFeedForward (kS, kG, kV, kA) of arm + * @param armMainAbsoluteEncoder null if no absolute + * @param armMotorIDOfBackupRelativeEncoder -1 if none + * @param armAbsoluteZeroOffset zeroOffset (in degrees) of abs enc, 0 should always be down! + * @param bottomLimit lowest value the arm can achieve (soft limit) + * @param topLimit highest value the arm can achieve (soft limit) + */ public ArmConfig( - double armRelativeGearReduction, double armAbosoluteGearReduction, + double armRelativeGearReduction, double armAbsoluteGearReduction, double armMaxVolts, int[] armFollowId, MotorConfig[] armFollowMotorType, boolean[] armFollowInversed, int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double[] armKPID, double armPIDTolerance, boolean armPIDContinuousInput, double[] armKFeedForward, AbsoluteEncoder armMainAbsoluteEncoder, int armMotorIDOfBackupRelativeEncoder, - Object armStates + //Object armStates, + double armAbsoluteZeroOffset, + double bottomLimit, + double topLimit ) { this.armRelativeGearReduction = armRelativeGearReduction; - this.armAbosoluteGearReduction = armAbosoluteGearReduction; + this.armAbsoluteGearReduction = armAbsoluteGearReduction; this.armFollowId = armFollowId; this.armFollowMotorType = armFollowMotorType; this.armFollowInversed = armFollowInversed; @@ -72,22 +99,148 @@ public ArmConfig( this.armMaxVolts = armMaxVolts; this.armKPID = armKPID; this.armKFeedForward = armKFeedForward; + this.armAbsoluteZeroOffset = armAbsoluteZeroOffset; + this.bottomLimit = bottomLimit; + this.topLimit = topLimit; + + checkRequirements( + //armStates, + armMotorIDOfBackupRelativeEncoder); + + } + + + double defaultArmPIDTolerance = 2; + + /** + * Create a single motor arm config with PID, no FeedForward + * @param armMasterMotorId + * @param armMasterMotorType + * @param armMasterInversed + * @param armKPID + * @param armStates + * @param bottomLimit + * @param topLimit + * @return + */ + public ArmConfig SimpleArmConfig(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double[] armKPID, + //Object armStates, + double bottomLimit, double topLimit) { + return new ArmConfig(1, 1, 14, null, null, null, armMasterMotorId, armMasterMotorType, armMasterInversed, armKPID, defaultArmPIDTolerance, false, null, null, -1, + //armStates, + 0, bottomLimit, topLimit); + } + + /** + * Create a single motor arm config no PID, no FeedForward + * @param armMasterMotorId + * @param armMasterMotorType + * @param armMasterInversed + * @param armStates + * @param bottomLimit + * @param topLimit + * @return + */ + public ArmConfig SimpleArmConfig(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, + //Object armStates, + double bottomLimit, double topLimit) { + return SimpleArmConfig(armMasterMotorId, armMasterMotorType, armMasterInversed, null, + //armStates, + bottomLimit, topLimit); + } - checkRequirements(armStates, armMotorIDOfBackupRelativeEncoder); + /** + * Creates a single NEO arm config with PID + * @param armMasterMotorId + * @param armMasterInversed + * @param armKPID + * @param armStates + * @param bottomLimit + * @param topLimit + * @return + */ + public ArmConfig SimpleNeoArmConfig(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, + //Object armStates, + double bottomLimit, double topLimit) { + return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO, armMasterInversed, armKPID, + //armStates, + bottomLimit, topLimit); + } + /** + * Create a single NEO arm config without PID + * @param armMasterMotorId + * @param armMasterInversed + * @param armStates + * @param bottomLimit + * @param topLimit + * @return + */ + public ArmConfig SimpleNeoArmConfig(int armMasterMotorId, boolean armMasterInversed, + //Object armStates, + double bottomLimit, double topLimit) { + return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO, armMasterInversed, + //armStates, + bottomLimit, topLimit); } + /** + * Createa a single vortex arm config, with PID + * @param armMasterMotorId + * @param armMasterInversed + * @param armKPID + * @param armStates + * @param bottomLimit + * @param topLimit + * @return + */ + public ArmConfig SimpleVortexArmConfig(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, + //Object armStates, + double bottomLimit, double topLimit) { + return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, armKPID, + //armStates, + bottomLimit, topLimit); + } + + /** + * Create a single vortex arm config, no PID + * @param armMasterMotorId + * @param armMasterInversed + * @param armStates + * @param bottomLimit + * @param topLimit + * @return + */ + public ArmConfig SimpleVortexArmConfig(int armMasterMotorId, boolean armMasterInversed, + //Object armStates, + double bottomLimit, double topLimit) { + return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, + //armStates, + bottomLimit, topLimit); + } - private void checkRequirements(Object armStates, int armMotorOfBackupRelativeEncoder) { + /** + * Check that arguments are good and set some of the parameters of the config. + * @param armStates give stateEnum + * @param armMotorOfBackupRelativeEncoderId give motor Id + */ + private void checkRequirements( + //Object armStates, + int armMotorOfBackupRelativeEncoderId) { matchMotors(); checkMotorConfigs(); //Prevent future issues with new motors - checkEncoders(armMotorOfBackupRelativeEncoder); + checkEncoders(armMotorOfBackupRelativeEncoderId); checkPID(); checkFeedForward(); checkGearReduction(); - checkEnum(armStates); + //checkEnum(armStates); + checkVolts(); + checkLimits(); } + /** + * Make sure that each corresponding motor id has a MotorConfig and inverse config + */ private void matchMotors() { if (armFollowId != null) { if (armFollowId.length == armFollowInversed.length @@ -105,6 +258,9 @@ private void matchMotors() { throw new IllegalArgumentException("Master motor needs to exist"); } + /** + * Check that the motor Configs are not any new configs that would not work or null. + */ private void checkMotorConfigs() { for (MotorConfig config: armFollowMotorType) { if (config != MotorConfig.NEO && config != MotorConfig.NEO_VORTEX && config != MotorConfig.NEO_550) { @@ -116,10 +272,17 @@ private void checkMotorConfigs() { } } - private void checkEncoders(int armMotorOfBackupRelativeEncoder) { + /** + * + * @param armMotorOfBackupRelativeEncoderId + */ + private void checkEncoders(int armMotorOfBackupRelativeEncoderId) { if (armMainAbsoluteEncoder != null) { armMainAbsoluteEncoderExists = true; } + else { + armMainAbsoluteEncoderExists = false; + } if (armMasterMotorType == MotorConfig.NEO_VORTEX) { SparkFlexConfig flexConfig = new SparkFlexConfig(); @@ -141,9 +304,10 @@ private void checkEncoders(int armMotorOfBackupRelativeEncoder) { armMasterMotor.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); armMainRelativeEncoder = armMasterMotor.getEncoder(); } - if (armMotorOfBackupRelativeEncoder != -1) { + armMainRelativeEncoderExists = true; + if (armMotorOfBackupRelativeEncoderId != -1) { for (int i = 0; i < armFollowId.length; i++) { - if (armFollowId[i] == armMotorOfBackupRelativeEncoder) { + if (armFollowId[i] == armMotorOfBackupRelativeEncoderId) { if (armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { SparkFlexConfig flexConfig = new SparkFlexConfig(); SparkFlex dummyFlex = MotorControllerFactory.createSparkFlex(armMasterMotorId); @@ -160,12 +324,14 @@ private void checkEncoders(int armMotorOfBackupRelativeEncoder) { dummyMax.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); armBackupRelativeEncoder = dummyMax.getEncoder(); } + armMainRelativeEncoderExists = true; return; } } - throw new IllegalArgumentException("Backup motor id of " + armMotorOfBackupRelativeEncoder + " is not part of provided motors :("); + throw new IllegalArgumentException("Backup motor id of " + armMotorOfBackupRelativeEncoderId + " is not part of provided motors :("); } else { + armMainRelativeEncoderExists = false; System.out.println("Arm does not have backup encoder, highly recommended"); } @@ -173,74 +339,111 @@ private void checkEncoders(int armMotorOfBackupRelativeEncoder) { } private void checkPID() { - if (armKPID.length == 3) { - armKP = armKPID[0]; - armKI = armKPID[1]; - armKD = armKPID[2]; - armPID = new PIDController(armKP, armKI, armKD); - armPID.setTolerance(armPIDTolerance); - if (armPIDContinuousInput) { - armPID.enableContinuousInput(-180, 180); + if (armKPID != null) { + if (armKPID.length == 3) { + armKP = armKPID[0]; + armKI = armKPID[1]; + armKD = armKPID[2]; + armPID = new PIDController(armKP, armKI, armKD); + armPID.setTolerance(armPIDTolerance); + if (armPIDContinuousInput) { + armPID.enableContinuousInput(-180, 180); + } + armPIDExists = false; + return; } - return; + throw new IllegalArgumentException("Need to have 3 values for PID"); } - throw new IllegalArgumentException("Need to have 3 values for PID"); + armKP = 0; + armKI = 0; + armKD = 0; + armPIDExists = false; + System.out.println("ArmPID is off"); } private void checkFeedForward() { - if (armKFeedForward.length == 4) { - armKS = armKFeedForward[0]; - armKG = armKFeedForward[1]; - armKV = armKFeedForward[2]; - armKA = armKFeedForward[3]; - armFeed = new ArmFeedforward(armKS, armKG, armKV, armKA); - return; - } - throw new IllegalArgumentException("Need to have 4 values for Arm FeedForward: Friction, Gravity, Voltage, and Acceleration"); + if (armKFeedForward != null) { + if (armKFeedForward.length == 4) { + armKS = armKFeedForward[0]; + armKG = armKFeedForward[1]; + armKV = armKFeedForward[2]; + armKA = armKFeedForward[3]; + armFeedForward = new ArmFeedforward(armKS, armKG, armKV, armKA); + armFeedForwardExists = true; + return; + } + throw new IllegalArgumentException("Need to have 4 values for Arm FeedForward: Friction, Gravity, Voltage, and Acceleration"); + } + armKS = 0; + armKG = 0; + armKV = 0; + armFeedForwardExists = false; + System.out.println("ArmFeedForward is off"); } private void checkGearReduction() { if (armRelativeGearReduction == 0) { throw new IllegalArgumentException("Gear reduction for relative encoders cannot be 0"); } - if (armMainAbsoluteEncoderExists && armAbosoluteGearReduction == 0) { + if (armMainAbsoluteEncoderExists && armAbsoluteGearReduction == 0) { throw new IllegalArgumentException("Absolute gear reduction cannot be 0 if absolute encoder exists"); } } - private void checkEnum(Object armStates) { - Class cls = armStates.getClass(); - - if (!cls.isEnum()) { - throw new IllegalArgumentException("Expected an enum"); - } + // private void checkEnum(Object armStates) { + // if (armStates != null) { + // Class cls = armStates.getClass(); + + // if (!cls.isEnum()) { + // throw new IllegalArgumentException("Expected an enum"); + // } + + // java.lang.reflect.Method getValueMethod; + // // Ensure a getValue() method exists + // try { + // getValueMethod = cls.getDeclaredMethod("getValue"); + // } catch (NoSuchMethodException e) { + // throw new IllegalArgumentException("Enum must have a getValue() method returning the value of each state"); + // } - java.lang.reflect.Method getValueMethod; - // Ensure a getValue() method exists - try { - getValueMethod = cls.getDeclaredMethod("getValue"); - } catch (NoSuchMethodException e) { - throw new IllegalArgumentException("Enum must have a getValue() method returning the value of each state"); + // Object[] values = cls.getEnumConstants(); + + // for (Object constant : values) { + // try { + // Object result = getValueMethod.invoke(constant); + + // if (!(result instanceof Number)) { + // throw new IllegalArgumentException( + // "getValue() must return a numeric type: " + constant); + // } + + // double value = ((Number) result).doubleValue(); + // if (value > topLimit || value < bottomLimit) { + // throw new IllegalArgumentException("Enum Position cannot be outside of the bounding limits"); + // } + + // } catch (Exception e) { + // throw new RuntimeException("Failed to read enum value", e); + // } + // } + // this.armStates = (Enum) armStates; + // } + // else { + // System.out.println("Daniel is sad that you don't like states"); + // } + // } + + private void checkVolts() { + if (armMaxVolts <= 0) { + throw new IllegalArgumentException("armMaxVolts must be a positive number."); } - - Object[] values = cls.getEnumConstants(); - - for (Object constant : values) { - try { - Object result = getValueMethod.invoke(constant); - - if (!(result instanceof Number)) { - throw new IllegalArgumentException( - "getValue() must return a numeric type: " + constant); - } - - double value = ((Number) result).doubleValue(); - - } catch (Exception e) { - throw new RuntimeException("Failed to read enum value", e); - } + return; + } + private void checkLimits() { + if (bottomLimit >= topLimit) { + throw new IllegalArgumentException("Top limit must be much greater than bottom limit. (Cannot be equal)"); } - this.armStates = (Enum) armStates; } + } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java index 5438a1a..e8e83ab 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java @@ -4,14 +4,316 @@ package org.carlmontrobotics.lib199.SimpleMechs.Arm; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkFlex; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SimpleArm extends SubsystemBase { /** Creates a new SimpleArm. */ - public SimpleArm() {} + ArmConfig armConfig; + SparkBase armMasterMotor; + RelativeEncoder armFeedbackEncoder; + AbsoluteEncoder armResetEncoder; + RelativeEncoder armBackupEncoder; + SparkBase[] armFollowMotors; + double currentDif = 0; + double pastDif = 0; + final double difDanger = 1; + + double armGoal; // in degrees + double latestManualInput; // in percentage of power (-1, 1) + + ArmControlState currentState = ArmControlState.AUTO; + IdleMode armIdleMode = IdleMode.kBrake; + + Timer armTimer = new Timer(); + + public enum ArmControlState { + AUTO, + MANUAL, + BRAKE, + COAST + + } + + public SimpleArm(ArmConfig armConfig) { + this.armConfig = armConfig; + configMotors(); + resetFeedbackEncoder(); + } + + public void setGoal(double goal){ + if (goal <= armConfig.topLimit && goal >= armConfig.bottomLimit) {armGoal = goal;} + } + + public void setManualInput(double input) { + latestManualInput = input; + } + + public void toggleControlMode() { + if (currentState == ArmControlState.AUTO) { + currentState = ArmControlState.MANUAL; + } + else if (currentState == ArmControlState.MANUAL) { + currentState = ArmControlState.AUTO; + } + } + + public void setArmControlState(ArmControlState controlState) { + switch (controlState) { + case AUTO -> setAdjustingOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> System.out.println("Such control mode has not been implemented yet"); + } + } + + public void setAdjustingOn() { + if (armConfig.armPIDExists || armConfig.armFeedForwardExists) { + currentState = ArmControlState.AUTO; + } + else { + System.out.println("Any sort of autonomous control mode is disabled due to no PID or FeedForward"); + } + } + + public void setManualOn() { + currentState = ArmControlState.MANUAL; + } + + public void setBrakeOn() { + currentState = ArmControlState.BRAKE; + if (armIdleMode == IdleMode.kCoast) { + armIdleMode = IdleMode.kBrake; + brakeMotors(); + + } + } + + public void setCoastOn() { + currentState = ArmControlState.COAST; + if (armIdleMode == IdleMode.kBrake) { + armIdleMode = IdleMode.kCoast; + coastMotors(); + } + } + + private void resetFeedbackEncoder() { + if (armConfig.armMainAbsoluteEncoderExists) { + armFeedbackEncoder.setPosition((armResetEncoder.getPosition() + armConfig.armAbsoluteZeroOffset)/armConfig.armAbsoluteGearReduction*armConfig.armRelativeGearReduction); + } + else { + armFeedbackEncoder.setPosition(0); + } + } + private void checkFeedBackEncoder() { + pastDif = currentDif; + currentDif = armFeedbackEncoder.getPosition() - armBackupEncoder.getPosition(); + if (currentDif - pastDif > difDanger) { + System.out.println("Arm encoder seems to be off!"); + } + } + + + private void autoArm() { + double pidOutput; + double feedforwardOutput; + double curPosition = armFeedbackEncoder.getPosition(); + if (armConfig.armPIDExists) { + pidOutput = armConfig.armPID.calculate(curPosition, armGoal); + } + else { + pidOutput = 0; + } + + if (armConfig.armFeedForwardExists) { + feedforwardOutput = armConfig.armFeedForward.calculate(armGoal/360*2*Math.PI + Math.PI/2, 0); + } + else { + feedforwardOutput = 0; + } + if (curPosition < armConfig.topLimit) { + if (curPosition > armConfig.bottomLimit) { + armMasterMotor.set(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, armConfig.armMaxVolts)); + } + else { + armMasterMotor.set(MathUtil.clamp(pidOutput + feedforwardOutput, 0, armConfig.armMaxVolts)); + } + } + else { + armMasterMotor.set(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, 0)); + } + } + + private void manualArm() { + double curPosition = armFeedbackEncoder.getPosition(); + if (curPosition < armConfig.topLimit) { + if (curPosition > armConfig.bottomLimit) { + armMasterMotor.set(MathUtil.clamp(latestManualInput, -armConfig.armMaxVolts/12, armConfig.armMaxVolts/12)); + } + else { + armMasterMotor.set(MathUtil.clamp(latestManualInput, 0, armConfig.armMaxVolts/12)); + } + } + else { + armMasterMotor.set(MathUtil.clamp(latestManualInput, -armConfig.armMaxVolts/12, 0)); + } + } + + private void configMotors() { + configFollowMotors(); + configMasterMotor(); + } + + private void configFollowMotors() { + if (armConfig.armFollowId != null) { + armFollowMotors = new SparkBase[armConfig.armFollowId.length]; + + SparkFlexConfig flexFollowConfig = new SparkFlexConfig(); + flexFollowConfig.idleMode(IdleMode.kBrake); + SparkMaxConfig maxFollowConfig = new SparkMaxConfig(); + maxFollowConfig.idleMode(IdleMode.kBrake); + + for (int i = 0; i < armConfig.armFollowId.length; i++) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + flexFollowConfig.follow(armConfig.armMasterMotorId, !(armConfig.armMasterInversed && armConfig.armFollowInversed[i])); + SparkFlex followFlex = MotorControllerFactory.createSparkFlex(armConfig.armFollowId[i]); + followFlex.configure(flexFollowConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFollowMotors[i] = followFlex; + } + else { + maxFollowConfig.follow(armConfig.armMasterMotorId, !(armConfig.armMasterInversed && armConfig.armFollowInversed[i])); + SparkMax followMax = MotorControllerFactory.createSparkMax(armConfig.armFollowId[i], armConfig.armFollowMotorType[i]); + followMax.configure(maxFollowConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFollowMotors[i] = followMax; + } + } + } + } + + private void configMasterMotor() { + if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig flexMasterConfig = new SparkFlexConfig(); + flexMasterConfig.inverted(armConfig.armMasterInversed); + flexMasterConfig.idleMode(IdleMode.kBrake); + armMasterMotor = MotorControllerFactory.createSparkFlex(armConfig.armMasterMotorId); + armMasterMotor.configure(flexMasterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig maxMasterConfig = new SparkMaxConfig(); + maxMasterConfig.inverted(armConfig.armMasterInversed); + maxMasterConfig.idleMode(IdleMode.kBrake); + armMasterMotor = MotorControllerFactory.createSparkMax(armConfig.armMasterMotorId, armConfig.armMasterMotorType); + armMasterMotor.configure(maxMasterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to brake + */ + private void brakeMotors() { + SparkFlexConfig flexBrakeConfig = new SparkFlexConfig(); + SparkMaxConfig maxBrakeConfig = new SparkMaxConfig(); + + flexBrakeConfig.idleMode(IdleMode.kBrake); + maxBrakeConfig.idleMode(IdleMode.kBrake); + + if (armConfig.armFollowId != null) { + for (int i = 0; i < armFollowMotors.length; i++) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + armFollowMotors[i].configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armFollowMotors[i].configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + } + if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { + armMasterMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armMasterMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to coast + */ + private void coastMotors() { + SparkFlexConfig flexCoastConfig = new SparkFlexConfig(); + SparkMaxConfig maxCoastConfig = new SparkMaxConfig(); + + flexCoastConfig.idleMode(IdleMode.kCoast); + maxCoastConfig.idleMode(IdleMode.kCoast); + + if (armConfig.armFollowId != null) { + for (int i = 0; i < armFollowMotors.length; i++) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + armFollowMotors[i].configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armFollowMotors[i].configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + } + if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { + armMasterMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + armMasterMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Use this to do any fun stuff you want to do, for armSubsystemPeriodic + */ + private void userPeriodic() { + + } + + private void postSmartDashboardValues() { + SmartDashboard.putData(this); + SmartDashboard.putNumber("armVelocity", armFeedbackEncoder.getVelocity()); + SmartDashboard.putNumber("armPositionDegrees", armFeedbackEncoder.getPosition()); //From vertical down 0 + switch (currentState) { + case AUTO -> SmartDashboard.putString("armControlState", "Autonomous"); + case MANUAL -> SmartDashboard.putString("armControlState", "Manual"); + case BRAKE -> SmartDashboard.putString("armControlState", "Idling: Brake"); + case COAST -> SmartDashboard.putString("armControlState", "Idling: Coast"); + } + SmartDashboard.putNumber("armGoal", armGoal); + + } + + /** + * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! + * Use {@link #userPeriodic()} instead. + */ @Override public void periodic() { - // This method will be called once per scheduler run + if (currentState == ArmControlState.AUTO) {autoArm();} + else if (currentState == ArmControlState.MANUAL) {manualArm();} + checkFeedBackEncoder(); + userPeriodic(); + postSmartDashboardValues(); } + } From 4bd05845082607d331265ef5ad2536b219499110 Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Sun, 14 Dec 2025 20:14:08 -0800 Subject: [PATCH 04/10] Arm reading change --- .../carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java | 2 +- .../carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java | 6 ++++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java index 6f027f9..4b4ad7f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -66,7 +66,7 @@ public final class ArmConfig { * @param armKFeedForward (kS, kG, kV, kA) of arm * @param armMainAbsoluteEncoder null if no absolute * @param armMotorIDOfBackupRelativeEncoder -1 if none - * @param armAbsoluteZeroOffset zeroOffset (in degrees) of abs enc, 0 should always be down! + * @param armAbsoluteZeroOffset zeroOffset (in degrees) of abs enc, 0 should always be horizontal! * @param bottomLimit lowest value the arm can achieve (soft limit) * @param topLimit highest value the arm can achieve (soft limit) */ diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java index e8e83ab..7f4968d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java @@ -44,6 +44,8 @@ public class SimpleArm extends SubsystemBase { Timer armTimer = new Timer(); + private final double defaultRelativeEncoderResetValue = -90; + public enum ArmControlState { AUTO, MANUAL, @@ -120,7 +122,7 @@ private void resetFeedbackEncoder() { armFeedbackEncoder.setPosition((armResetEncoder.getPosition() + armConfig.armAbsoluteZeroOffset)/armConfig.armAbsoluteGearReduction*armConfig.armRelativeGearReduction); } else { - armFeedbackEncoder.setPosition(0); + armFeedbackEncoder.setPosition(defaultRelativeEncoderResetValue); } } @@ -145,7 +147,7 @@ private void autoArm() { } if (armConfig.armFeedForwardExists) { - feedforwardOutput = armConfig.armFeedForward.calculate(armGoal/360*2*Math.PI + Math.PI/2, 0); + feedforwardOutput = armConfig.armFeedForward.calculate(armGoal/360*2*Math.PI, 0); } else { feedforwardOutput = 0; From a1b114b24ad83fa51506a42fdc2c7c9744035b1c Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Sun, 14 Dec 2025 20:15:39 -0800 Subject: [PATCH 05/10] Fixed docs --- .../carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java index 4b4ad7f..5e60142 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -118,7 +118,6 @@ public ArmConfig( * @param armMasterMotorType * @param armMasterInversed * @param armKPID - * @param armStates * @param bottomLimit * @param topLimit * @return @@ -136,7 +135,6 @@ public ArmConfig SimpleArmConfig(int armMasterMotorId, MotorConfig armMasterMoto * @param armMasterMotorId * @param armMasterMotorType * @param armMasterInversed - * @param armStates * @param bottomLimit * @param topLimit * @return @@ -154,7 +152,6 @@ public ArmConfig SimpleArmConfig(int armMasterMotorId, MotorConfig armMasterMoto * @param armMasterMotorId * @param armMasterInversed * @param armKPID - * @param armStates * @param bottomLimit * @param topLimit * @return @@ -171,7 +168,6 @@ public ArmConfig SimpleNeoArmConfig(int armMasterMotorId, boolean armMasterInver * Create a single NEO arm config without PID * @param armMasterMotorId * @param armMasterInversed - * @param armStates * @param bottomLimit * @param topLimit * @return @@ -189,7 +185,6 @@ public ArmConfig SimpleNeoArmConfig(int armMasterMotorId, boolean armMasterInver * @param armMasterMotorId * @param armMasterInversed * @param armKPID - * @param armStates * @param bottomLimit * @param topLimit * @return @@ -206,7 +201,6 @@ public ArmConfig SimpleVortexArmConfig(int armMasterMotorId, boolean armMasterIn * Create a single vortex arm config, no PID * @param armMasterMotorId * @param armMasterInversed - * @param armStates * @param bottomLimit * @param topLimit * @return @@ -221,7 +215,6 @@ public ArmConfig SimpleVortexArmConfig(int armMasterMotorId, boolean armMasterIn /** * Check that arguments are good and set some of the parameters of the config. - * @param armStates give stateEnum * @param armMotorOfBackupRelativeEncoderId give motor Id */ private void checkRequirements( From 421b3a39016729c452973cc21f93ac80b4fec6b2 Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Mon, 22 Dec 2025 13:56:22 -0800 Subject: [PATCH 06/10] Fixy drivetrain and made elevator slightly --- .../lib199/SimpleMechs/Arm/ArmConfig.java | 14 +- .../lib199/SimpleMechs/Arm/SimpleArm.java | 60 ++-- .../Drivetrain/SimpleDrivetrain.java | 84 +++++- .../SimpleRotateToFieldRelativeAngle.java | 0 .../Drivetrain/SimpleTeleopDrive.java | 144 +++++++++ .../SimpleMechs/Elevator/ElevatorConfig.java | 136 ++++++++- .../SimpleMechs/Elevator/SimpleElevator.java | 276 +++++++++++++++++- 7 files changed, 674 insertions(+), 40 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java index 5e60142..fda42aa 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -15,6 +15,7 @@ import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DriverStation; public final class ArmConfig { @@ -261,7 +262,7 @@ private void checkMotorConfigs() { } } if (armMasterMotorType != MotorConfig.NEO && armMasterMotorType != MotorConfig.NEO_VORTEX && armMasterMotorType != MotorConfig.NEO_550) { - throw new IllegalArgumentException("What is this config???"); + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); } } @@ -325,7 +326,7 @@ private void checkEncoders(int armMotorOfBackupRelativeEncoderId) { } else { armMainRelativeEncoderExists = false; - System.out.println("Arm does not have backup encoder, highly recommended"); + DriverStation.reportWarning("Arm does not have backup encoder, highly recommended", true); } @@ -342,7 +343,7 @@ private void checkPID() { if (armPIDContinuousInput) { armPID.enableContinuousInput(-180, 180); } - armPIDExists = false; + armPIDExists = true; return; } throw new IllegalArgumentException("Need to have 3 values for PID"); @@ -351,7 +352,7 @@ private void checkPID() { armKI = 0; armKD = 0; armPIDExists = false; - System.out.println("ArmPID is off"); + DriverStation.reportWarning("ArmPID is off", true); } private void checkFeedForward() { @@ -370,8 +371,9 @@ private void checkFeedForward() { armKS = 0; armKG = 0; armKV = 0; + armKA = 0; armFeedForwardExists = false; - System.out.println("ArmFeedForward is off"); + DriverStation.reportWarning("ArmFeedForward is off", true); } private void checkGearReduction() { @@ -422,7 +424,7 @@ private void checkGearReduction() { // this.armStates = (Enum) armStates; // } // else { - // System.out.println("Daniel is sad that you don't like states"); + // DriverStation.reportWarning("Daniel is sad that you don't like states", true); // } // } diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java index 7f4968d..106e590 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java @@ -20,29 +20,30 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SimpleArm extends SubsystemBase { /** Creates a new SimpleArm. */ - ArmConfig armConfig; - SparkBase armMasterMotor; - RelativeEncoder armFeedbackEncoder; - AbsoluteEncoder armResetEncoder; - RelativeEncoder armBackupEncoder; - SparkBase[] armFollowMotors; - double currentDif = 0; - double pastDif = 0; - final double difDanger = 1; + private ArmConfig armConfig; + private SparkBase armMasterMotor; + private RelativeEncoder armFeedbackEncoder; + private AbsoluteEncoder armResetEncoder; + private RelativeEncoder armBackupEncoder; + private SparkBase[] armFollowMotors; + private double currentDif = 0; + private double pastDif = 0; + private final double difDanger = 1; - double armGoal; // in degrees - double latestManualInput; // in percentage of power (-1, 1) + private double armGoal; // in degrees + private double latestManualInput; // in percentage of power (-1, 1) - ArmControlState currentState = ArmControlState.AUTO; - IdleMode armIdleMode = IdleMode.kBrake; + private ArmControlState currentState = ArmControlState.AUTO; + private IdleMode armIdleMode = IdleMode.kBrake; - Timer armTimer = new Timer(); + private Timer armTimer = new Timer(); private final double defaultRelativeEncoderResetValue = -90; @@ -51,7 +52,6 @@ public enum ArmControlState { MANUAL, BRAKE, COAST - } public SimpleArm(ArmConfig armConfig) { @@ -64,10 +64,18 @@ public void setGoal(double goal){ if (goal <= armConfig.topLimit && goal >= armConfig.bottomLimit) {armGoal = goal;} } + public double getGoal() { + return armGoal; + } + public void setManualInput(double input) { latestManualInput = input; } + public double getLatestManualInput() { + return latestManualInput; + } + public void toggleControlMode() { if (currentState == ArmControlState.AUTO) { currentState = ArmControlState.MANUAL; @@ -79,20 +87,20 @@ else if (currentState == ArmControlState.MANUAL) { public void setArmControlState(ArmControlState controlState) { switch (controlState) { - case AUTO -> setAdjustingOn(); + case AUTO -> setAutoOn(); case MANUAL -> setManualOn(); case BRAKE -> setBrakeOn(); case COAST -> setCoastOn(); - default -> System.out.println("Such control mode has not been implemented yet"); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); } } - public void setAdjustingOn() { + public void setAutoOn() { if (armConfig.armPIDExists || armConfig.armFeedForwardExists) { currentState = ArmControlState.AUTO; } else { - System.out.println("Any sort of autonomous control mode is disabled due to no PID or FeedForward"); + DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true); } } @@ -130,7 +138,7 @@ private void checkFeedBackEncoder() { pastDif = currentDif; currentDif = armFeedbackEncoder.getPosition() - armBackupEncoder.getPosition(); if (currentDif - pastDif > difDanger) { - System.out.println("Arm encoder seems to be off!"); + DriverStation.reportWarning("Arm encoder seems to be off!", true); } } @@ -284,6 +292,10 @@ private void coastMotors() { } } + public void stopArm() { + armMasterMotor.set(0); + } + /** * Use this to do any fun stuff you want to do, for armSubsystemPeriodic */ @@ -311,8 +323,12 @@ private void postSmartDashboardValues() { */ @Override public void periodic() { - if (currentState == ArmControlState.AUTO) {autoArm();} - else if (currentState == ArmControlState.MANUAL) {manualArm();} + switch (currentState) { + case AUTO -> autoArm(); + case MANUAL -> manualArm(); + case BRAKE -> stopArm(); + case COAST -> stopArm(); + } checkFeedBackEncoder(); userPeriodic(); postSmartDashboardValues(); diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java index 75791b0..7bed80d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java @@ -155,16 +155,82 @@ public enum Mode { double mu = 1; double autoCentripetalAccel = mu * g * 2; + + //Extra + double wheelBase; + double trackWidth; + + int driveFrontLeftPort; + int turnFrontLeftPort; + int canCoderPortFL; + + int driveFrontRightPort; + int turnFrontRightPort; + int canCoderPortFR; + + int driveBackLeftPort; + int turnBackLeftPort; + int canCoderPortBL; + + int driveBackRightPort; + int turnBackRightPort; + int canCoderPortBR; + + double secsPer12Volts; + double wheelDiameterMeters; + double driveGearing; + double turnGearing; + + boolean isGyroReversed; + double maxSpeed; + + RobotConfig robotConfig; + + double COLLISION_ACCELERATION_THRESHOLD; + + + /** * * @param wheelBase in meters * @param trackWidth in meters */ public SimpleDrivetrain( - SwerveConfig swerveconfig + SwerveConfig swerveConfig, + double wheelBase, double trackWidth, + int driveFrontLeftPort, int turnFrontLeftPort, int canCoderPortFL, + int driveFrontRightPort, int turnFrontRightPort, int canCoderPortFR, + int driveBackLeftPort, int turnBackLeftPort, int canCoderPortBL, + int driveBackRightPort, int turnBackRightPort, int canCoderPortBR, + double secsPer12Volts, double wheelDiameterMeters, double driveGearing, double turnGearing, + boolean isGyroReversed, double maxSpeed, + RobotConfig robotConfig, + double COLLISION_ACCELERATION_THRESHOLD ) { this.swerveConfig = swerveConfig; + this.wheelBase = wheelBase; + this.trackWidth = trackWidth; + this.driveFrontLeftPort = driveFrontLeftPort; + this.turnFrontLeftPort = turnFrontLeftPort; + this.canCoderPortFL = canCoderPortFL; + this.driveFrontRightPort = driveFrontRightPort; + this.turnFrontRightPort = turnFrontRightPort; + this.canCoderPortFR = canCoderPortFR; + this.driveBackLeftPort = driveBackLeftPort; + this.turnBackLeftPort = turnBackLeftPort; + this.canCoderPortBL = canCoderPortBL; + this.driveBackRightPort = driveBackRightPort; + this.turnBackRightPort = turnBackRightPort; + this.canCoderPortBR = canCoderPortBR; + this.secsPer12Volts = secsPer12Volts; + this.wheelDiameterMeters = wheelDiameterMeters; + this.driveGearing = driveGearing; + this.turnGearing = turnGearing; + this.isGyroReversed = isGyroReversed; + this.maxSpeed = maxSpeed; + this.robotConfig = robotConfig; + this.COLLISION_ACCELERATION_THRESHOLD = COLLISION_ACCELERATION_THRESHOLD; AutoBuilder(); @@ -214,22 +280,22 @@ public SimpleDrivetrain( moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, driveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), - turnEncoders[0] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFL), 0, pitchSupplier, rollSupplier); + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); //SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); - moduleFR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.FR, + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, driveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), - turnEncoders[1] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortFR), 1, pitchSupplier, rollSupplier); + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); - moduleBL = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BL, + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, driveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), - turnEncoders[2] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBL), 2, pitchSupplier, rollSupplier); + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); - moduleBR = new SwerveModule(Constants.Drivetrainc.swerveConfig, SwerveModule.ModuleType.BR, + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, driveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), - turnEncoders[3] = SensorFactory.createCANCoder(Constants.Drivetrainc.canCoderPortBR), 3, pitchSupplier, rollSupplier); + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; turnPidControllers[0] = turnMotors[0].getClosedLoopController(); turnPidControllers[1] = turnMotors[1].getClosedLoopController(); @@ -588,7 +654,7 @@ public void drive(SwerveModuleState[] moduleStates) { * Configures PathPlanner AutoBuilder */ public void AutoBuilder() { - RobotConfig config = Constants.Drivetrainc.Autoc.robotConfig; + RobotConfig config = robotConfig; AutoBuilder.configure( //Supplier poseSupplier, this::getPose, // Robot pose supplier diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java new file mode 100644 index 0000000..93fb739 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java @@ -0,0 +1,144 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Drivetrain; + +import java.util.function.BooleanSupplier; +import java.util.function.DoubleSupplier; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; + +public class SimpleTeleopDrive extends Command { + + private static double robotPeriod = Robot.kDefaultPeriod; + private final SimpleDrivetrain drivetrain; + private DoubleSupplier fwd; + private DoubleSupplier str; + private DoubleSupplier rcw; + private BooleanSupplier slow; + private double currentForwardVel = 0; + private double currentStrafeVel = 0; + private double prevTimestamp; + GenericHID manipulatorController; + BooleanSupplier babyModeSupplier; + + /** + * Creates a new TeleopDrive. + */ + public SimpleTeleopDrive(SimpleDrivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, + BooleanSupplier slow, GenericHID manipulatorController, BooleanSupplier babyModeSupplier) { + addRequirements(this.drivetrain = drivetrain); + this.fwd = fwd; + this.str = str; + this.rcw = rcw; + this.slow = slow; + this.manipulatorController = manipulatorController; + this.babyModeSupplier = babyModeSupplier; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + // SmartDashboard.putNumber("slow turn const", kSlowDriveRotation); + // SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed); + // SmartDashboard.putNumber("normal turn const", kNormalDriveRotation); + // SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed); + prevTimestamp = Timer.getFPGATimestamp(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double currentTime = Timer.getFPGATimestamp(); + robotPeriod = currentTime - prevTimestamp; + if (!hasDriverInput()) { + drivetrain.drive(0,0,0); + } + else { + double[] speeds = getRequestedSpeeds(); + // SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp); + prevTimestamp = currentTime; + // kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation); + // kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed); + // kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation); + // kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed); + + // SmartDashboard.putNumber("fwd", speeds[0]); + // SmartDashboard.putNumber("strafe", speeds[1]); + // SmartDashboard.putNumber("turn", speeds[2]); + drivetrain.drive(speeds[0], speeds[1], speeds[2]); + } + } + + public double[] getRequestedSpeeds() { + // Sets all values less than or equal to a very small value (determined by the + // idle joystick state) to zero. + // Used to make sure that the robot does not try to change its angle unless it + // is moving, + double forward = fwd.getAsDouble(); + double strafe = str.getAsDouble(); + double rotateClockwise = rcw.getAsDouble(); + // SmartDashboard.putNumber("fwdIN", forward); + // SmartDashboard.putNumber("strafeIN", strafe); + // SmartDashboard.putNumber("turnIN", rotateClockwise); + // System.out.println("fwd str rcw: "+forward+", "+strafe+", "+rotateClockwise); + boolean slow2 = slow.getAsBoolean(); + forward *= maxForward; + strafe *= maxStrafe; + rotateClockwise *= maxRCW; + + // System.out.println("teleopDrive ExtraSpeedMult%: "+drivetrain.extraSpeedMult); + double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); + double rotationMultiplier = drivetrain.extraSpeedMult + (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); + // double driveMultiplier = (slow.getAsBoolean() ? kSlowDriveSpeed : kNormalDriveSpeed); + // double rotationMultiplier = (slow.getAsBoolean() ? kSlowDriveRotation : kNormalDriveRotation); + + if(babyModeSupplier.getAsBoolean()){ + driveMultiplier = kBabyDriveSpeed; + rotationMultiplier = kBabyDriveRotation; + } + // double driveMultiplier = kNormalDriveSpeed; + // double rotationMultiplier = kNormalDriveRotation; + + forward *= driveMultiplier; + strafe *= driveMultiplier; + rotateClockwise *= rotationMultiplier; + + // Limit acceleration of the robot + // double accelerationX = (forward - currentForwardVel) / robotPeriod; + // double accelerationY = (strafe - currentStrafeVel) / robotPeriod; + // double translationalAcceleration = Math.hypot(accelerationX, accelerationY); + // SmartDashboard.putNumber("Translational Acceleration", translationalAcceleration); + // if (translationalAcceleration > autoMaxAccelMps2 && false) {//DOES NOT RUN!! + // Translation2d limitedAccelerationVector = new Translation2d(autoMaxAccelMps2, + // Rotation2d.fromRadians(Math.atan2(accelerationY, accelerationX))); + // Translation2d limitedVelocityVector = limitedAccelerationVector.times(robotPeriod); + // currentForwardVel += limitedVelocityVector.getX(); + // currentStrafeVel += limitedVelocityVector.getY(); + // } else { + currentForwardVel = forward; + currentStrafeVel = strafe; + // } + // SmartDashboard.putNumber("current velocity", Math.hypot(currentForwardVel, currentStrafeVel)); + + return new double[] { currentForwardVel, currentStrafeVel, -rotateClockwise }; + } + + public boolean hasDriverInput() { + return MathUtil.applyDeadband(fwd.getAsDouble(), Constants.OI.JOY_THRESH)!=0 + || MathUtil.applyDeadband(str.getAsDouble(), Constants.OI.JOY_THRESH)!=0 + || MathUtil.applyDeadband(rcw.getAsDouble(), Constants.OI.JOY_THRESH)!=0; + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java index c7e1633..525fa2b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java @@ -1,8 +1,142 @@ package org.carlmontrobotics.lib199.SimpleMechs.Elevator; +import java.util.function.BooleanSupplier; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ElevatorFeedforward; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.DriverStation; public final class ElevatorConfig { + public final int elevatorMasterId; + public final int elevatorFollowId; //make your own config if you have more than 2 motors + public final MotorConfig elevatorMasterMotorType; + public final MotorConfig elevatorFollowMotorType; + public final boolean elevatorMasterInverted; + public final boolean elevatorFollowInverted; + public final BooleanSupplier bottomReset; + public final BooleanSupplier topReset; + public final double topLimit; //meters + public final double bottomLimit; //meters + public final double gearReduction; //rotations to meters + public double[] elevatorKPID; + public double[] elevatorKFeedForward; + public double elevatorPIDTolerance; + public double maxVolts; //14 for no limit + public double maxManualInput; //percentage of power + + public double elevatorKP; + public double elevatorKI; + public double elevatorKD; + public double elevatorKS; + public double elevatorKG; + public double elevatorKV; + public double elevatorKA; + + public boolean elevatorPIDExists; + public boolean elevatorFeedForwardExists; + + public PIDController elevatorPID; + public ElevatorFeedforward elevatorFeedforward; + + + + public ElevatorConfig(int elevatorMasterId, MotorConfig elevatorMasterMotorType, boolean elevatorMasterInverted, + int elevatorFollowId, MotorConfig elevatorFolllowMotorType, boolean elevatorFollowInverted, + BooleanSupplier bottomReset, BooleanSupplier topReset, double bottomLimit, double topLimit, + double[] elevatorKPID, double[] elevatorKFeedForward, + double gearReduction, double elevatorPIDTolerance, double maxVolts, double maxManualInput) { + + this.elevatorMasterId = elevatorMasterId; + this.elevatorMasterMotorType = elevatorMasterMotorType; + this.elevatorMasterInverted = elevatorMasterInverted; + this.elevatorFollowId = elevatorFollowId; + this.elevatorFollowMotorType = elevatorFolllowMotorType; + this.elevatorFollowInverted = elevatorFollowInverted; + this.bottomReset = bottomReset; + this.topReset = topReset; + this.bottomLimit = bottomLimit; + this.topLimit = topLimit; + this.gearReduction = gearReduction; + this.elevatorPIDTolerance = elevatorPIDTolerance; + this.maxVolts = maxVolts; + this.maxManualInput = maxManualInput; + checkRequirements(); + } - public ElevatorConfig() { + private void checkRequirements() { + checkMotorConfigs(); //Prevent future issues with new motors + checkPID(); + checkFeedForward(); + checkGearReduction(); + checkLimits(); + } + + /** + * Check that the motor Configs are not any new configs that would not work or null. + */ + private void checkMotorConfigs() { + if (elevatorFollowMotorType != MotorConfig.NEO && elevatorFollowMotorType != MotorConfig.NEO_VORTEX && elevatorFollowMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + if (elevatorMasterMotorType != MotorConfig.NEO && elevatorMasterMotorType != MotorConfig.NEO_VORTEX && elevatorMasterMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + + + private void checkPID() { + if (elevatorKPID != null) { + if (elevatorKPID.length == 3) { + elevatorKP = elevatorKPID[0]; + elevatorKI = elevatorKPID[1]; + elevatorKD = elevatorKPID[2]; + elevatorPID = new PIDController(elevatorKP, elevatorKI, elevatorKD); + elevatorPID.setTolerance(elevatorPIDTolerance); + elevatorPIDExists = true; + return; + } + throw new IllegalArgumentException("Need to have 3 values for PID"); + } + elevatorKP = 0; + elevatorKI = 0; + elevatorKD = 0; + elevatorPIDExists = false; + DriverStation.reportWarning("Elevator PID is off", true); + } + + private void checkFeedForward() { + if (elevatorKFeedForward != null) { + if (elevatorKFeedForward.length == 4) { + elevatorKS = elevatorKFeedForward[0]; + elevatorKG = elevatorKFeedForward[1]; + elevatorKV = elevatorKFeedForward[2]; + elevatorKA = elevatorKFeedForward[3]; + elevatorFeedforward = new ElevatorFeedforward(elevatorKS, elevatorKG, elevatorKV, elevatorKA); + elevatorFeedForwardExists = true; + return; + } + throw new IllegalArgumentException("Need to have 4 values for Elevator FeedForward: Friction, Gravity, Voltage, and Acceleration"); + } + elevatorKS = 0; + elevatorKG = 0; + elevatorKV = 0; + elevatorKA = 0; + elevatorFeedForwardExists = false; + DriverStation.reportWarning("ElevatorFeedForward is off", true); + } + + private void checkGearReduction() { + if (gearReduction == 0) { + throw new IllegalArgumentException("Gear reduction cannot be 0"); + } + } + private void checkLimits() { + if (bottomLimit >= topLimit) { + throw new IllegalArgumentException("Top limit must be much greater than bottom limit. (Cannot be equal)"); + } } } \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java index 94e1c47..a54964d 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java @@ -4,14 +4,286 @@ package org.carlmontrobotics.lib199.SimpleMechs.Elevator; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SimpleMechs.Arm.SimpleArm.ArmControlState; + +import java.nio.channels.Pipe; + +import javax.management.relation.Relation; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class SimpleElevator extends SubsystemBase { /** Creates a new Elevator. */ - public SimpleElevator() {} + private ElevatorConfig elevatorConfig; + private SparkBase masterMotor; + private SparkBase followMotor; + private RelativeEncoder feedbackEncoder; + private RelativeEncoder backupEncoder; + + private double elevatorGoal; + private double latestManualInput; // in percentage of power (-1, 1) + + private ElevatorControlState currentState = ElevatorControlState.AUTO; + private IdleMode elevatorIdleMode = IdleMode.kBrake; + + private double currentDif = 0; + private double pastDif = 0; + private final double difDanger = 1; + + public enum ElevatorControlState { + AUTO, + MANUAL, + BRAKE, + COAST + } + + public SimpleElevator(ElevatorConfig elevatorConfig) { + this.elevatorConfig = elevatorConfig; + configureMotors(); + resetFeedbackEncoder(); + } + + public void setGoal(double goal){ + if (goal <= elevatorConfig.topLimit && goal >= elevatorConfig.bottomLimit) {elevatorGoal = goal;} + } + + public double getGoal() { + return elevatorGoal; + } + + public void setManualInput(double input) { + latestManualInput = input; + } + + public double getLatestManualInput() { + return latestManualInput; + } + + public void setElevatorControlState(ArmControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } + } + + public void setAutoOn() { + if (elevatorConfig.elevatorPIDExists || elevatorConfig.elevatorFeedForwardExists) { + currentState = ElevatorControlState.AUTO; + } + else { + DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true); + } + } + + public void setManualOn() { + currentState = ElevatorControlState.MANUAL; + } + + public void setBrakeOn() { + currentState = ElevatorControlState.BRAKE; + if (elevatorIdleMode == IdleMode.kCoast) { + elevatorIdleMode = IdleMode.kBrake; + brakeMotors(); + + } + } + + public void setCoastOn() { + currentState = ElevatorControlState.COAST; + if (elevatorIdleMode == IdleMode.kBrake) { + elevatorIdleMode = IdleMode.kCoast; + coastMotors(); + } + } + + /** + * Set all motors idleModes to brake + */ + private void brakeMotors() { + SparkFlexConfig flexBrakeConfig = new SparkFlexConfig(); + SparkMaxConfig maxBrakeConfig = new SparkMaxConfig(); + + flexBrakeConfig.idleMode(IdleMode.kBrake); + maxBrakeConfig.idleMode(IdleMode.kBrake); + if (elevatorConfig.elevatorFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to coast + */ + private void coastMotors() { + SparkFlexConfig flexCoastConfig = new SparkFlexConfig(); + SparkMaxConfig maxCoastConfig = new SparkMaxConfig(); + + flexCoastConfig.idleMode(IdleMode.kCoast); + maxCoastConfig.idleMode(IdleMode.kCoast); + if (elevatorConfig.elevatorFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + private void checkFeedBackEncoder() { + pastDif = currentDif; + currentDif = feedbackEncoder.getPosition() - backupEncoder.getPosition(); + if (currentDif - pastDif > difDanger) { + DriverStation.reportWarning("Elevator encoder seems to be off!", true); + } + } + + + + public double getElevatorHeight() { + return feedbackEncoder.getPosition(); + } + + public double getElevatorVelocity() { + return feedbackEncoder.getVelocity(); + } + + + + private void configureMotors() { + if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig masterFlexConfig = new SparkFlexConfig(); + masterFlexConfig.idleMode(IdleMode.kBrake) + .inverted(elevatorConfig.elevatorMasterInverted) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkFlex(elevatorConfig.elevatorMasterId); + masterMotor.configure(masterFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig masterMaxConfig = new SparkMaxConfig(); + masterMaxConfig.idleMode(IdleMode.kBrake) + .inverted(elevatorConfig.elevatorMasterInverted) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkMax(elevatorConfig.elevatorMasterId, elevatorConfig.elevatorMasterMotorType); + masterMotor.configure(masterMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + feedbackEncoder = masterMotor.getEncoder(); + + if (elevatorConfig.elevatorFollowMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig followFlexConfig = new SparkFlexConfig(); + followFlexConfig.idleMode(IdleMode.kBrake) + .inverted(elevatorConfig.elevatorFollowInverted) + .follow(elevatorConfig.elevatorMasterId, (elevatorConfig.elevatorMasterInverted != elevatorConfig.elevatorFollowInverted)) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkFlex(elevatorConfig.elevatorFollowId); + followMotor.configure(followFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig followMaxConfig = new SparkMaxConfig(); + followMaxConfig.idleMode(IdleMode.kBrake) + .follow(elevatorConfig.elevatorMasterId, (elevatorConfig.elevatorMasterInverted != elevatorConfig.elevatorFollowInverted)) + .inverted(elevatorConfig.elevatorFollowInverted) + .encoder.positionConversionFactor(elevatorConfig.gearReduction) + .velocityConversionFactor(elevatorConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkMax(elevatorConfig.elevatorFollowId, elevatorConfig.elevatorFollowMotorType); + followMotor.configure(followMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + backupEncoder = followMotor.getEncoder(); + } + + private void autoElevator() { + //TODO + } + + private void manualElevator() { + //TODO + } + + public void stopElevator() { + masterMotor.set(0); + } + + public void resetFeedbackEncoder() { + resetFeedbackEncoder(elevatorConfig.bottomLimit); + } + + public void resetFeedbackEncoder(double position) { + if (position >= elevatorConfig.bottomLimit && position <= elevatorConfig.topLimit) { + feedbackEncoder.setPosition(position); + } + else { + DriverStation.reportWarning("Cannot reset position to be outside the limits", true); + } + } + + private void updateFeedbackEncoder() { + if (elevatorConfig.bottomReset != null) { + if (elevatorConfig.bottomReset.getAsBoolean()) { + feedbackEncoder.setPosition(elevatorConfig.bottomLimit); + } + } + if (elevatorConfig.topReset != null) { + if (elevatorConfig.topReset.getAsBoolean()) { + feedbackEncoder.setPosition(elevatorConfig.topLimit); + } + } + } + /** + * Use this to do any fun stuff you want to do, for elevatorSubsystemPeriodic + */ + public void userPeriodic() { + + } + + private void postSmartDashboardValues() { + + } + /** + * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! + * Use {@link #userPeriodic()} instead. + */ @Override public void periodic() { - // This method will be called once per scheduler run + switch (currentState) { + case AUTO -> autoElevator(); + case MANUAL -> manualElevator(); + case BRAKE -> stopElevator(); + case COAST -> stopElevator(); + } + checkFeedBackEncoder(); + updateFeedbackEncoder(); + userPeriodic(); + postSmartDashboardValues(); } } From 32558551979d70c8f01e88c011add2ebaeedde9a Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Mon, 29 Dec 2025 19:51:02 -0800 Subject: [PATCH 07/10] Simple Mechs complete Created a detailed plug and play common mechs that allow programming not to waste time reinventing the wheel each year --- .../lib199/MotorControllerFactory.java | 1 - .../lib199/SimpleMechs/Arm/ArmConfig.java | 340 ++++------ .../lib199/SimpleMechs/Arm/SimpleArm.java | 350 +++++++++-- .../SimpleMechs/Arm/SimpleSetArmGoal.java | 39 ++ .../Drivetrain/SimpleDrivetrain.java | 587 ++++++++++-------- .../SimpleRotateToFieldRelativeAngle.java | 52 ++ .../Drivetrain/SimpleTeleopDrive.java | 29 +- .../lib199/SimpleMechs/Elastic.java | 390 ++++++++++++ .../SimpleMechs/Elevator/ElevatorConfig.java | 61 +- .../SimpleMechs/Elevator/SimpleElevator.java | 341 +++++++++- .../Elevator/SimpleManualElevator.java | 52 ++ .../Elevator/SimpleRaiseElevator.java | 73 +++ .../Spinner/SimpleSpinSpinner.java | 40 ++ .../SimpleMechs/Spinner/SimpleSpinner.java | 511 ++++++++++++++- .../SimpleMechs/Spinner/SpinnerConfig.java | 224 ++++++- .../lib199/swerve/SwerveModule.java | 424 +++++++++++-- .../lib199/swerve/SwerveModuleSim.java | 56 +- 17 files changed, 2962 insertions(+), 608 deletions(-) create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java create mode 100644 src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java diff --git a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java index 8e33230..1604845 100644 --- a/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java +++ b/src/main/java/org/carlmontrobotics/lib199/MotorControllerFactory.java @@ -17,7 +17,6 @@ import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.servohub.ServoHub.ResetMode; import com.revrobotics.spark.ClosedLoopSlot; import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkFlex; diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java index fda42aa..82fa97e 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -1,17 +1,7 @@ package org.carlmontrobotics.lib199.SimpleMechs.Arm; import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.spark.SparkBase; -import com.revrobotics.spark.SparkFlex; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.config.SparkFlexConfig; -import com.revrobotics.spark.config.SparkMaxConfig; -import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.PIDController; @@ -19,37 +9,31 @@ public final class ArmConfig { - double armRelativeGearReduction, armAbsoluteGearReduction, armPIDTolerance, armMaxVolts; - int[] armFollowId; - MotorConfig[] armFollowMotorType; - boolean[] armFollowInversed; - - int armMasterMotorId; - MotorConfig armMasterMotorType; - boolean armMasterInversed; + public final double armRelativeGearReduction, armAbsoluteGearReduction, armPIDTolerance, armMaxVolts; + public final int[] armFollowId; + public final MotorConfig[] armFollowMotorType; + public final boolean[] armFollowInversed; + + public final int armMasterMotorId; + public final MotorConfig armMasterMotorType; + public final boolean armMasterInversed; - double [] armKPID, armKFeedForward; - double armKP, armKI, armKD, armKS, armKG, armKV, armKA; + public final double [] armKPID, armKFeedForward; + public double armKP, armKI, armKD, armKS, armKG, armKV, armKA; - AbsoluteEncoder armMainAbsoluteEncoder; - RelativeEncoder armMainRelativeEncoder; - RelativeEncoder armBackupRelativeEncoder; + public AbsoluteEncoder armMainAbsoluteEncoder; - ArmFeedforward armFeedForward; - PIDController armPID; - boolean armMainAbsoluteEncoderExists, armMainRelativeEncoderExists, armBackupRelativeEncoderExists, armPIDContinuousInput, armPIDExists, armFeedForwardExists; - //Enum armStates; + public ArmFeedforward armFeedForward; + public PIDController armPID; + public boolean armPIDContinuousInput; - SparkBase armMasterMotor; + public double armAbsoluteZeroOffset; + public double bottomLimit; + public double topLimit; - double armAbsoluteZeroOffset; - double bottomLimit; - double topLimit; + public int armMotorOfBackupRelativeEncoderId; - - - //@param armStates provide an enum State of different positions the arm should be at, null if no state machine /** * Creates a ArmConfig for a SimpleArm * @param armRelativeGearReduction Gear reduction from shaft spinning where motors are to the arm axis @@ -68,8 +52,17 @@ public final class ArmConfig { * @param armMainAbsoluteEncoder null if no absolute * @param armMotorIDOfBackupRelativeEncoder -1 if none * @param armAbsoluteZeroOffset zeroOffset (in degrees) of abs enc, 0 should always be horizontal! - * @param bottomLimit lowest value the arm can achieve (soft limit) - * @param topLimit highest value the arm can achieve (soft limit) + * @param bottomLimit lowest value the arm can achieve (soft limit) in degrees with 0 being horizontal + * @param topLimit highest value the arm can achieve (soft limit) in degrees with 0 being horizontal + * @throws IllegalArgumentException if each follow motor does not have an id, motor type, and inverse config + * @throws IllegalArgumentException if the master motor does not have an id, motor type, and inverse config + * @throws IllegalArgumentException if the any of the provided motor types are not one of the 3 expected {@link MotorConfig} + * @throws IllegalArgumentException if the id of the backupEncoder is not part of the provided set of follow motors + * @throws IllegalArgumentException if spinnerKPID is not three values with each being non negative + * @throws IllegalArgumentException if spinnerKFeedForward is not three values with each being non negative + * @throws IllegalArgumentException if gearReduction is 0 + * @throws IllegalArgumentException if maxVolts is not positive + * @throws IllegalArgumentException if the top limit of the arm is lower or equal to the bottom limit of the arm */ public ArmConfig( double armRelativeGearReduction, double armAbsoluteGearReduction, @@ -79,7 +72,6 @@ public ArmConfig( double[] armKPID, double armPIDTolerance, boolean armPIDContinuousInput, double[] armKFeedForward, AbsoluteEncoder armMainAbsoluteEncoder, int armMotorIDOfBackupRelativeEncoder, - //Object armStates, double armAbsoluteZeroOffset, double bottomLimit, double topLimit @@ -103,131 +95,108 @@ public ArmConfig( this.armAbsoluteZeroOffset = armAbsoluteZeroOffset; this.bottomLimit = bottomLimit; this.topLimit = topLimit; + this.armMotorOfBackupRelativeEncoderId = armMotorIDOfBackupRelativeEncoder; - checkRequirements( - //armStates, - armMotorIDOfBackupRelativeEncoder); + checkRequirements(armMotorIDOfBackupRelativeEncoder); } - double defaultArmPIDTolerance = 2; + private static double defaultArmPIDTolerance = 2; //degrees /** * Create a single motor arm config with PID, no FeedForward - * @param armMasterMotorId - * @param armMasterMotorType - * @param armMasterInversed - * @param armKPID - * @param bottomLimit - * @param topLimit - * @return + * @param armMasterMotorId master id + * @param armMasterMotorType @link MotorConfig} of the master motor + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param armKPID double array with 3 values, kP, kI, kD + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if MotorType is not one of the 3 expected {@link MotorConfig}s + * @throws IllegalArgumentException if armKPID does not have 3 nonnegative values + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit */ - public ArmConfig SimpleArmConfig(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double[] armKPID, - //Object armStates, - double bottomLimit, double topLimit) { - return new ArmConfig(1, 1, 14, null, null, null, armMasterMotorId, armMasterMotorType, armMasterInversed, armKPID, defaultArmPIDTolerance, false, null, null, -1, - //armStates, - 0, bottomLimit, topLimit); + public static ArmConfig motorWithPID(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double[] armKPID, double bottomLimit, double topLimit) { + return new ArmConfig(1, 1, 14, null, null, null, armMasterMotorId, armMasterMotorType, armMasterInversed, armKPID, defaultArmPIDTolerance, false, null, null, -1, 0, bottomLimit, topLimit); } /** * Create a single motor arm config no PID, no FeedForward - * @param armMasterMotorId - * @param armMasterMotorType - * @param armMasterInversed - * @param bottomLimit - * @param topLimit - * @return + * @param armMasterMotorId master id + * @param armMasterMotorType {@link MotorConfig} of the master motor + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if MotorType is not one of the 3 expected {@link MotorConfig}s + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit */ - public ArmConfig SimpleArmConfig(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, - //Object armStates, - double bottomLimit, double topLimit) { - return SimpleArmConfig(armMasterMotorId, armMasterMotorType, armMasterInversed, null, - //armStates, - bottomLimit, topLimit); + public static ArmConfig motorNoPID(int armMasterMotorId, MotorConfig armMasterMotorType, boolean armMasterInversed, double bottomLimit, double topLimit) { + return motorWithPID(armMasterMotorId, armMasterMotorType, armMasterInversed, null, bottomLimit, topLimit); } /** * Creates a single NEO arm config with PID - * @param armMasterMotorId - * @param armMasterInversed - * @param armKPID - * @param bottomLimit - * @param topLimit - * @return + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param armKPID double array with 3 values, kP, kI, kD + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if armKPID does not have 3 nonnegative values + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit */ - public ArmConfig SimpleNeoArmConfig(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, - //Object armStates, - double bottomLimit, double topLimit) { - return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO, armMasterInversed, armKPID, - //armStates, - bottomLimit, topLimit); + public static ArmConfig neoWithPID(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, double bottomLimit, double topLimit) { + return motorWithPID(armMasterMotorId, MotorConfig.NEO, armMasterInversed, armKPID, bottomLimit, topLimit); } /** * Create a single NEO arm config without PID - * @param armMasterMotorId - * @param armMasterInversed - * @param bottomLimit - * @param topLimit - * @return + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit */ - public ArmConfig SimpleNeoArmConfig(int armMasterMotorId, boolean armMasterInversed, - //Object armStates, - double bottomLimit, double topLimit) { - return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO, armMasterInversed, - //armStates, - bottomLimit, topLimit); + public static ArmConfig neoNoPID(int armMasterMotorId, boolean armMasterInversed, double bottomLimit, double topLimit) { + return motorNoPID(armMasterMotorId, MotorConfig.NEO, armMasterInversed, bottomLimit, topLimit); } /** * Createa a single vortex arm config, with PID - * @param armMasterMotorId - * @param armMasterInversed - * @param armKPID - * @param bottomLimit - * @param topLimit - * @return + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param armKPID double array with 3 values, kP, kI, kD + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if armKPID does not have 3 nonnegative values + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit */ - public ArmConfig SimpleVortexArmConfig(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, - //Object armStates, - double bottomLimit, double topLimit) { - return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, armKPID, - //armStates, - bottomLimit, topLimit); + public static ArmConfig vortexWithPID(int armMasterMotorId, boolean armMasterInversed, double[] armKPID, double bottomLimit, double topLimit) { + return motorWithPID(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, armKPID, bottomLimit, topLimit); } /** * Create a single vortex arm config, no PID - * @param armMasterMotorId - * @param armMasterInversed - * @param bottomLimit - * @param topLimit - * @return + * @param armMasterMotorId master id + * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up + * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal + * @param topLimit highest angle in degrees the arm can safely achieve with 0 being horizontal + * @throws IllegalArgumentException if the bottom limit is higher or equal to the top limit */ - public ArmConfig SimpleVortexArmConfig(int armMasterMotorId, boolean armMasterInversed, - //Object armStates, - double bottomLimit, double topLimit) { - return SimpleArmConfig(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, - //armStates, - bottomLimit, topLimit); + public static ArmConfig vortexNoPID(int armMasterMotorId, boolean armMasterInversed, double bottomLimit, double topLimit) { + return motorNoPID(armMasterMotorId, MotorConfig.NEO_VORTEX, armMasterInversed, bottomLimit, topLimit); } /** * Check that arguments are good and set some of the parameters of the config. * @param armMotorOfBackupRelativeEncoderId give motor Id */ - private void checkRequirements( - //Object armStates, - int armMotorOfBackupRelativeEncoderId) { + private void checkRequirements(int armMotorOfBackupRelativeEncoderId) { matchMotors(); checkMotorConfigs(); //Prevent future issues with new motors checkEncoders(armMotorOfBackupRelativeEncoderId); checkPID(); checkFeedForward(); checkGearReduction(); - //checkEnum(armStates); checkVolts(); checkLimits(); } @@ -256,9 +225,11 @@ private void matchMotors() { * Check that the motor Configs are not any new configs that would not work or null. */ private void checkMotorConfigs() { - for (MotorConfig config: armFollowMotorType) { - if (config != MotorConfig.NEO && config != MotorConfig.NEO_VORTEX && config != MotorConfig.NEO_550) { - throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + if (armFollowId != null) { + for (MotorConfig config: armFollowMotorType) { + if (config != MotorConfig.NEO && config != MotorConfig.NEO_VORTEX && config != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } } } if (armMasterMotorType != MotorConfig.NEO && armMasterMotorType != MotorConfig.NEO_VORTEX && armMasterMotorType != MotorConfig.NEO_550) { @@ -267,74 +238,40 @@ private void checkMotorConfigs() { } /** - * + * Create {@link #armMainRelativeEncoder}, {@link #armMainAbsoluteEncoder}, and {@link #armBackupRelativeEncoder} if such are provided. + * Will update the exists boolean for each, and throw illegal arguments if does not see the backup encoder id in the follow ids * @param armMotorOfBackupRelativeEncoderId */ private void checkEncoders(int armMotorOfBackupRelativeEncoderId) { - if (armMainAbsoluteEncoder != null) { - armMainAbsoluteEncoderExists = true; - } - else { - armMainAbsoluteEncoderExists = false; - } - - if (armMasterMotorType == MotorConfig.NEO_VORTEX) { - SparkFlexConfig flexConfig = new SparkFlexConfig(); - this.armMasterMotor = MotorControllerFactory.createSparkFlex(armMasterMotorId); - flexConfig.inverted(armMasterInversed) - .idleMode(IdleMode.kBrake) - .encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees - .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second - armMasterMotor.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - armMainRelativeEncoder = armMasterMotor.getEncoder(); - } - else { - SparkMaxConfig maxConfig = new SparkMaxConfig(); - armMasterMotor = MotorControllerFactory.createSparkMax(armMasterMotorId, armMasterMotorType); - maxConfig.inverted(armMasterInversed) - .idleMode(IdleMode.kBrake) - .encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees - .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second - armMasterMotor.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - armMainRelativeEncoder = armMasterMotor.getEncoder(); - } - armMainRelativeEncoderExists = true; if (armMotorOfBackupRelativeEncoderId != -1) { - for (int i = 0; i < armFollowId.length; i++) { - if (armFollowId[i] == armMotorOfBackupRelativeEncoderId) { - if (armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { - SparkFlexConfig flexConfig = new SparkFlexConfig(); - SparkFlex dummyFlex = MotorControllerFactory.createSparkFlex(armMasterMotorId); - flexConfig.encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees - .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second - dummyFlex.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - armBackupRelativeEncoder = dummyFlex.getEncoder(); - } - else { - SparkMaxConfig maxConfig = new SparkMaxConfig(); - SparkMax dummyMax = MotorControllerFactory.createSparkMax(armMasterMotorId, armMasterMotorType); - maxConfig.encoder.positionConversionFactor(armRelativeGearReduction * 360) // Degrees - .velocityConversionFactor(armRelativeGearReduction/60); //Rotations per second - dummyMax.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - armBackupRelativeEncoder = dummyMax.getEncoder(); + if (armFollowId != null) { + for (int i = 0; i < armFollowId.length; i++) { + if (armFollowId[i] == armMotorOfBackupRelativeEncoderId) { + return; } - armMainRelativeEncoderExists = true; - return; } + throw new IllegalArgumentException("Backup motor id of " + armMotorOfBackupRelativeEncoderId + " is not part of provided motors :("); + } + else { + throw new IllegalArgumentException("Cannot have a backup relative encoder without follow motors"); } - throw new IllegalArgumentException("Backup motor id of " + armMotorOfBackupRelativeEncoderId + " is not part of provided motors :("); } else { - armMainRelativeEncoderExists = false; DriverStation.reportWarning("Arm does not have backup encoder, highly recommended", true); } } + /** + * Checks PID to have 3 values, updates {@link #armPIDExists} and makes sure the values are legitimate + */ private void checkPID() { if (armKPID != null) { if (armKPID.length == 3) { + if (armKPID[0] < 0 || armKPID[1] < 0 || armKPID[2] < 0) { + throw new IllegalArgumentException("PID values have to be non negative"); + } armKP = armKPID[0]; armKI = armKPID[1]; armKD = armKPID[2]; @@ -343,7 +280,6 @@ private void checkPID() { if (armPIDContinuousInput) { armPID.enableContinuousInput(-180, 180); } - armPIDExists = true; return; } throw new IllegalArgumentException("Need to have 3 values for PID"); @@ -351,19 +287,23 @@ private void checkPID() { armKP = 0; armKI = 0; armKD = 0; - armPIDExists = false; DriverStation.reportWarning("ArmPID is off", true); } + /** + * Checks FeedForward to have 4 values, updates {@link #armFeedForwardExists} and makes sure the values are legitimate + */ private void checkFeedForward() { if (armKFeedForward != null) { if (armKFeedForward.length == 4) { + if (armKFeedForward[0] < 0|| armKFeedForward[2] < 0 || armKFeedForward[3] < 0) { + throw new IllegalArgumentException("FeedForward Values of kS, kV, and kA need to have non negative values"); + } armKS = armKFeedForward[0]; armKG = armKFeedForward[1]; armKV = armKFeedForward[2]; armKA = armKFeedForward[3]; armFeedForward = new ArmFeedforward(armKS, armKG, armKV, armKA); - armFeedForwardExists = true; return; } throw new IllegalArgumentException("Need to have 4 values for Arm FeedForward: Friction, Gravity, Voltage, and Acceleration"); @@ -372,68 +312,34 @@ private void checkFeedForward() { armKG = 0; armKV = 0; armKA = 0; - armFeedForwardExists = false; DriverStation.reportWarning("ArmFeedForward is off", true); } + /** + * Makes sure gear reduction is not 0 + */ private void checkGearReduction() { if (armRelativeGearReduction == 0) { throw new IllegalArgumentException("Gear reduction for relative encoders cannot be 0"); } - if (armMainAbsoluteEncoderExists && armAbsoluteGearReduction == 0) { + if (armMainAbsoluteEncoder != null && armAbsoluteGearReduction == 0) { throw new IllegalArgumentException("Absolute gear reduction cannot be 0 if absolute encoder exists"); } } - // private void checkEnum(Object armStates) { - // if (armStates != null) { - // Class cls = armStates.getClass(); - - // if (!cls.isEnum()) { - // throw new IllegalArgumentException("Expected an enum"); - // } - - // java.lang.reflect.Method getValueMethod; - // // Ensure a getValue() method exists - // try { - // getValueMethod = cls.getDeclaredMethod("getValue"); - // } catch (NoSuchMethodException e) { - // throw new IllegalArgumentException("Enum must have a getValue() method returning the value of each state"); - // } - - // Object[] values = cls.getEnumConstants(); - - // for (Object constant : values) { - // try { - // Object result = getValueMethod.invoke(constant); - - // if (!(result instanceof Number)) { - // throw new IllegalArgumentException( - // "getValue() must return a numeric type: " + constant); - // } - - // double value = ((Number) result).doubleValue(); - // if (value > topLimit || value < bottomLimit) { - // throw new IllegalArgumentException("Enum Position cannot be outside of the bounding limits"); - // } - - // } catch (Exception e) { - // throw new RuntimeException("Failed to read enum value", e); - // } - // } - // this.armStates = (Enum) armStates; - // } - // else { - // DriverStation.reportWarning("Daniel is sad that you don't like states", true); - // } - // } - + /** + * Check for volt limit to be greater than 0 + */ private void checkVolts() { if (armMaxVolts <= 0) { throw new IllegalArgumentException("armMaxVolts must be a positive number."); } return; } + + /** + * Makes sure that the user has not mixed up the top and bottom limits. + */ private void checkLimits() { if (bottomLimit >= topLimit) { throw new IllegalArgumentException("Top limit must be much greater than bottom limit. (Cannot be equal)"); diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java index 106e590..07b3b2f 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleArm.java @@ -2,11 +2,17 @@ // 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. +/* + * Important info about SimpleArm! + * Encoders are set to read in degrees from the perspective of the rotating object, with 0 being horizontal in the more often direction the use would use + */ + package org.carlmontrobotics.lib199.SimpleMechs.Arm; import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SimpleMechs.Elastic; import com.revrobotics.AbsoluteEncoder; import com.revrobotics.RelativeEncoder; @@ -20,21 +26,31 @@ import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; public class SimpleArm extends SubsystemBase { /** Creates a new SimpleArm. */ - private ArmConfig armConfig; + private final ArmConfig armConfig; private SparkBase armMasterMotor; private RelativeEncoder armFeedbackEncoder; private AbsoluteEncoder armResetEncoder; private RelativeEncoder armBackupEncoder; private SparkBase[] armFollowMotors; - private double currentDif = 0; - private double pastDif = 0; + private double encoderDif = 0; private final double difDanger = 1; private double armGoal; // in degrees @@ -43,10 +59,37 @@ public class SimpleArm extends SubsystemBase { private ArmControlState currentState = ArmControlState.AUTO; private IdleMode armIdleMode = IdleMode.kBrake; - private Timer armTimer = new Timer(); - private final double defaultRelativeEncoderResetValue = -90; + private double currentStamp; + private double pastStamp; + private double currentPeriod = 0.02; + + private boolean encoderFaultReported = false; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutAngle m_angle = Radians.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0); + + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(5)//maximum sysID test time + ); + + + private final SysIdRoutine sysIdRoutine; + + /** + * Control State for the arm + * {@link #AUTO} For automatically controling the arm to go to a certain goal + * {@link #MANUAL} For manually controlling the arm by providing percentage of voltage + * {@link #BRAKE} For holding the arm in a rigidish state, not moving + * {@link #COAST} For setting the arm to not resist movement, allowing accessibility when the robot is disabled. + */ public enum ArmControlState { AUTO, MANUAL, @@ -54,28 +97,135 @@ public enum ArmControlState { COAST } + /** + * Creates an object of {@link SimpleArm}, can be extended off this class to create a more complicated arm without requiring to fully remake a subsystem. + * @param armConfig + */ public SimpleArm(ArmConfig armConfig) { this.armConfig = armConfig; + armResetEncoder = armConfig.armMainAbsoluteEncoder; configMotors(); resetFeedbackEncoder(); + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + voltage -> { + manualArm(voltage); + }, + log -> { + log.motor("Spinner-Master") + .voltage( + m_appliedVoltage + .mut_replace(armMasterMotor.getBusVoltage() * armMasterMotor.getAppliedOutput(), Volts)) + .angularPosition(m_angle.mut_replace(Units.degreesToRadians(armFeedbackEncoder.getPosition()), Radians)) + .angularVelocity(m_velocity.mut_replace(Units.degreesToRadians(armFeedbackEncoder.getVelocity()), RadiansPerSecond)); + }, + this) + ); + + if (isSysIdTesting()) { + sysIdSetup(); + } + SmartDashboard.putData(this); + } + + /** + * Sets up commands in SmartDashboard/Elastic to be ran to record data for mechanism modeling + */ + private void sysIdSetup() { + Elastic.Notification notification = new Elastic.Notification(Elastic.NotificationLevel.INFO, "SysId Testing On", "Please add Arm Quasistatic and Dynamics and begin testing.").withDisplaySeconds(5); + Elastic.sendNotification(notification); + Elastic.selectTab("SysId"); + SmartDashboard.putData("Arm Quasistatic Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + SmartDashboard.putData("Arm Quasistatic Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Arm Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Arm Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + /** Override this method and return true if you want to do sysid testing, return false if you don't + * @return false to do sysid testing + */ + protected boolean isSysIdTesting() { + return false; + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); } + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in. + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + /** + * @return position in degrees from horizontal + */ + public double getArmPosition() { + return armFeedbackEncoder.getPosition(); + } + + + /** + * Sets {@link #armGoal}, as long as the value is within the provided limits in the {@link ArmConfig}. + * @param goal in degrees. + */ public void setGoal(double goal){ if (goal <= armConfig.topLimit && goal >= armConfig.bottomLimit) {armGoal = goal;} } + /** + * Gets the current value of {@link #armGoal}. + * @return {@link #armGoal} in degrees. + */ public double getGoal() { return armGoal; } + /** + * Checks if {@link SimpleArm} is at {@link #armGoal} within the given tolerance in the {@link ArmConfig}. + * @return true if it is at the {@link #armGoal}. + */ + public boolean atGoal() { + return Math.abs(armGoal - armFeedbackEncoder.getPosition()) <= armConfig.armPIDTolerance; + } + + /** + * Sets {@link #latestManualInput}. + * @param input -1.0 - 1.0 range for inputing voltage percentage into the arm. + */ public void setManualInput(double input) { latestManualInput = input; } + /** + * Fetches the user input being applied on the arm if the {@link #currentState} is MANUAL. + * @return {@link #latestManualInput} + */ public double getLatestManualInput() { return latestManualInput; } + /** + * @return active {@link ArmControlState}. + */ + protected ArmControlState getCurrentState() { + return currentState; + } + + /** + * Toggles {@link #currentState} between AUTO and MANUAL + * Does not do anything if arm is idling + */ public void toggleControlMode() { if (currentState == ArmControlState.AUTO) { currentState = ArmControlState.MANUAL; @@ -85,6 +235,10 @@ else if (currentState == ArmControlState.MANUAL) { } } + /** + * Sets {@link #currentState} to be a certain {@link ArmControlState} + * @param controlState desired control state + */ public void setArmControlState(ArmControlState controlState) { switch (controlState) { case AUTO -> setAutoOn(); @@ -95,8 +249,11 @@ public void setArmControlState(ArmControlState controlState) { } } + /** + * Sets {@link #currentState} to be AUTO making the arm approach the {@link #armGoal} + */ public void setAutoOn() { - if (armConfig.armPIDExists || armConfig.armFeedForwardExists) { + if (armConfig.armPID != null || armConfig.armFeedForward != null) { currentState = ArmControlState.AUTO; } else { @@ -104,10 +261,16 @@ public void setAutoOn() { } } + /** + * Sets {@link #currentState} to be MANUAL allowing user to move the arm manually + */ public void setManualOn() { currentState = ArmControlState.MANUAL; } + /** + * Sets {@link #currentState} to be BRAKE and brakes the motors + */ public void setBrakeOn() { currentState = ArmControlState.BRAKE; if (armIdleMode == IdleMode.kCoast) { @@ -117,6 +280,9 @@ public void setBrakeOn() { } } + /** + * Sets {@link #currentState} to be COAST and coasts the motors + */ public void setCoastOn() { currentState = ArmControlState.COAST; if (armIdleMode == IdleMode.kBrake) { @@ -125,55 +291,74 @@ public void setCoastOn() { } } - private void resetFeedbackEncoder() { - if (armConfig.armMainAbsoluteEncoderExists) { + /** + * Resets feedback encoder using the {@link #armResetEncoder}, if such does not exist will use a {@link #defaultRelativeEncoderResetValue} + */ + public void resetFeedbackEncoder() { + if (armConfig.armMainAbsoluteEncoder != null) { armFeedbackEncoder.setPosition((armResetEncoder.getPosition() + armConfig.armAbsoluteZeroOffset)/armConfig.armAbsoluteGearReduction*armConfig.armRelativeGearReduction); + if (armBackupEncoder == null) {return;} + armBackupEncoder.setPosition((armResetEncoder.getPosition() + armConfig.armAbsoluteZeroOffset)/armConfig.armAbsoluteGearReduction*armConfig.armRelativeGearReduction); } else { armFeedbackEncoder.setPosition(defaultRelativeEncoderResetValue); + if (armBackupEncoder == null) {return;} + armBackupEncoder.setPosition(defaultRelativeEncoderResetValue); } } + /** + * Called periodically + * Checks difference between active encoder and backup encoder to see if there seems to be a mishap, such as encoder not measuring properly or just turning off. + */ private void checkFeedBackEncoder() { - pastDif = currentDif; - currentDif = armFeedbackEncoder.getPosition() - armBackupEncoder.getPosition(); - if (currentDif - pastDif > difDanger) { + if (armBackupEncoder == null) {return;} + encoderDif = armFeedbackEncoder.getPosition() - armBackupEncoder.getPosition(); + if (Math.abs(encoderDif) >= difDanger && !encoderFaultReported) { + encoderFaultReported = true; DriverStation.reportWarning("Arm encoder seems to be off!", true); } } - - private void autoArm() { + /** + * Called in periodic if {@link #currentState} is auto. + * Will calculate the desired voltage to achieve the {@link #armGoal} + */ + protected void autoArm() { double pidOutput; double feedforwardOutput; double curPosition = armFeedbackEncoder.getPosition(); - if (armConfig.armPIDExists) { + if (armConfig.armPID != null) { pidOutput = armConfig.armPID.calculate(curPosition, armGoal); } else { pidOutput = 0; } - if (armConfig.armFeedForwardExists) { - feedforwardOutput = armConfig.armFeedForward.calculate(armGoal/360*2*Math.PI, 0); + if (armConfig.armFeedForward != null) { + feedforwardOutput = armConfig.armFeedForward.calculate(Units.degreesToRadians(curPosition), 0); } else { feedforwardOutput = 0; } if (curPosition < armConfig.topLimit) { if (curPosition > armConfig.bottomLimit) { - armMasterMotor.set(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, armConfig.armMaxVolts)); + armMasterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, armConfig.armMaxVolts)); } else { - armMasterMotor.set(MathUtil.clamp(pidOutput + feedforwardOutput, 0, armConfig.armMaxVolts)); + armMasterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, 0, armConfig.armMaxVolts)); } } else { - armMasterMotor.set(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, 0)); + armMasterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -armConfig.armMaxVolts, 0)); } } - private void manualArm() { + /** + * Called in periodic if {@link #currentState} is manual. + * Will apply a percentage of voltage to the arm based of the users input: {@link #latestManualInput}, while soft locking it to avoid the arm leaving the intended bounds + */ + protected void manualArm() { double curPosition = armFeedbackEncoder.getPosition(); if (curPosition < armConfig.topLimit) { if (curPosition > armConfig.bottomLimit) { @@ -188,12 +373,35 @@ private void manualArm() { } } + /** + * Mainly used by SysId to characterize the spinner model + * @param volts Voltage unit input + */ + protected void manualArm(Voltage volts) { + double curPosition = armFeedbackEncoder.getPosition(); + double voltage = volts.in(Volts); + if (curPosition < armConfig.topLimit) { + if (curPosition > armConfig.bottomLimit) { + armMasterMotor.setVoltage(MathUtil.clamp(voltage, -armConfig.armMaxVolts, armConfig.armMaxVolts)); + } + else { + armMasterMotor.setVoltage(MathUtil.clamp(voltage, 0, armConfig.armMaxVolts)); + } + } + else { + armMasterMotor.setVoltage(MathUtil.clamp(voltage, -armConfig.armMaxVolts, 0)); + } + } + private void configMotors() { configFollowMotors(); configMasterMotor(); } - private void configFollowMotors() { + /** + * Configures follow motors to follow the master, and also to brake, and have the correct inverse for encoder readings, and to follow the master with the correct inverse. + */ + protected void configFollowMotors() { if (armConfig.armFollowId != null) { armFollowMotors = new SparkBase[armConfig.armFollowId.length]; @@ -203,6 +411,24 @@ private void configFollowMotors() { maxFollowConfig.idleMode(IdleMode.kBrake); for (int i = 0; i < armConfig.armFollowId.length; i++) { + if (armConfig.armFollowId[i] == armConfig.armMotorOfBackupRelativeEncoderId) { + if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { + SparkFlexConfig flexConfig = new SparkFlexConfig(); + SparkFlex dummyFlex = MotorControllerFactory.createSparkFlex(armConfig.armFollowId[i]); + flexConfig.encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60 * 360); //degrees per second + dummyFlex.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armBackupEncoder = dummyFlex.getEncoder(); + } + else { + SparkMaxConfig maxConfig = new SparkMaxConfig(); + SparkMax dummyMax = MotorControllerFactory.createSparkMax(armConfig.armFollowId[i], armConfig.armFollowMotorType[i]); + maxConfig.encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60 *360); //degrees per second + dummyMax.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armBackupEncoder = dummyMax.getEncoder(); + } + } if (armConfig.armFollowMotorType[i] == MotorConfig.NEO_VORTEX) { flexFollowConfig.follow(armConfig.armMasterMotorId, !(armConfig.armMasterInversed && armConfig.armFollowInversed[i])); SparkFlex followFlex = MotorControllerFactory.createSparkFlex(armConfig.armFollowId[i]); @@ -219,21 +445,30 @@ private void configFollowMotors() { } } - private void configMasterMotor() { + /** + * Configures master motor with brake and correct inverse config + */ + protected void configMasterMotor() { if (armConfig.armMasterMotorType == MotorConfig.NEO_VORTEX) { - SparkFlexConfig flexMasterConfig = new SparkFlexConfig(); - flexMasterConfig.inverted(armConfig.armMasterInversed); - flexMasterConfig.idleMode(IdleMode.kBrake); - armMasterMotor = MotorControllerFactory.createSparkFlex(armConfig.armMasterMotorId); - armMasterMotor.configure(flexMasterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - } - else { - SparkMaxConfig maxMasterConfig = new SparkMaxConfig(); - maxMasterConfig.inverted(armConfig.armMasterInversed); - maxMasterConfig.idleMode(IdleMode.kBrake); + SparkFlexConfig flexConfig = new SparkFlexConfig(); + this.armMasterMotor = MotorControllerFactory.createSparkFlex(armConfig.armMasterMotorId); + flexConfig.inverted(armConfig.armMasterInversed) + .idleMode(IdleMode.kBrake) + .encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60 * 360); //degrees per second + armMasterMotor.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFeedbackEncoder = armMasterMotor.getEncoder(); + } + else { + SparkMaxConfig maxConfig = new SparkMaxConfig(); armMasterMotor = MotorControllerFactory.createSparkMax(armConfig.armMasterMotorId, armConfig.armMasterMotorType); - armMasterMotor.configure(maxMasterConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - } + maxConfig.inverted(armConfig.armMasterInversed) + .idleMode(IdleMode.kBrake) + .encoder.positionConversionFactor(armConfig.armRelativeGearReduction * 360) // Degrees + .velocityConversionFactor(armConfig.armRelativeGearReduction/60* 360); //degrees per second + armMasterMotor.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + armFeedbackEncoder = armMasterMotor.getEncoder(); + } } /** @@ -299,24 +534,50 @@ public void stopArm() { /** * Use this to do any fun stuff you want to do, for armSubsystemPeriodic */ - private void userPeriodic() { + protected void userPeriodic() { } - private void postSmartDashboardValues() { - SmartDashboard.putData(this); - SmartDashboard.putNumber("armVelocity", armFeedbackEncoder.getVelocity()); - SmartDashboard.putNumber("armPositionDegrees", armFeedbackEncoder.getPosition()); //From vertical down 0 + protected void postSmartDashboardValues() { + SmartDashboard.putNumber("armVelocity (Degrees/s)", armFeedbackEncoder.getVelocity()); + SmartDashboard.putNumber("armPosition (Degrees)", armFeedbackEncoder.getPosition()); //From horizontal being 0 and up is positive switch (currentState) { case AUTO -> SmartDashboard.putString("armControlState", "Autonomous"); case MANUAL -> SmartDashboard.putString("armControlState", "Manual"); case BRAKE -> SmartDashboard.putString("armControlState", "Idling: Brake"); case COAST -> SmartDashboard.putString("armControlState", "Idling: Coast"); } - SmartDashboard.putNumber("armGoal", armGoal); + SmartDashboard.putNumber("armGoal (Degrees from horizontal)", armGoal); + SmartDashboard.putNumber("Current period of Arm (ms)", currentPeriod*1000); + + } + /** + * Checks Subsystem periodic for running under expected time limits, reports inconsitencies to user, also is usable by feedforward + */ + private void checkPeriodic() { + currentStamp = Timer.getFPGATimestamp(); + currentPeriod = currentStamp - pastStamp; + pastStamp = currentStamp; + if (currentPeriod > 0.02) { + DriverStation.reportWarning("Arm Periodic is slow: " + currentPeriod *1000 + " ms. Check for any lag reasons", true); + } } + /** + * @return current calculated period in seconds + */ + protected double getCurrentPeriod() { + return currentPeriod; + } + + /*Overide these methods to customize what the mechanism does in different states */ + protected void handleAuto() { autoArm();} + protected void handleManual() { manualArm();} + protected void handleBrake() { stopArm();} + protected void handleCoast() { stopArm();} + + /** * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! * Use {@link #userPeriodic()} instead. @@ -324,14 +585,15 @@ private void postSmartDashboardValues() { @Override public void periodic() { switch (currentState) { - case AUTO -> autoArm(); - case MANUAL -> manualArm(); - case BRAKE -> stopArm(); - case COAST -> stopArm(); + case AUTO -> handleAuto(); + case MANUAL -> handleManual(); + case BRAKE -> handleBrake(); + case COAST -> handleCoast(); } checkFeedBackEncoder(); userPeriodic(); postSmartDashboardValues(); + checkPeriodic(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java new file mode 100644 index 0000000..46d11e8 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/SimpleSetArmGoal.java @@ -0,0 +1,39 @@ +// 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 org.carlmontrobotics.lib199.SimpleMechs.Arm; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleSetArmGoal extends Command { + /** Creates a new MoveArm. */ + SimpleArm arm; + double goal; + public SimpleSetArmGoal(SimpleArm arm, double goal) { + addRequirements(this.arm = arm); + this.goal = goal; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + arm.setAutoOn(); + arm.setGoal(goal); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return arm.atGoal(); + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java index 7bed80d..ec64e62 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java @@ -2,6 +2,9 @@ import java.util.Arrays; import java.util.Map; import java.util.function.Supplier; + +import javax.print.attribute.standard.PrinterInfo; + import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; import org.carlmontrobotics.lib199.SensorFactory; @@ -30,6 +33,7 @@ import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.ClosedLoopConfig; import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor; @@ -63,6 +67,7 @@ import edu.wpi.first.units.MutableMeasure; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.util.sendable.SendableBuilder; import edu.wpi.first.util.sendable.SendableRegistry; import edu.wpi.first.wpilibj.DriverStation; @@ -98,15 +103,17 @@ public class SimpleDrivetrain extends SubsystemBase { private Pose2d autoGyroOffset = new Pose2d(0., 0., new Rotation2d(0.)); // ^used by PathPlanner for chaining paths private SwerveDriveKinematics kinematics = null; - // private SwerveDriveOdometry odometry = null; + private SwerveDriveOdometry odometry = null; private SwerveDrivePoseEstimator poseEstimator = null; private SwerveModule modules[]; private boolean fieldOriented = true; private double fieldOffset = 0; - private SparkFlex[] driveMotors = new SparkFlex[] { null, null, null, null }; - private SparkMax[] turnMotors = new SparkMax[] { null, null, null, null }; + private SparkFlex[] flexDriveMotors = new SparkFlex[] { null, null, null, null }; + private SparkFlex[] flexTurnMotors = new SparkFlex[] { null, null, null, null }; + private SparkMax[] maxDriveMotors = new SparkMax[] { null, null, null, null }; + private SparkMax[] maxTurnMotors = new SparkMax[] { null, null, null, null }; private CANcoder[] turnEncoders = new CANcoder[] { null, null, null, null }; private final SparkClosedLoopController[] turnPidControllers = new SparkClosedLoopController[] {null, null, null, null}; public final float initPitch; @@ -151,42 +158,49 @@ public enum Mode { } //Constants - double g = 9.8; //m/s^2 - double mu = 1; - double autoCentripetalAccel = mu * g * 2; + private double g = 9.8; //m/s^2 + private double mu = 1; + private double autoCentripetalAccel = mu * g * 2; //Extra - double wheelBase; - double trackWidth; + private double wheelBase; + private double trackWidth; - int driveFrontLeftPort; - int turnFrontLeftPort; - int canCoderPortFL; + private int driveFrontLeftPort; + private int turnFrontLeftPort; + private int canCoderPortFL; - int driveFrontRightPort; - int turnFrontRightPort; - int canCoderPortFR; + private int driveFrontRightPort; + private int turnFrontRightPort; + private int canCoderPortFR; - int driveBackLeftPort; - int turnBackLeftPort; - int canCoderPortBL; + private int driveBackLeftPort; + private int turnBackLeftPort; + private int canCoderPortBL; + + private int driveBackRightPort; + private int turnBackRightPort; + private int canCoderPortBR; + + private double secsPer12Volts; + private double wheelDiameterMeters; + private double driveGearing; + private double turnGearing; + + private boolean isGyroReversed; + private double maxSpeed; - int driveBackRightPort; - int turnBackRightPort; - int canCoderPortBR; + private RobotConfig robotConfig; - double secsPer12Volts; - double wheelDiameterMeters; - double driveGearing; - double turnGearing; + private double COLLISION_ACCELERATION_THRESHOLD; - boolean isGyroReversed; - double maxSpeed; + private boolean setupSysId; - RobotConfig robotConfig; + private boolean useFlexDrive; + private boolean useFlexTurn; - double COLLISION_ACCELERATION_THRESHOLD; + private Rotation2d simGyroOffset = new Rotation2d(); @@ -205,7 +219,9 @@ public SimpleDrivetrain( double secsPer12Volts, double wheelDiameterMeters, double driveGearing, double turnGearing, boolean isGyroReversed, double maxSpeed, RobotConfig robotConfig, - double COLLISION_ACCELERATION_THRESHOLD + double COLLISION_ACCELERATION_THRESHOLD, + boolean useFlexDrive, + boolean useFlexTurn ) { this.swerveConfig = swerveConfig; @@ -231,6 +247,8 @@ public SimpleDrivetrain( this.maxSpeed = maxSpeed; this.robotConfig = robotConfig; this.COLLISION_ACCELERATION_THRESHOLD = COLLISION_ACCELERATION_THRESHOLD; + this.useFlexDrive = useFlexDrive; + this.useFlexTurn = useFlexDrive; AutoBuilder(); @@ -276,111 +294,237 @@ public SimpleDrivetrain( // Supplier pitchSupplier = () -> gyro.getPitch(); // Supplier rollSupplier = () -> gyro.getRoll(); - - moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, - driveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), - turnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), - turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); - //SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); - moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, - driveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), - turnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), - turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); - - moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, - driveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), - turnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), - turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); - - moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, - driveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), - turnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), - turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); - modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; - turnPidControllers[0] = turnMotors[0].getClosedLoopController(); - turnPidControllers[1] = turnMotors[1].getClosedLoopController(); - turnPidControllers[2] = turnMotors[2].getClosedLoopController(); - turnPidControllers[3] = turnMotors[3].getClosedLoopController(); + if (useFlexDrive) { + if (useFlexTurn) { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + flexDriveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), + flexTurnMotors[0] = MotorControllerFactory.createSparkFlex(turnFrontLeftPort), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + flexDriveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), + flexTurnMotors[1] = MotorControllerFactory.createSparkFlex(turnFrontRightPort), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + flexDriveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), + flexTurnMotors[2] = MotorControllerFactory.createSparkFlex(turnBackLeftPort), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + flexDriveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), + flexTurnMotors[3] = MotorControllerFactory.createSparkFlex(turnBackRightPort), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkFlexConfig driveConfig = new SparkFlexConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkFlex driveMotor : flexDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkFlexConfig turnConfig = new SparkFlexConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkFlex turnMotor : flexTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = flexTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = flexTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = flexTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = flexTurnMotors[3].getClosedLoopController(); + } + else { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + flexDriveMotors[0] = MotorControllerFactory.createSparkFlex(driveFrontLeftPort), + maxTurnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + flexDriveMotors[1] = MotorControllerFactory.createSparkFlex(driveFrontRightPort), + maxTurnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + flexDriveMotors[2] = MotorControllerFactory.createSparkFlex(driveBackLeftPort), + maxTurnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + flexDriveMotors[3] = MotorControllerFactory.createSparkFlex(driveBackRightPort), + maxTurnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkFlexConfig driveConfig = new SparkFlexConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkFlex driveMotor : flexDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : maxTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = maxTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = maxTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = maxTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = maxTurnMotors[3].getClosedLoopController(); + } + } + else { + if (useFlexTurn) { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + maxDriveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), + flexTurnMotors[0] = MotorControllerFactory.createSparkFlex(turnFrontLeftPort), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + maxDriveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), + flexTurnMotors[1] = MotorControllerFactory.createSparkFlex(turnFrontRightPort), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + maxDriveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), + flexTurnMotors[2] = MotorControllerFactory.createSparkFlex(turnBackLeftPort), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + maxDriveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), + flexTurnMotors[3] = MotorControllerFactory.createSparkFlex(turnBackRightPort), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkMax driveMotor : maxDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkFlexConfig turnConfig = new SparkFlexConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkFlex turnMotor : flexTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = flexTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = flexTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = flexTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = flexTurnMotors[3].getClosedLoopController(); + } + else { + moduleFL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FL, + maxDriveMotors[0] = MotorControllerFactory.createSparkMax(driveFrontLeftPort, MotorConfig.NEO), + maxTurnMotors[0] = MotorControllerFactory.createSparkMax(turnFrontLeftPort, MotorConfig.NEO), + turnEncoders[0] = SensorFactory.createCANCoder(canCoderPortFL), 0, pitchSupplier, rollSupplier); + moduleFR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.FR, + maxDriveMotors[1] = MotorControllerFactory.createSparkMax(driveFrontRightPort, MotorConfig.NEO), + maxTurnMotors[1] = MotorControllerFactory.createSparkMax(turnFrontRightPort, MotorConfig.NEO), + turnEncoders[1] = SensorFactory.createCANCoder(canCoderPortFR), 1, pitchSupplier, rollSupplier); + + moduleBL = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BL, + maxDriveMotors[2] = MotorControllerFactory.createSparkMax(driveBackLeftPort, MotorConfig.NEO), + maxTurnMotors[2] = MotorControllerFactory.createSparkMax(turnBackLeftPort, MotorConfig.NEO), + turnEncoders[2] = SensorFactory.createCANCoder(canCoderPortBL), 2, pitchSupplier, rollSupplier); + + moduleBR = new SwerveModule(swerveConfig, SwerveModule.ModuleType.BR, + maxDriveMotors[3] = MotorControllerFactory.createSparkMax(driveBackRightPort, MotorConfig.NEO), + maxTurnMotors[3] = MotorControllerFactory.createSparkMax(turnBackRightPort, MotorConfig.NEO), + turnEncoders[3] = SensorFactory.createCANCoder(canCoderPortBR), 3, pitchSupplier, rollSupplier); + modules = new SwerveModule[] { moduleFL, moduleFR, moduleBL, moduleBR }; + SparkMaxConfig driveConfig = new SparkMaxConfig(); + driveConfig.openLoopRampRate(secsPer12Volts); + driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); + driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); + driveConfig.encoder.uvwAverageDepth(2); + driveConfig.encoder.uvwMeasurementPeriod(16); + driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); + + for (SparkMax driveMotor : maxDriveMotors) { + driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + SparkMaxConfig turnConfig = new SparkMaxConfig(); + turnConfig.encoder.positionConversionFactor(360/turnGearing); + turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); + turnConfig.encoder.uvwAverageDepth(2); + turnConfig.encoder.uvwMeasurementPeriod(16); + + //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); + for (SparkMax turnMotor : maxTurnMotors) { + turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + turnPidControllers[0] = maxTurnMotors[0].getClosedLoopController(); + turnPidControllers[1] = maxTurnMotors[1].getClosedLoopController(); + turnPidControllers[2] = maxTurnMotors[2].getClosedLoopController(); + turnPidControllers[3] = maxTurnMotors[3].getClosedLoopController(); + } + } if (RobotBase.isSimulation()) { moduleSims = new SwerveModuleSim[] { moduleFL.createSim(), moduleFR.createSim(), moduleBL.createSim(), moduleBR.createSim() }; gyroYawSim = new SimDeviceSim("navX-Sensor[0]").getDouble("Yaw"); } + SmartDashboard.putData("Module FL",moduleFL); SmartDashboard.putData("Module FR",moduleFR); SmartDashboard.putData("Module BL",moduleBL); SmartDashboard.putData("Module BR",moduleBR); - SmartDashboard.putNumber("bigoal", 0); - - SparkMaxConfig driveConfig = new SparkMaxConfig(); - driveConfig.openLoopRampRate(secsPer12Volts); - driveConfig.encoder.positionConversionFactor(wheelDiameterMeters * Math.PI / driveGearing); - driveConfig.encoder.velocityConversionFactor(wheelDiameterMeters * Math.PI / driveGearing / 60); - driveConfig.encoder.uvwAverageDepth(2); - driveConfig.encoder.uvwMeasurementPeriod(16); - driveConfig.smartCurrentLimit(MotorConfig.NEO.currentLimitAmps); - - for (SparkFlex driveMotor : driveMotors) { - driveMotor.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - } - SparkMaxConfig turnConfig = new SparkMaxConfig(); - turnConfig.encoder.positionConversionFactor(360/turnGearing); - turnConfig.encoder.velocityConversionFactor(360/turnGearing/60); - turnConfig.encoder.uvwAverageDepth(2); - turnConfig.encoder.uvwMeasurementPeriod(16); - - //turnConfig.closedLoop.pid(kP, kI, kD).feedbackSensor(FeedbackSensor.kPrimaryEncoder); - for (SparkMax turnMotor : turnMotors) { - turnMotor.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - } - for (CANcoder coder : turnEncoders) { coder.getAbsolutePosition().setUpdateFrequency(500); coder.getPosition().setUpdateFrequency(500); coder.getVelocity().setUpdateFrequency(500); } - - //SmartDashboard.putData("Field", field); - //SmartDashboard.putData("Odometry Field", odometryField); - //martDashboard.putData("Pose with Limelight Field", poseWithLimelightField); accelX = gyro.getWorldLinearAccelX(); // Acceleration along the X-axis accelY = gyro.getWorldLinearAccelY(); // Acceleration along the Y-axis accelXY = Math.sqrt(gyro.getWorldLinearAccelX() * gyro.getWorldLinearAccelX() + gyro.getWorldLinearAccelY() * gyro.getWorldLinearAccelY()); - // for(SparkMax driveMotor : driveMotors) - // driveMotor.setSmartCurrentLimit(80); // Must call this method for SysId to run - if (CONFIG.isSysIdTesting()) { + if (setupSysId) { sysIdSetup(); } } - // odometry = new SwerveDriveOdometry(kinematics, - // Rotation2d.fromDegrees(getHeading()), getModulePositions(), - // new Pose2d()); - + odometry = new SwerveDriveOdometry(kinematics, + Rotation2d.fromDegrees(getHeading()), getModulePositions(), + new Pose2d()); poseEstimator = new SwerveDrivePoseEstimator( getKinematics(), Rotation2d.fromDegrees(getHeading()), getModulePositions(), new Pose2d()); - - // Setup autopath builder - //configurePPLAutoBuilder(); - // SmartDashboard.putNumber("chassis speeds x", 0); - // SmartDashboard.putNumber("chassis speeds y", 0); - - // SmartDashboard.putNumber("chassis speeds theta", 0); - SmartDashboard.putData(this); // For seeing drivetrain data in SmartDashboard + + SmartDashboard.putData(this); } - public boolean isAtAngle(double desiredAngleDeg, double toleranceDeg){ for (SwerveModule module : modules) { if (!(Math.abs(MathUtil.inputModulus(module.getModuleAngle() - desiredAngleDeg, -90, 90)) < toleranceDeg)) @@ -441,79 +585,44 @@ public void setDrivingIdleMode(boolean brake) { else { mode = IdleMode.kCoast; } - for (SparkMax turnMotor : turnMotors) { - SparkMaxConfig config = new SparkMaxConfig(); - config.idleMode(mode); - turnMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + if (useFlexDrive) { + for (SparkFlex driveMotor : flexDriveMotors) { + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(mode); + driveMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } } - for (SparkFlex driveMotor : driveMotors) { - SparkMaxConfig config = new SparkMaxConfig(); - config.idleMode(mode); - driveMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + else { + for (SparkMax driveMotor : maxDriveMotors) { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(mode); + driveMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } } + if (useFlexTurn) { + for (SparkFlex turnMotor : flexTurnMotors) { + SparkFlexConfig config = new SparkFlexConfig(); + config.idleMode(mode); + turnMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + else { + for (SparkMax turnMotor : maxTurnMotors) { + SparkMaxConfig config = new SparkMaxConfig(); + config.idleMode(mode); + turnMotor.configure(config, SparkBase.ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + } @Override public void periodic() { detectCollision(); //This does nothing PathPlannerLogging.logCurrentPose(getPose()); - - //maybe add the field with the position of the robot with only limelight and the field with the position of the robot with only odometry? - //We can compare the two fields to see if odometry is causing the pose to be inaccurate when it hits the reef. - - // SmartDashboard.getNumber("GoalPos", turnEncoders[0].getVelocity().getValueAsDouble()); - // SmartDashboard.putNumber("FL Motor Val", turnMotors[0].getEncoder().getPosition()); - // double goal = SmartDashboard.getNumber("GoalPos", 0); - // PIDController pid = new PIDController(kP, kI, kD); - // kP = SmartDashboard.getNumber("kP", 0); - // kI = SmartDashboard.getNumber("kI", 0); - // kD = SmartDashboard.getNumber("kD", 0); - //pid.setIZone(20); - //SmartDashboard.putBoolean("atgoal", pid.atSetpoint()); - // SparkMaxConfig config = new SparkMaxConfig(); - - //config.closedLoop.feedbackSensor(ClosedLoopConfig.FeedbackSensor.kPrimaryEncoder); - // System.out.println(kP); - // config.closedLoop.pid(kP ,kI,kD); - // config.encoder.positionConversionFactor(360/Constants.Drivetrainc.turnGearing); - // turnMotors[0].configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - // //moduleFL.move(0.0000001, 180); - //moduleFL.move(0.01, 180); - // moduleFR.move(0.000000001, 0); - // moduleBR.move(0.0000001, 0); - // moduleFL.move(0.000001, 0); - // moduleBL.move(0.000001, 0); - // turnPidControllers[0].setReference(goal - - // , ControlType.kPosition, ClosedLoopSlot.kSlot0); - - // 167 -> -200 - // 138 -> 360 - // for (CANcoder coder : turnEncoders) { - // SignalLogger.writeDouble("Regular position " + coder.toString(), - // coder.getPosition().getValue().baseUnitMagnitude()); - // SignalLogger.writeDouble("Velocity " + coder.toString(), - // coder.getVelocity().getValue().baseUnitMagnitude()); - // SignalLogger.writeDouble("Absolute position " + coder.toString(), - // coder.getAbsolutePosition().getValue().baseUnitMagnitude()); - // } - // String out=""; int i=0; - // for (CANcoder coder : turnEncoders) { - // out+=String.format("[i] Abs Pos: %.3f Goal Pos: %.3f ", coder.getAbsolutePosition().getValue().baseUnitMagnitude(),0); - // i++; - // } - // lobotomized to prevent ucontrollabe swerve behavior - // turnMotors[2].setVoltage(SmartDashboard.getNumber("kS", 0)); - // moduleFL.periodic(); - // moduleFR.periodic(); - // moduleBL.periodic(); - // moduleBR.periodic(); - double goal = SmartDashboard.getNumber("bigoal", 0); for (SwerveModule module : modules) { - // module.turnPeriodic(); - // module.turnPeriodic(); - module.move(0.00000000001, goal); + // module.turnPeriodic(); For testing module.periodic(); } @@ -551,7 +660,7 @@ public void periodic() { // SmartDashboard.putNumber("Y position with limelight", getPoseWithLimelight().getY()); SmartDashboard.putNumber("X position with gyro", getPose().getX()); SmartDashboard.putNumber("Y position with gyro", getPose().getY()); - SmartDashboard.putData(CONFIG); + SmartDashboard.putBoolean("setupSysId", setupSysId); //For finding acceleration of drivetrain for collision detector SmartDashboard.putNumber("Accel X", accelX); @@ -572,6 +681,11 @@ public void periodic() { SmartDashboard.putNumber("front right encoder", moduleFR.getModuleAngle()); SmartDashboard.putNumber("back left encoder", moduleBL.getModuleAngle()); SmartDashboard.putNumber("back right encoder", moduleBR.getModuleAngle()); + userPeriodic(); + } + + public void userPeriodic() { + //Override this } @Override @@ -691,12 +805,12 @@ public void AutoBuilder() { //---------------------------------------------------------- public void autoCancelDtCommand() { - if(!(getDefaultCommand() instanceof TeleopDrive) || DriverStation.isAutonomous()) return; + if(!(getDefaultCommand() instanceof SimpleTeleopDrive) || DriverStation.isAutonomous()) return; // Use hasDriverInput to get around acceleration limiting on slowdown - if (((TeleopDrive) getDefaultCommand()).hasDriverInput()) { + if (((SimpleTeleopDrive) getDefaultCommand()).hasDriverInput()) { Command currentDtCommand = getCurrentCommand(); - if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof RotateToFieldRelativeAngle) + if (currentDtCommand != getDefaultCommand() && !(currentDtCommand instanceof SimpleRotateToFieldRelativeAngle) && currentDtCommand != null) { currentDtCommand.cancel(); } @@ -784,40 +898,19 @@ public Pose2d getPose() { // return odometry.getPoseMeters(); return poseEstimator.getEstimatedPosition(); } - - private Rotation2d simGyroOffset = new Rotation2d(); + public void setPose(Pose2d initialPose) { Rotation2d gyroRotation = gyro.getRotation2d(); - // odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); - poseEstimator.resetPosition(gyroRotation, getModulePositions(), initialPose); - // Remember the offset that the above call to resetPosition() will cause the odometry.update() will add to the gyro rotation in the future - // We need the offset so that we can compensate for it during simulationPeriodic(). simGyroOffset = initialPose.getRotation().minus(gyroRotation); - //odometry.resetPosition(Rotation2d.fromDegrees(getHeading()), getModulePositions(), initialPose); + odometry.resetPosition(gyroRotation, getModulePositions(), initialPose); } //This method will set the pose using limelight if it sees a tag and if not it is supposed to run like setPose() - public void setPoseWithLimelight(Pose2d backupPose){ //the pose will be set to backupPose if no tag is seen - // Rotation2d gyroRotation = gyro.getRotation2d(); - // Pose2d pose; - - // if (LimelightHelpers.getTV(REEF_LL)) { - - // pose = LimelightHelpers.getBotPose2d_wpiBlue(REEF_LL); - - // } else if (LimelightHelpers.getTV(CORAL_LL)) { - - // pose = LimelightHelpers.getBotPose2d_wpiBlue(CORAL_LL); - // } - // else { - // pose = backupPose; - // } - - // poseEstimator.resetPosition(gyroRotation, getModulePositions(), pose); - // simGyroOffset = pose.getRotation().minus(gyroRotation); - + public void setPoseWithLimelight(Pose2d backupPose){ + DriverStation.reportError("Need to override setPoseWithLimelight and make the method setPose", true); } + /** * Detects if the robot has experienced a collision based on acceleration thresholds. * @return true if a 2D acceleration greater than the {@link #COLLISION_ACCELERATION_THRESHOLD} false otherwise. @@ -845,6 +938,7 @@ public double getPitch() { public double getRoll() { return gyro.getRoll(); } + /** * true stands for fieldOriented, false stands for robotOriented * @return boolean @@ -860,18 +954,21 @@ public boolean getFieldOriented() { public void setFieldOriented(boolean fieldOriented) { this.fieldOriented = fieldOriented; } + /** * Sets the current direction the robot is facing is to be 0 */ public void resetFieldOrientation() { fieldOffset = gyro.getAngle(); } + /** * Sets the current direction the robot is facing is to be 180 */ public void resetFieldOrientationBackwards() { fieldOffset = 180 + gyro.getAngle(); } + /** * Sets the current direction the robot is facing is plus @param angle to be 0 * @param angle in degrees @@ -879,9 +976,8 @@ public void resetFieldOrientationBackwards() { public void resetFieldOrientationWithAngle(double angle) { fieldOffset = angle + gyro.getAngle(); } - public void resetPoseEstimator() { - // odometry.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); + public void resetPoseEstimator() { poseEstimator.resetPosition(new Rotation2d(), getModulePositions(), new Pose2d()); gyro.reset(); } @@ -977,14 +1073,26 @@ private enum SysIdTest { // DRIVE private void motorLogShort_drive(SysIdRoutineLog log, int id) { - String name = new String[] { "fl", "fr", "bl", "br" }[id]; + if (useFlexDrive) { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; log.motor(name) .voltage(m_appliedVoltage[id].mut_replace( - driveMotors[id].getBusVoltage() * driveMotors[id].getAppliedOutput(), Volts)) + flexDriveMotors[id].getBusVoltage() * flexDriveMotors[id].getAppliedOutput(), Volts)) .linearPosition( - m_distance[id].mut_replace(driveMotors[id].getEncoder().getPosition(), Meters)) - .linearVelocity(m_velocity[id].mut_replace(driveMotors[id].getEncoder().getVelocity(), + m_distance[id].mut_replace(flexDriveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(flexDriveMotors[id].getEncoder().getVelocity(), MetersPerSecond)); + } + else { + String name = new String[] { "fl", "fr", "bl", "br" }[id]; + log.motor(name) + .voltage(m_appliedVoltage[id].mut_replace( + maxDriveMotors[id].getBusVoltage() * maxDriveMotors[id].getAppliedOutput(), Volts)) + .linearPosition( + m_distance[id].mut_replace(maxDriveMotors[id].getEncoder().getPosition(), Meters)) + .linearVelocity(m_velocity[id].mut_replace(maxDriveMotors[id].getEncoder().getVelocity(), + MetersPerSecond)); + } } // Create a new SysId routine for characterizing the drive. @@ -993,8 +1101,14 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { new SysIdRoutine.Mechanism( // Tell SysId how to give the driving voltage to the motors. (Voltage volts) -> { - driveMotors[0].setVoltage(volts.in(Volts)); - driveMotors[1].setVoltage(volts.in(Volts)); + if (useFlexDrive) { + flexDriveMotors[0].setVoltage(volts.in(Volts)); + flexDriveMotors[1].setVoltage(volts.in(Volts)); + } + else { + maxDriveMotors[0].setVoltage(volts.in(Volts)); + maxDriveMotors[1].setVoltage(volts.in(Volts)); + } modules[2].coast(); modules[3].coast(); }, @@ -1012,8 +1126,14 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { modules[1].coast(); modules[2].brake(); modules[3].brake(); - driveMotors[2].setVoltage(volts.in(Volts)); - driveMotors[3].setVoltage(volts.in(Volts)); + if (useFlexDrive) { + flexDriveMotors[2].setVoltage(volts.in(Volts)); + flexDriveMotors[3].setVoltage(volts.in(Volts)); + } + else { + maxDriveMotors[2].setVoltage(volts.in(Volts)); + maxDriveMotors[3].setVoltage(volts.in(Volts)); + } }, log -> {// BACK motorLogShort_drive(log, 2);// bl @@ -1025,8 +1145,15 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { defaultSysIdConfig, new SysIdRoutine.Mechanism( (Voltage volts) -> { - for (SparkFlex dm : driveMotors) { - dm.setVoltage(volts.in(Volts)); + if (useFlexDrive) { + for (SparkFlex dm : flexDriveMotors) { + dm.setVoltage(volts.in(Volts)); + } + } + else { + for (SparkMax dm : maxDriveMotors) { + dm.setVoltage(volts.in(Volts)); + } } }, log -> { @@ -1038,16 +1165,26 @@ private void motorLogShort_drive(SysIdRoutineLog log, int id) { this)); private SysIdRoutine sysidroutshort_turn(int id, String logname) { + double busVoltage; + double appliedOutput; + if (useFlexTurn) { + busVoltage = flexTurnMotors[id].getBusVoltage(); + appliedOutput = flexTurnMotors[id].getAppliedOutput(); + } + else { + busVoltage = maxTurnMotors[id].getBusVoltage(); + appliedOutput = maxTurnMotors[id].getAppliedOutput(); + } return new SysIdRoutine( defaultSysIdConfig, // new SysIdRoutine.Config(Volts.of(.1).per(Seconds.of(.1)), Volts.of(.6), // Seconds.of(3)), new SysIdRoutine.Mechanism( - (Voltage volts) -> turnMotors[id].setVoltage(volts.in(Volts)), + (Voltage volts) -> {if (useFlexTurn) {flexTurnMotors[id].setVoltage(volts.in(Volts));} else {maxTurnMotors[id].setVoltage(volts.in(Volts));}}, log -> log.motor(logname + "_turn") .voltage(m_appliedVoltage[id + 4].mut_replace( // ^because drivemotors take up the first 4 slots of the unit holders - turnMotors[id].getBusVoltage() * turnMotors[id].getAppliedOutput(), Volts)) + busVoltage * appliedOutput, Volts)) .angularPosition( m_revs[id].mut_replace(turnEncoders[id].getPosition().getValue())) .angularVelocity(m_revs_vel[id].mut_replace( @@ -1085,46 +1222,6 @@ private void sysIdSetup() { Supplier stopNwait = () -> new SequentialCommandGroup( new InstantCommand(this::stop), new WaitCommand(2)); - /* - * Alex's old sysId tests - * sysIdTab.add("All sysid tests", new SequentialCommandGroup( - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,2), - * (Command)stopNwait.get()), - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,2), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,2), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,2), - * (Command)stopNwait.get()) - * )); - * sysIdTab.add("All sysid tests - FRONT wheels", new SequentialCommandGroup( - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,0), - * (Command)stopNwait.get()), - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,0), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,0), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,0), - * (Command)stopNwait.get()) - * )); - * sysIdTab.add("All sysid tests - BACK wheels", new SequentialCommandGroup( - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kForward,1), - * (Command)stopNwait.get()), - * new - * SequentialCommandGroup(sysIdQuasistatic(SysIdRoutine.Direction.kReverse,1), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kForward,1), - * (Command)stopNwait.get()), - * new SequentialCommandGroup(sysIdDynamic(SysIdRoutine.Direction.kReverse,1), - * (Command)stopNwait.get()) - * )); - */ - // sysidtabshorthand_qsi("Quasistatic Forward", SysIdRoutine.Direction.kForward); // sysidtabshorthand_qsi("Quasistatic Backward", SysIdRoutine.Direction.kReverse); // sysidtabshorthand_dyn("Dynamic Forward", SysIdRoutine.Direction.kForward); diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java index e69de29..8382052 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleRotateToFieldRelativeAngle.java @@ -0,0 +1,52 @@ +package org.carlmontrobotics.lib199.SimpleMechs.Drivetrain; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.util.sendable.SendableRegistry; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; + +public class SimpleRotateToFieldRelativeAngle extends Command { + + public final SimpleTeleopDrive teleopDrive; + public final SimpleDrivetrain drivetrain; + + double[] thetaPIDController = {0.1,0,0}; + double positionTolerance = 5; //degrees + double velocityTolerance = 5; //degrees/sec + + public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1], + thetaPIDController[2]); + + public SimpleRotateToFieldRelativeAngle(Rotation2d angle, SimpleDrivetrain drivetrain) { + this.drivetrain = drivetrain; + this.teleopDrive = (SimpleTeleopDrive) drivetrain.getDefaultCommand(); + + rotationPID.enableContinuousInput(-180, 180); + rotationPID.setSetpoint(MathUtil.inputModulus(angle.getDegrees(), -180, 180)); + rotationPID.setTolerance(positionTolerance, velocityTolerance); + SendableRegistry.addChild(this, rotationPID); + // SmartDashboard.pu + + addRequirements(drivetrain); + } + + @Override + public void execute() { + if (teleopDrive == null) + drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading())); + else { + double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds(); + drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1], + rotationPID.calculate(drivetrain.getHeading())); + } + } + + @Override + public boolean isFinished() { + //SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint()); + //SmartDashboard.putNumber("Error", rotationPID.getPositionError()); + return rotationPID.atSetpoint(); + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java index 93fb739..97ea3ce 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleTeleopDrive.java @@ -11,7 +11,7 @@ public class SimpleTeleopDrive extends Command { - private static double robotPeriod = Robot.kDefaultPeriod; + private static double robotPeriod = 0.2; private final SimpleDrivetrain drivetrain; private DoubleSupplier fwd; private DoubleSupplier str; @@ -23,11 +23,25 @@ public class SimpleTeleopDrive extends Command { GenericHID manipulatorController; BooleanSupplier babyModeSupplier; + + //Extra + private final double maxForward; + private final double maxStrafe; + private final double maxRCW; + private final double kSlowDriveSpeed = 0.4; + private final double kNormalDriveSpeed = 1; + private final double kSlowDriveRotation = 0.25; + private final double kNormalDriveRotation = 0.5; + private final double kBabyDriveSpeed = 0.3; + private final double kBabyDriveRotation = 0.2; + private final double JOY_THRESH = 0.13; + + /** * Creates a new TeleopDrive. */ public SimpleTeleopDrive(SimpleDrivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str, DoubleSupplier rcw, - BooleanSupplier slow, GenericHID manipulatorController, BooleanSupplier babyModeSupplier) { + BooleanSupplier slow, GenericHID manipulatorController, BooleanSupplier babyModeSupplier, double maxSpeed, double swerveRadius ) { addRequirements(this.drivetrain = drivetrain); this.fwd = fwd; this.str = str; @@ -35,7 +49,10 @@ public SimpleTeleopDrive(SimpleDrivetrain drivetrain, DoubleSupplier fwd, Double this.slow = slow; this.manipulatorController = manipulatorController; this.babyModeSupplier = babyModeSupplier; - } + maxForward = maxSpeed; + maxStrafe = maxSpeed; + maxRCW = maxSpeed/swerveRadius; + } // Called when the command is initially scheduled. @Override @@ -126,9 +143,9 @@ public double[] getRequestedSpeeds() { } public boolean hasDriverInput() { - return MathUtil.applyDeadband(fwd.getAsDouble(), Constants.OI.JOY_THRESH)!=0 - || MathUtil.applyDeadband(str.getAsDouble(), Constants.OI.JOY_THRESH)!=0 - || MathUtil.applyDeadband(rcw.getAsDouble(), Constants.OI.JOY_THRESH)!=0; + return MathUtil.applyDeadband(fwd.getAsDouble(), JOY_THRESH)!=0 + || MathUtil.applyDeadband(str.getAsDouble(), JOY_THRESH)!=0 + || MathUtil.applyDeadband(rcw.getAsDouble(), JOY_THRESH)!=0; } // Called once the command ends or is interrupted. diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java new file mode 100644 index 0000000..cf4b521 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elastic.java @@ -0,0 +1,390 @@ +// Copyright (c) 2023-2025 Gold87 and other Elastic contributors +// This software can be modified and/or shared under the terms +// defined by the Elastic license: +// https://github.com/Gold872/elastic_dashboard/blob/main/LICENSE + +package org.carlmontrobotics.lib199.SimpleMechs; + +import com.fasterxml.jackson.annotation.JsonProperty; +import com.fasterxml.jackson.core.JsonProcessingException; +import com.fasterxml.jackson.databind.ObjectMapper; +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.PubSubOption; +import edu.wpi.first.networktables.StringPublisher; +import edu.wpi.first.networktables.StringTopic; + +public final class Elastic { + private static final StringTopic notificationTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/RobotNotifications"); + private static final StringPublisher notificationPublisher = + notificationTopic.publish(PubSubOption.sendAll(true), PubSubOption.keepDuplicates(true)); + private static final StringTopic selectedTabTopic = + NetworkTableInstance.getDefault().getStringTopic("/Elastic/SelectedTab"); + private static final StringPublisher selectedTabPublisher = + selectedTabTopic.publish(PubSubOption.keepDuplicates(true)); + private static final ObjectMapper objectMapper = new ObjectMapper(); + + /** + * Represents the possible levels of notifications for the Elastic dashboard. These levels are + * used to indicate the severity or type of notification. + */ + public enum NotificationLevel { + /** Informational Message */ + INFO, + /** Warning message */ + WARNING, + /** Error message */ + ERROR + } + + /** + * Sends an notification to the Elastic dashboard. The notification is serialized as a JSON string + * before being published. + * + * @param notification the {@link Notification} object containing notification details + */ + public static void sendNotification(Notification notification) { + try { + notificationPublisher.set(objectMapper.writeValueAsString(notification)); + } catch (JsonProcessingException e) { + e.printStackTrace(); + } + } + + /** + * Selects the tab of the dashboard with the given name. If no tab matches the name, this will + * have no effect on the widgets or tabs in view. + * + *

If the given name is a number, Elastic will select the tab whose index equals the number + * provided. + * + * @param tabName the name of the tab to select + */ + public static void selectTab(String tabName) { + selectedTabPublisher.set(tabName); + } + + /** + * Selects the tab of the dashboard at the given index. If this index is greater than or equal to + * the number of tabs, this will have no effect. + * + * @param tabIndex the index of the tab to select. + */ + public static void selectTab(int tabIndex) { + selectTab(Integer.toString(tabIndex)); + } + + /** + * Represents an notification object to be sent to the Elastic dashboard. This object holds + * properties such as level, title, description, display time, and dimensions to control how the + * notification is displayed on the dashboard. + */ + public static class Notification { + @JsonProperty("level") + private NotificationLevel level; + + @JsonProperty("title") + private String title; + + @JsonProperty("description") + private String description; + + @JsonProperty("displayTime") + private int displayTimeMillis; + + @JsonProperty("width") + private double width; + + @JsonProperty("height") + private double height; + + /** + * Creates a new Notification with all default parameters. This constructor is intended to be + * used with the chainable decorator methods + * + *

Title and description fields are empty. + */ + public Notification() { + this(NotificationLevel.INFO, "", ""); + } + + /** + * Creates a new Notification with all properties specified. + * + * @param level the level of the notification (e.g., INFO, WARNING, ERROR) + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the time in milliseconds for which the notification is displayed + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, + String title, + String description, + int displayTimeMillis, + double width, + double height) { + this.level = level; + this.title = title; + this.displayTimeMillis = displayTimeMillis; + this.description = description; + this.height = height; + this.width = width; + } + + /** + * Creates a new Notification with default display time and dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + */ + public Notification(NotificationLevel level, String title, String description) { + this(level, title, description, 3000, 350, -1); + } + + /** + * Creates a new Notification with a specified display time and default dimensions. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param displayTimeMillis the display time in milliseconds + */ + public Notification( + NotificationLevel level, String title, String description, int displayTimeMillis) { + this(level, title, description, displayTimeMillis, 350, -1); + } + + /** + * Creates a new Notification with specified dimensions and default display time. If the height + * is below zero, it is automatically inferred based on screen size. + * + * @param level the level of the notification + * @param title the title text of the notification + * @param description the descriptive text of the notification + * @param width the width of the notification display area + * @param height the height of the notification display area, inferred if below zero + */ + public Notification( + NotificationLevel level, String title, String description, double width, double height) { + this(level, title, description, 3000, width, height); + } + + /** + * Updates the level of this notification + * + * @param level the level to set the notification to + */ + public void setLevel(NotificationLevel level) { + this.level = level; + } + + /** + * @return the level of this notification + */ + public NotificationLevel getLevel() { + return level; + } + + /** + * Updates the title of this notification + * + * @param title the title to set the notification to + */ + public void setTitle(String title) { + this.title = title; + } + + /** + * Gets the title of this notification + * + * @return the title of this notification + */ + public String getTitle() { + return title; + } + + /** + * Updates the description of this notification + * + * @param description the description to set the notification to + */ + public void setDescription(String description) { + this.description = description; + } + + public String getDescription() { + return description; + } + + /** + * Updates the display time of the notification + * + * @param seconds the number of seconds to display the notification for + */ + public void setDisplayTimeSeconds(double seconds) { + setDisplayTimeMillis((int) Math.round(seconds * 1000)); + } + + /** + * Updates the display time of the notification in milliseconds + * + * @param displayTimeMillis the number of milliseconds to display the notification for + */ + public void setDisplayTimeMillis(int displayTimeMillis) { + this.displayTimeMillis = displayTimeMillis; + } + + /** + * Gets the display time of the notification in milliseconds + * + * @return the number of milliseconds the notification is displayed for + */ + public int getDisplayTimeMillis() { + return displayTimeMillis; + } + + /** + * Updates the width of the notification + * + * @param width the width to set the notification to + */ + public void setWidth(double width) { + this.width = width; + } + + /** + * Gets the width of the notification + * + * @return the width of the notification + */ + public double getWidth() { + return width; + } + + /** + * Updates the height of the notification + * + *

If the height is set to -1, the height will be determined automatically by the dashboard + * + * @param height the height to set the notification to + */ + public void setHeight(double height) { + this.height = height; + } + + /** + * Gets the height of the notification + * + * @return the height of the notification + */ + public double getHeight() { + return height; + } + + /** + * Modifies the notification's level and returns itself to allow for method chaining + * + * @param level the level to set the notification to + * @return the current notification + */ + public Notification withLevel(NotificationLevel level) { + this.level = level; + return this; + } + + /** + * Modifies the notification's title and returns itself to allow for method chaining + * + * @param title the title to set the notification to + * @return the current notification + */ + public Notification withTitle(String title) { + setTitle(title); + return this; + } + + /** + * Modifies the notification's description and returns itself to allow for method chaining + * + * @param description the description to set the notification to + * @return the current notification + */ + public Notification withDescription(String description) { + setDescription(description); + return this; + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param seconds the number of seconds to display the notification for + * @return the current notification + */ + public Notification withDisplaySeconds(double seconds) { + return withDisplayMilliseconds((int) Math.round(seconds * 1000)); + } + + /** + * Modifies the notification's display time and returns itself to allow for method chaining + * + * @param displayTimeMillis the number of milliseconds to display the notification for + * @return the current notification + */ + public Notification withDisplayMilliseconds(int displayTimeMillis) { + setDisplayTimeMillis(displayTimeMillis); + return this; + } + + /** + * Modifies the notification's width and returns itself to allow for method chaining + * + * @param width the width to set the notification to + * @return the current notification + */ + public Notification withWidth(double width) { + setWidth(width); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + * @param height the height to set the notification to + * @return the current notification + */ + public Notification withHeight(double height) { + setHeight(height); + return this; + } + + /** + * Modifies the notification's height and returns itself to allow for method chaining + * + *

This will set the height to -1 to have it automatically determined by the dashboard + * + * @return the current notification + */ + public Notification withAutomaticHeight() { + setHeight(-1); + return this; + } + + /** + * Modifies the notification to disable the auto dismiss behavior + * + *

This sets the display time to 0 milliseconds + * + *

The auto dismiss behavior can be re-enabled by setting the display time to a number + * greater than 0 + * + * @return the current notification + */ + public Notification withNoAutoDismiss() { + setDisplayTimeMillis(0); + return this; + } + } +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java index 525fa2b..914c63b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/ElevatorConfig.java @@ -2,9 +2,7 @@ import java.util.function.BooleanSupplier; import org.carlmontrobotics.lib199.MotorConfig; -import org.carlmontrobotics.lib199.MotorControllerFactory; -import edu.wpi.first.math.controller.ArmFeedforward; import edu.wpi.first.math.controller.ElevatorFeedforward; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.wpilibj.DriverStation; @@ -42,9 +40,30 @@ public final class ElevatorConfig { public ElevatorFeedforward elevatorFeedforward; - + /** + * Creates a elevator config for a {@link SimpleElevator} + * @param elevatorMasterId the port id of the master motor, int + * @param elevatorMasterMotorType the {@link MotorConfig} of the master motor, that is NEO_550, NEO, and NEO_VORTEX + * @param elevatorMasterInverted setting for positive direction of the master motor would make the elevator go up, boolean + * @param elevatorFollowId the port id for the follow motor, int + * @param elevatorFollowMotorType the {@link MotorConfig} of the follow motor, that is NEO_550, NEO, and NEO_VORTEX, generally should be the same as the master + * @param elevatorFollowInverted setting for positive direction of teh follow motor would make the elevator go up, boolean + * @param bottomReset a {@link BooleanSupplier}, that would be true if the elevator is fully down (Limit switch), null if none + * @param topReset a {@link BooleanSupplier}, that would be true if the elevator is fully up(Limit switch), null if none + * @param bottomLimit the lowest value possible the elevator can achieve based of encoder readings in meters, generally 0 + * @param topLimit the highest value possible the elevator can achieve based of encoder readings in meters + * @param elevatorKPID double array of [kP, kI, kD] have to be >= 0 + * @param elevatorKFeedForward double array of [kS, kG, kV, kA] all but kG have to be >= 0 + * @param gearReduction # of rotations of the motor to raise the elevator 1 meter (Elevator must have linear increase in height) + * @param elevatorPIDTolerance double, how many METERS of tolerance is the user fine with + * @param maxVolts maximum volts to use on the motors, 14 if no limit + * @param maxManualInput maximum percentage of how much voltage a user can use from 0-1.0 + * @throws IllegalArgumentException + * @throws IllegalArgumentException + * @throws IllegalArgumentException + */ public ElevatorConfig(int elevatorMasterId, MotorConfig elevatorMasterMotorType, boolean elevatorMasterInverted, - int elevatorFollowId, MotorConfig elevatorFolllowMotorType, boolean elevatorFollowInverted, + int elevatorFollowId, MotorConfig elevatorFollowMotorType, boolean elevatorFollowInverted, BooleanSupplier bottomReset, BooleanSupplier topReset, double bottomLimit, double topLimit, double[] elevatorKPID, double[] elevatorKFeedForward, double gearReduction, double elevatorPIDTolerance, double maxVolts, double maxManualInput) { @@ -53,7 +72,7 @@ public ElevatorConfig(int elevatorMasterId, MotorConfig elevatorMasterMotorType, this.elevatorMasterMotorType = elevatorMasterMotorType; this.elevatorMasterInverted = elevatorMasterInverted; this.elevatorFollowId = elevatorFollowId; - this.elevatorFollowMotorType = elevatorFolllowMotorType; + this.elevatorFollowMotorType = elevatorFollowMotorType; this.elevatorFollowInverted = elevatorFollowInverted; this.bottomReset = bottomReset; this.topReset = topReset; @@ -63,9 +82,22 @@ public ElevatorConfig(int elevatorMasterId, MotorConfig elevatorMasterMotorType, this.elevatorPIDTolerance = elevatorPIDTolerance; this.maxVolts = maxVolts; this.maxManualInput = maxManualInput; + this.elevatorKPID = elevatorKPID; + this.elevatorKFeedForward = elevatorKFeedForward; checkRequirements(); } + public static ElevatorConfig NEOElevatorConfig(int elevatorMasterId, boolean elevatorMasterInverted, int elevatorFollowId, boolean elevatorFollowInverted, BooleanSupplier bottomReset, BooleanSupplier topReset, double topLimit, double[] elevatorKPID, double[] elevatorKFeedForward, double gearReduction, double maxManualInput) { + return new ElevatorConfig(elevatorMasterId, MotorConfig.NEO, elevatorMasterInverted, elevatorFollowId, MotorConfig.NEO, elevatorFollowInverted, bottomReset, topReset, 0, topLimit, elevatorKPID, elevatorKFeedForward, gearReduction, 0.02, 14, maxManualInput); + } + + public static ElevatorConfig VortexElevatorConfig(int elevatorMasterId, boolean elevatorMasterInverted, int elevatorFollowId, boolean elevatorFollowInverted, BooleanSupplier bottomReset, BooleanSupplier topReset, double topLimit, double[] elevatorKPID, double[] elevatorKFeedForward, double gearReduction, double maxManualInput) { + return new ElevatorConfig(elevatorMasterId, MotorConfig.NEO_VORTEX, elevatorMasterInverted, elevatorFollowId, MotorConfig.NEO_VORTEX, elevatorFollowInverted, bottomReset, topReset, 0, topLimit, elevatorKPID, elevatorKFeedForward, gearReduction, 0.02, 14, maxManualInput); + } + + /** + * Checks the elevatorConfig to not have any issues, and creates certain extra values automatically like {@link #elevatorPID} and {@link #elevatorFeedforward} + */ private void checkRequirements() { checkMotorConfigs(); //Prevent future issues with new motors checkPID(); @@ -86,10 +118,15 @@ private void checkMotorConfigs() { } } - + /** + * Checks PID to have 3 values, updates {@link #elevatorPIDExists} and makes sure the values are legitimate + */ private void checkPID() { if (elevatorKPID != null) { if (elevatorKPID.length == 3) { + if (elevatorKPID[0] < 0 || elevatorKPID[1] < 0 || elevatorKPID[2] < 0) { + throw new IllegalArgumentException("PID values have to be non negative"); + } elevatorKP = elevatorKPID[0]; elevatorKI = elevatorKPID[1]; elevatorKD = elevatorKPID[2]; @@ -107,9 +144,15 @@ private void checkPID() { DriverStation.reportWarning("Elevator PID is off", true); } + /** + * Checks FeedForward to have 4 values, updates {@link #elevatorFeedForwardExists} and makes sure the values are legitimate + */ private void checkFeedForward() { if (elevatorKFeedForward != null) { if (elevatorKFeedForward.length == 4) { + if (elevatorKFeedForward[0] < 0|| elevatorKFeedForward[2] < 0 || elevatorKFeedForward[3] < 0) { + throw new IllegalArgumentException("FeedForward Values of kS, kV, and kA need to have non negative values"); + } elevatorKS = elevatorKFeedForward[0]; elevatorKG = elevatorKFeedForward[1]; elevatorKV = elevatorKFeedForward[2]; @@ -128,12 +171,18 @@ private void checkFeedForward() { DriverStation.reportWarning("ElevatorFeedForward is off", true); } + /** + * Makes sure gear reduction is not 0 + */ private void checkGearReduction() { if (gearReduction == 0) { throw new IllegalArgumentException("Gear reduction cannot be 0"); } } + /** + * Makes sure that the user has not mixed up the top and bottom limits. + */ private void checkLimits() { if (bottomLimit >= topLimit) { throw new IllegalArgumentException("Top limit must be much greater than bottom limit. (Cannot be equal)"); diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java index a54964d..c268fd7 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleElevator.java @@ -7,11 +7,7 @@ import org.carlmontrobotics.lib199.MotorConfig; import org.carlmontrobotics.lib199.MotorControllerFactory; -import org.carlmontrobotics.lib199.SimpleMechs.Arm.SimpleArm.ArmControlState; - -import java.nio.channels.Pipe; - -import javax.management.relation.Relation; +import org.carlmontrobotics.lib199.SimpleMechs.Elastic; import com.revrobotics.RelativeEncoder; import com.revrobotics.spark.SparkBase; @@ -21,12 +17,25 @@ import com.revrobotics.spark.config.SparkMaxConfig; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import edu.wpi.first.math.MathUtil; +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.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.Meters; +import static edu.wpi.first.units.Units.MetersPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; public class SimpleElevator extends SubsystemBase { /** Creates a new Elevator. */ - private ElevatorConfig elevatorConfig; + private final ElevatorConfig elevatorConfig; private SparkBase masterMotor; private SparkBase followMotor; private RelativeEncoder feedbackEncoder; @@ -39,9 +48,37 @@ public class SimpleElevator extends SubsystemBase { private IdleMode elevatorIdleMode = IdleMode.kBrake; private double currentDif = 0; - private double pastDif = 0; - private final double difDanger = 1; + private final double difDanger = 0.03; + + private double currentStamp; + private double pastStamp; + private double currentPeriod = 0.02; + + private boolean encoderFaultReported = false; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutDistance m_distance = Meters.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutLinearVelocity m_velocity = MetersPerSecond.mutable(0); + + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(5)//maximum sysID test time + ); + + + private final SysIdRoutine sysIdRoutine; + /** + * Different control states for the elevator + * {@link #AUTO} runs the elevator autonomously to go to a certain height + * {@link #MANUAL} allows the elevator to be manually run by the user with soft locks, and speed limits + * {@link #BRAKE} idles the elevator in brake, which in cases will cause the elevator to slowly drift downwards due to gravity + * {@link #COAST} idles the elevator in coast allowing for easy movement of the elevator during disable mode, will likely cause elevator to drop very fast though + */ public enum ElevatorControlState { AUTO, MANUAL, @@ -49,29 +86,133 @@ public enum ElevatorControlState { COAST } + /** + * Creates an object of a simple elevator, this class can be extended for more complicated use. + * @param elevatorConfig {@link ElevatorConfig} for all needs of the elevator + */ public SimpleElevator(ElevatorConfig elevatorConfig) { this.elevatorConfig = elevatorConfig; configureMotors(); resetFeedbackEncoder(); + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + voltage -> { + manualElevator(voltage); + }, + log -> { + log.motor("Elevator-Master") + .voltage( + m_appliedVoltage + .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts)) + .linearPosition(m_distance.mut_replace(feedbackEncoder.getPosition(), Meters)) + .linearVelocity(m_velocity.mut_replace(feedbackEncoder.getVelocity(), MetersPerSecond)); + }, + this) + ); + + if (isSysIdTesting()) { + sysIdSetup(); + } + SmartDashboard.putData(this); } + /** + * Sets up commands in SmartDashboard/Elastic to be ran to record data for mechanism modeling + */ + private void sysIdSetup() { + Elastic.Notification notification = new Elastic.Notification(Elastic.NotificationLevel.INFO, "SysId Testing On", "Please add Elevator Quasistatic and Dynamics and begin testing.").withDisplaySeconds(5); + Elastic.sendNotification(notification); + Elastic.selectTab("SysId"); + SmartDashboard.putData("Elevator Quasistatic Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + SmartDashboard.putData("Elevator Quasistatic Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Elevator Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Elevator Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + + /** Override this method and return true if you want to do sysid testing, return false if you don't + * @return false to do sysid testing + */ + protected boolean isSysIdTesting() { + return false; + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + /** + * Sets {@link #elevatorGoal} + * @param goal in meters + */ public void setGoal(double goal){ if (goal <= elevatorConfig.topLimit && goal >= elevatorConfig.bottomLimit) {elevatorGoal = goal;} } + /** + * @return current {@link #elevatorGoal} in meters + */ public double getGoal() { return elevatorGoal; } + /** + * Checks if at {@link #elevatorGoal} + * @return true if at {@link #elevatorGoal} + */ + public boolean atGoal() { + return Math.abs(elevatorGoal - feedbackEncoder.getPosition()) <= elevatorConfig.elevatorPIDTolerance; + } + + /** + * Updates {@link #latestManualInput} to manually control elevator + * @param input voltage percentage from -1.0 - 1.0 + */ public void setManualInput(double input) { latestManualInput = input; } + /** + * @return {@link #latestManualInput} a value between -1.0 - 1.0, inclusive + */ public double getLatestManualInput() { return latestManualInput; } - public void setElevatorControlState(ArmControlState controlState) { + /** + * @return {@link ElevatorConfig} of the object + */ + public ElevatorConfig getConfig() { + return elevatorConfig; + } + + /** + * @return the current {@link ElevatorControlState} + */ + public ElevatorControlState getCurrentControlState() { + return currentState; + } + + /** + * Sets {@link #currentState} to the inputed state + * @param controlState An {@link ElevatorControlState} + */ + public void setElevatorControlState(ElevatorControlState controlState) { switch (controlState) { case AUTO -> setAutoOn(); case MANUAL -> setManualOn(); @@ -81,6 +222,9 @@ public void setElevatorControlState(ArmControlState controlState) { } } + /** + * Sets {@link #currentState} to be in AUTO mode + */ public void setAutoOn() { if (elevatorConfig.elevatorPIDExists || elevatorConfig.elevatorFeedForwardExists) { currentState = ElevatorControlState.AUTO; @@ -90,10 +234,16 @@ public void setAutoOn() { } } + /** + * Sets {@link #currentState} to be in MANUAL mode + */ public void setManualOn() { currentState = ElevatorControlState.MANUAL; } + /** + * Sets {@link #currentState} to be in BRAKE mode, also changes the idle mode of the motors to brake. + */ public void setBrakeOn() { currentState = ElevatorControlState.BRAKE; if (elevatorIdleMode == IdleMode.kCoast) { @@ -103,6 +253,9 @@ public void setBrakeOn() { } } + /** + * Sets {@link #currentState} to be in COAST mode, also changes the idle mode of the motors to coast. + */ public void setCoastOn() { currentState = ElevatorControlState.COAST; if (elevatorIdleMode == IdleMode.kBrake) { @@ -157,27 +310,38 @@ private void coastMotors() { } } - private void checkFeedBackEncoder() { - pastDif = currentDif; + /** + * Called periodically, checks for any issues with the main encoder by comparing it with the backup encoder + */ + protected void checkFeedBackEncoder() { currentDif = feedbackEncoder.getPosition() - backupEncoder.getPosition(); - if (currentDif - pastDif > difDanger) { + if (Math.abs(currentDif) > difDanger && !encoderFaultReported) { DriverStation.reportWarning("Elevator encoder seems to be off!", true); + encoderFaultReported = true; } } - - + /** + * @return current elevator height in meters + */ public double getElevatorHeight() { return feedbackEncoder.getPosition(); } + /** + * @return current elevator velocity in meters per second + */ public double getElevatorVelocity() { return feedbackEncoder.getVelocity(); } - - private void configureMotors() { + /** + * Called in constructor. + * Configures the master and follow motors in brake mode and makes follow motor to follow the master. + * Also configures encoders to read in meters, and meters per second for position and velocity respectively + */ + protected void configureMotors() { if (elevatorConfig.elevatorMasterMotorType == MotorConfig.NEO_VORTEX) { SparkFlexConfig masterFlexConfig = new SparkFlexConfig(); masterFlexConfig.idleMode(IdleMode.kBrake) @@ -221,32 +385,110 @@ private void configureMotors() { backupEncoder = followMotor.getEncoder(); } - private void autoElevator() { - //TODO + /** + * Called periodically if in AUTO mode, will approach the current active {@link #elevatorGoal} using PID and FeedForward if such are provided + */ + protected void autoElevator() { + double pidOutput; + double feedforwardOutput; + double curPosition = feedbackEncoder.getPosition(); + if (elevatorConfig.elevatorPIDExists) { + pidOutput = elevatorConfig.elevatorPID.calculate(curPosition, elevatorGoal); + } + else { + pidOutput = 0; + } + + if (elevatorConfig.elevatorFeedForwardExists) { + feedforwardOutput = elevatorConfig.elevatorFeedforward.calculate(0); //Voltage to hold at desired spot + } + else { + feedforwardOutput = 0; + } + if (curPosition < elevatorConfig.topLimit) { + if (curPosition > elevatorConfig.bottomLimit) { + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -elevatorConfig.maxVolts, elevatorConfig.maxVolts)); + } + else { + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, 0, elevatorConfig.maxVolts)); + } + } + else { + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -elevatorConfig.maxVolts, 0)); + } } - private void manualElevator() { - //TODO + /** + * Called periodically if in MANUAL mode, will use {@link #latestManualInput} to move the elevator using voltage percentage with soft locking to avoid elevator damage + */ + protected void manualElevator() { + double curPosition = feedbackEncoder.getPosition(); + if (curPosition < elevatorConfig.topLimit) { + if (curPosition > elevatorConfig.bottomLimit) { + masterMotor.set(MathUtil.clamp(latestManualInput, -elevatorConfig.maxManualInput, elevatorConfig.maxManualInput)); + } + else { + masterMotor.set(MathUtil.clamp(latestManualInput, 0, elevatorConfig.maxManualInput)); + } + } + else { + masterMotor.set(MathUtil.clamp(latestManualInput, -elevatorConfig.maxManualInput, 0)); + } } + /** + * Mainly used by SysId to characterize an elevator model + * @param volts Voltage unit input + */ + protected void manualElevator(Voltage volts) { + double curPosition = feedbackEncoder.getPosition(); + double voltage = volts.in(Volts); + if (curPosition < elevatorConfig.topLimit) { + if (curPosition > elevatorConfig.bottomLimit) { + masterMotor.setVoltage(MathUtil.clamp(voltage, -12, 12)); + } + else { + masterMotor.setVoltage(MathUtil.clamp(voltage, 0, 12)); + } + } + else { + masterMotor.set(MathUtil.clamp(voltage,-12, 0)); + } + } + + /** + * Sets voltage to 0 + */ public void stopElevator() { masterMotor.set(0); } + /** + * Called in constructor + * Resets the feedback encoder to the lowest point the elevator can achieve + */ public void resetFeedbackEncoder() { resetFeedbackEncoder(elevatorConfig.bottomLimit); } + /** + * Allow for manual reseting + * @param position resets to this position in meters + */ public void resetFeedbackEncoder(double position) { if (position >= elevatorConfig.bottomLimit && position <= elevatorConfig.topLimit) { feedbackEncoder.setPosition(position); + backupEncoder.setPosition(position); } else { DriverStation.reportWarning("Cannot reset position to be outside the limits", true); } } - private void updateFeedbackEncoder() { + /** + * Checks boolean suppliers for any bottom or top limit switches to update the feedbackEncoder in case it drifted a bit + */ + protected void updateFeedbackEncoder() { if (elevatorConfig.bottomReset != null) { if (elevatorConfig.bottomReset.getAsBoolean()) { feedbackEncoder.setPosition(elevatorConfig.bottomLimit); @@ -258,17 +500,61 @@ private void updateFeedbackEncoder() { } } } + /** * Use this to do any fun stuff you want to do, for elevatorSubsystemPeriodic */ - public void userPeriodic() { + protected void userPeriodic() { } - private void postSmartDashboardValues() { + /** + * @return current calculated period in seconds + */ + protected double getCurrentPeriod() { + return currentPeriod; + } + /** + * Posts important information about elevator into smartdashboard + */ + protected void postSmartDashboardValues() { + SmartDashboard.putBoolean("At Goal", atGoal()); + SmartDashboard.putNumber("Goal(m)", elevatorGoal); + SmartDashboard.putNumber("Elevator Height(m)", getElevatorHeight()); + SmartDashboard.putNumber("Elevator Velocity (m/s)", getElevatorVelocity()); + SmartDashboard.putNumber("Latest manual input", latestManualInput); + SmartDashboard.putNumber("Current period of Elevator (ms)", currentPeriod*1000); + switch (currentState) { + case AUTO -> SmartDashboard.putString("elevatorControlState", "Autonomous"); + case MANUAL -> SmartDashboard.putString("elevatorControlState", "Manual"); + case BRAKE -> SmartDashboard.putString("elevatorControlState", "Idling: Brake"); + case COAST -> SmartDashboard.putString("elevatorControlState", "Idling: Coast"); + } } + /** + * Checks Subsystem periodic for running under expected time limits, reports inconsitencies to user, also is usable by feedforward + */ + private void checkPeriodic() { + if (pastStamp == 0) { + pastStamp = Timer.getFPGATimestamp(); + return; + } + currentStamp = Timer.getFPGATimestamp(); + currentPeriod = currentStamp - pastStamp; + pastStamp = currentStamp; + if (currentPeriod > 0.02) { + DriverStation.reportWarning("Elevator Periodic is slow: " + currentPeriod *1000 + " ms. Check for any lag reasons", true); + } + } + + /*Overide these methods to customize what the mechanism does in different states */ + protected void handleAuto() { autoElevator();} + protected void handleManual() { manualElevator();} + protected void handleBrake() { stopElevator();} + protected void handleCoast() { stopElevator();} + /** * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! * Use {@link #userPeriodic()} instead. @@ -276,14 +562,15 @@ private void postSmartDashboardValues() { @Override public void periodic() { switch (currentState) { - case AUTO -> autoElevator(); - case MANUAL -> manualElevator(); - case BRAKE -> stopElevator(); - case COAST -> stopElevator(); + case AUTO -> handleAuto(); + case MANUAL -> handleManual(); + case BRAKE -> handleBrake(); + case COAST -> handleCoast(); } checkFeedBackEncoder(); updateFeedbackEncoder(); userPeriodic(); postSmartDashboardValues(); + checkPeriodic(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java new file mode 100644 index 0000000..29e9f61 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleManualElevator.java @@ -0,0 +1,52 @@ +// 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 org.carlmontrobotics.lib199.SimpleMechs.Elevator; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleManualElevator extends Command { + private SimpleElevator elevator; + private DoubleSupplier manualInput; + private boolean remainAtGoal; + /** Creates a new SimpleManualElevator. */ + public SimpleManualElevator(SimpleElevator elevator, DoubleSupplier manualInput, boolean remainAtGoal) { + addRequirements(this.elevator = elevator); + this.manualInput = manualInput; + this.remainAtGoal = remainAtGoal; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.setManualOn(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + elevator.setManualInput(manualInput.getAsDouble()); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if (remainAtGoal) { + elevator.setGoal(elevator.getElevatorHeight()); + elevator.setAutoOn(); + } + else { + elevator.setBrakeOn(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java new file mode 100644 index 0000000..d6ad0ee --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Elevator/SimpleRaiseElevator.java @@ -0,0 +1,73 @@ +// 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 org.carlmontrobotics.lib199.SimpleMechs.Elevator; + +import org.carlmontrobotics.lib199.SimpleMechs.Elevator.SimpleElevator.ElevatorControlState; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleRaiseElevator extends Command { + private SimpleElevator elevator; + private double elevatorGoal; + private boolean useMechanicalLimit; + private boolean commandComplete = false; + private double safeMechanicalUsageSpeed = 0.1; + + /** Creates a new SimpleRaiseElevator. */ + public SimpleRaiseElevator(SimpleElevator elevator, double height, boolean useMechanicalLimit) { + addRequirements(this.elevator = elevator); + this.elevatorGoal = height; + this.useMechanicalLimit = useMechanicalLimit; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + elevator.setGoal(elevatorGoal); + elevator.setAutoOn(); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (elevator.atGoal()) { + if (elevator.getConfig().bottomLimit == elevatorGoal && useMechanicalLimit) { + if (elevator.getElevatorHeight() <= elevatorGoal) {commandComplete = true;} + if (elevator.getCurrentControlState() != ElevatorControlState.MANUAL) { + elevator.setManualOn(); + } + elevator.setManualInput(-safeMechanicalUsageSpeed); + } + else if (elevator.getConfig().topLimit == elevatorGoal && useMechanicalLimit) { + if (elevator.getElevatorHeight() >= elevatorGoal) {commandComplete = true;} + if (elevator.getCurrentControlState() != ElevatorControlState.MANUAL) { + elevator.setManualOn(); + } + elevator.setManualInput(safeMechanicalUsageSpeed); + } + else { + commandComplete = true; + } + } + else { + if (elevator.getCurrentControlState() != ElevatorControlState.AUTO) { + elevator.setAutoOn(); + } + } + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + elevator.setAutoOn(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return commandComplete; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java new file mode 100644 index 0000000..e16d810 --- /dev/null +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinSpinner.java @@ -0,0 +1,40 @@ +// 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 org.carlmontrobotics.lib199.SimpleMechs.Spinner; + +import edu.wpi.first.wpilibj2.command.Command; + +/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */ +public class SimpleSpinSpinner extends Command { + /** Creates a new SimpleSpinSpinner. */ + SimpleSpinner spinner; + double velocity; + public SimpleSpinSpinner(SimpleSpinner spinner, double velocity) { + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(this.spinner = spinner); + this.velocity = velocity; + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + spinner.setAutoOn(); + spinner.setGoal(velocity); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() {} + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return true; + } +} diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java index 5d19480..f851faa 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SimpleSpinner.java @@ -4,16 +4,523 @@ package org.carlmontrobotics.lib199.SimpleMechs.Spinner; + +import org.carlmontrobotics.lib199.MotorConfig; +import org.carlmontrobotics.lib199.MotorControllerFactory; +import org.carlmontrobotics.lib199.SimpleMechs.Elastic; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkFlexConfig; +import com.revrobotics.spark.config.SparkMaxConfig; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.units.measure.MutAngle; +import edu.wpi.first.units.measure.MutAngularAcceleration; +import edu.wpi.first.units.measure.MutAngularVelocity; +import edu.wpi.first.units.measure.MutVoltage; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import static edu.wpi.first.units.Units.Radians; +import static edu.wpi.first.units.Units.RadiansPerSecond; +import static edu.wpi.first.units.Units.RadiansPerSecondPerSecond; +import static edu.wpi.first.units.Units.Volts; +import static edu.wpi.first.units.Units.Seconds; public class SimpleSpinner extends SubsystemBase { - /** Creates a new SimpleSpinner. */ + private final SpinnerConfig spinnerConfig; + private SparkBase masterMotor; + private SparkBase followMotor; + private RelativeEncoder feedbackEncoder; + private RelativeEncoder backupEncoder; + private double spinnerGoal; + private double latestManualInput; // in percentage of power (-1, 1) + + private SpinnerControlState currentState = SpinnerControlState.AUTO; + private IdleMode spinnerIdleMode = IdleMode.kBrake; + + private double currentDif = 0; + private final double difDanger = 0.2; + + private double currentStamp = 0; + private double pastStamp = 0; + private double currentPeriod = 0.02; + + private double currentAcceleration = 0.0; + private double previousVelocity = 0; + + private boolean encoderFaultReported = false; + + // Mutable holder for unit-safe voltage values, persisted to avoid reallocation. + private final MutVoltage m_appliedVoltage = Volts.mutable(0); + // Mutable holder for unit-safe linear distance values, persisted to avoid reallocation. + private final MutAngle m_angle = Radians.mutable(0); + // Mutable holder for unit-safe linear velocity values, persisted to avoid reallocation. + private final MutAngularVelocity m_velocity = RadiansPerSecond.mutable(0); + // Mutable holder for unit-safe linear accleration values, persisted to avoid reallocation + private final MutAngularAcceleration m_accleration = RadiansPerSecondPerSecond.mutable(0); + + private SysIdRoutine.Config defaultSysIdConfig = new SysIdRoutine.Config( + Volts.of(1).per(Seconds),//ramp rate, volts/sec + Volts.of(1), //starting voltage, volts + Seconds.of(12)//maximum sysID test time + ); + + + private final SysIdRoutine sysIdRoutine; + + + /** + * Different control states for the spinner + * {@link #AUTO} runs the spinner autonomously to go to a certain speed + * {@link #MANUAL} allows the spinner to be manually run by the user with speed limits + * {@link #BRAKE} idles the spinner in brake which hold the spinner a certain position + * {@link #COAST} idles the spinner in coast allowing for easy movement + */ + public enum SpinnerControlState { + AUTO, + MANUAL, + BRAKE, + COAST + } + public SimpleSpinner(SpinnerConfig spinnerConfig) { + this.spinnerConfig = spinnerConfig; + configureMotors(); + resetFeedbackEncoder(); + sysIdRoutine = new SysIdRoutine( + defaultSysIdConfig, + new SysIdRoutine.Mechanism( + voltage -> { + manualSpinner(voltage); + }, + log -> { + log.motor("Spinner-Master") + .voltage( + m_appliedVoltage + .mut_replace(masterMotor.getBusVoltage() * masterMotor.getAppliedOutput(), Volts)) + .angularPosition(m_angle.mut_replace(feedbackEncoder.getPosition()*Math.PI*2, Radians)) + .angularVelocity(m_velocity.mut_replace(getSpinnerVelocity()*Math.PI*2, RadiansPerSecond)) + .angularAcceleration(m_accleration.mut_replace(getSpinnerAccleration()*Math.PI*2, RadiansPerSecondPerSecond)); + }, + this) + ); + + if (isSysIdTesting()) { + sysIdSetup(); + } + SmartDashboard.putData(this); + } + + /** + * Sets up commands in SmartDashboard/Elastic to be ran to record data for mechanism modeling + */ + private void sysIdSetup() { + Elastic.Notification notification = new Elastic.Notification(Elastic.NotificationLevel.INFO, "SysId Testing On", "Please add Spinner Quasistatic and Dynamics and begin testing.").withDisplaySeconds(5); + Elastic.sendNotification(notification); + Elastic.selectTab("SysId"); + SmartDashboard.putData("Spinner Quasistatic Backward", sysIdQuasistatic(SysIdRoutine.Direction.kReverse)); + SmartDashboard.putData("Spinner Quasistatic Forward", sysIdQuasistatic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Spinner Dynamic Forward", sysIdDynamic(SysIdRoutine.Direction.kForward)); + SmartDashboard.putData("Spinner Dynamic Backward", sysIdDynamic(SysIdRoutine.Direction.kReverse)); + } + + + /** Override this method and return true if you want to do sysid testing, return false if you don't + * @return false to do sysid testing + */ + protected boolean isSysIdTesting() { + return false; + } + + /** + * Returns a command that will execute a quasistatic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return sysIdRoutine.quasistatic(direction); + } + + /** + * Returns a command that will execute a dynamic test in the given direction. + * + * @param direction The direction (forward or reverse) to run the test in + */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return sysIdRoutine.dynamic(direction); + } + + /** + * Sets {@link #spinnerGoal} + * @param goal in rotations per second + */ + public void setGoal(double goal){ + spinnerGoal = goal; + } + + /** + * @return current {@link #spinnerGoal} in rotations per second + */ + public double getGoal() { + return spinnerGoal; + } + + /** + * Checks if at {@link #spinnerGoal} + * @return true if at {@link #spinnerGoal} + */ + public boolean atGoal() { + return Math.abs(spinnerGoal - feedbackEncoder.getVelocity()) <= spinnerConfig.spinnerPIDTolerance; + } + + /** + * Updates {@link #latestManualInput} to manually control spinner + * @param input voltage percentage from -1.0 - 1.0 + */ + public void setManualInput(double input) { + latestManualInput = input; + } + + /** + * @return {@link #latestManualInput} a value from -1.0 - 1.0 + */ + public double getLatestManualInput() { + return latestManualInput; + } + + /** + * @return {@link SpinnerConfig} of the object + */ + public SpinnerConfig getConfig() { + return spinnerConfig; + } + + /** + * Sets {@link #currentState} to the inputed state + * @param controlState An {@link SpinnerControlState} + */ + public void setSpinnerControlState(SpinnerControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } + } + + + /** + * @return current spinner velocity in rotations per second + */ + public double getSpinnerVelocity() { + return feedbackEncoder.getVelocity(); + } + + /** + * @return current spinner acceleration in rotations per second per second + */ + public double getSpinnerAccleration() { + return currentAcceleration; + } + /** + * Sets {@link #currentState} to the inputed state + * @param controlState An {@link SpinnerControlState} + */ + public void setElevatorControlState(SpinnerControlState controlState) { + switch (controlState) { + case AUTO -> setAutoOn(); + case MANUAL -> setManualOn(); + case BRAKE -> setBrakeOn(); + case COAST -> setCoastOn(); + default -> DriverStation.reportWarning("Such control mode has not been implemented yet", true); + } } + /** + * Sets {@link #currentState} to be in AUTO mode + */ + public void setAutoOn() { + if (spinnerConfig.spinnerPIDExists || spinnerConfig.spinnerFeedForwardExists) { + currentState = SpinnerControlState.AUTO; + } + else { + DriverStation.reportWarning("Any sort of autonomous control mode is disabled due to no PID or FeedForward", true); + } + } + + /** + * Sets {@link #currentState} to be in MANUAL mode + */ + public void setManualOn() { + currentState = SpinnerControlState.MANUAL; + } + + /** + * Sets {@link #currentState} to be in BRAKE mode, also changes the idle mode of the motors to brake. + */ + public void setBrakeOn() { + currentState = SpinnerControlState.BRAKE; + if (spinnerIdleMode == IdleMode.kCoast) { + spinnerIdleMode = IdleMode.kBrake; + brakeMotors(); + + } + } + + /** + * Sets {@link #currentState} to be in COAST mode, also changes the idle mode of the motors to coast. + */ + public void setCoastOn() { + currentState = SpinnerControlState.COAST; + if (spinnerIdleMode == IdleMode.kBrake) { + spinnerIdleMode = IdleMode.kCoast; + coastMotors(); + } + } + + /** + * Set all motors idleModes to brake + */ + private void brakeMotors() { + SparkFlexConfig flexBrakeConfig = new SparkFlexConfig(); + SparkMaxConfig maxBrakeConfig = new SparkMaxConfig(); + + flexBrakeConfig.idleMode(IdleMode.kBrake); + maxBrakeConfig.idleMode(IdleMode.kBrake); + if (spinnerConfig.spinnerFollowExists) { + if (spinnerConfig.spinnerFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + if (spinnerConfig.spinnerMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxBrakeConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + /** + * Set all motors idleModes to coast + */ + private void coastMotors() { + SparkFlexConfig flexCoastConfig = new SparkFlexConfig(); + SparkMaxConfig maxCoastConfig = new SparkMaxConfig(); + + flexCoastConfig.idleMode(IdleMode.kCoast); + maxCoastConfig.idleMode(IdleMode.kCoast); + if (spinnerConfig.spinnerFollowExists) { + if (spinnerConfig.spinnerFollowMotorType == MotorConfig.NEO_VORTEX) { + followMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + followMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + if (spinnerConfig.spinnerMasterMotorType == MotorConfig.NEO_VORTEX) { + masterMotor.configure(flexCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + masterMotor.configure(maxCoastConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + } + + + protected void configureMotors() { + if (spinnerConfig.spinnerMasterMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig masterFlexConfig = new SparkFlexConfig(); + masterFlexConfig.idleMode(IdleMode.kBrake) + .inverted(spinnerConfig.spinnerMasterInverted) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkFlex(spinnerConfig.spinnerMasterId); + masterMotor.configure(masterFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig masterMaxConfig = new SparkMaxConfig(); + masterMaxConfig.idleMode(IdleMode.kBrake) + .inverted(spinnerConfig.spinnerMasterInverted) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + masterMotor = MotorControllerFactory.createSparkMax(spinnerConfig.spinnerMasterId, spinnerConfig.spinnerMasterMotorType); + masterMotor.configure(masterMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + feedbackEncoder = masterMotor.getEncoder(); + if (spinnerConfig.spinnerFollowExists) { + if (spinnerConfig.spinnerFollowMotorType == MotorConfig.NEO_VORTEX) { + SparkFlexConfig followFlexConfig = new SparkFlexConfig(); + followFlexConfig.idleMode(IdleMode.kBrake) + .inverted(spinnerConfig.spinnerFollowInverted) + .follow(spinnerConfig.spinnerMasterId, (spinnerConfig.spinnerMasterInverted != spinnerConfig.spinnerFollowInverted)) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkFlex(spinnerConfig.spinnerFollowId); + followMotor.configure(followFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + else { + SparkMaxConfig followMaxConfig = new SparkMaxConfig(); + followMaxConfig.idleMode(IdleMode.kBrake) + .follow(spinnerConfig.spinnerMasterId, (spinnerConfig.spinnerMasterInverted != spinnerConfig.spinnerFollowInverted)) + .inverted(spinnerConfig.spinnerFollowInverted) + .encoder.positionConversionFactor(spinnerConfig.gearReduction) + .velocityConversionFactor(spinnerConfig.gearReduction/60); + followMotor = MotorControllerFactory.createSparkMax(spinnerConfig.spinnerFollowId, spinnerConfig.spinnerFollowMotorType); + followMotor.configure(followMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + backupEncoder = followMotor.getEncoder(); + } + } + + private void resetFeedbackEncoder() { + feedbackEncoder.setPosition(0); + if (spinnerConfig.spinnerFollowExists) { + backupEncoder.setPosition(0); + } + } + + /** + * Called periodically if in AUTO mode, will approach the current active {@link #spinnerGoal} using PID and FeedForward if such are provided + */ + protected void autoSpinner() { + double pidOutput; + double feedforwardOutput; + double curVelocity = feedbackEncoder.getVelocity(); + if (spinnerConfig.spinnerPIDExists) { + pidOutput = spinnerConfig.spinnerPID.calculate(curVelocity, spinnerGoal); + } + else { + pidOutput = 0; + } + + if (spinnerConfig.spinnerFeedForwardExists) { + feedforwardOutput = spinnerConfig.spinnerFeedforward.calculateWithVelocities(curVelocity, spinnerGoal); + } + else { + feedforwardOutput = 0; + } + masterMotor.setVoltage(MathUtil.clamp(pidOutput + feedforwardOutput, -spinnerConfig.maxVolts, 0)); + } + + /** + * Called periodically if in MANUAL mode, will use {@link #latestManualInput} to spin the spinner using voltage percentage + */ + protected void manualSpinner() { + masterMotor.set(MathUtil.clamp(latestManualInput, -spinnerConfig.maxManualInput, spinnerConfig.maxManualInput)); + } + + /** + * Mainly used by SysId to characterize the spinner model + * @param volts Voltage unit input + */ + protected void manualSpinner(Voltage volts) { + double voltage = volts.in(Volts); + masterMotor.setVoltage(MathUtil.clamp(voltage, -spinnerConfig.maxManualInput, spinnerConfig.maxManualInput)); + + } + + /** + * Called periodically, checks for any issues with the main encoder by comparing it with the backup encoder if such exists + */ + protected void checkFeedBackEncoder() { + if (spinnerConfig.spinnerFollowExists) { + currentDif = feedbackEncoder.getPosition() - backupEncoder.getPosition(); + if (Math.abs(currentDif) > difDanger && !encoderFaultReported) { + DriverStation.reportWarning("Spinner encoder seems to be off!", true); + encoderFaultReported = true; + } + } + } + + + /** + * Sets voltage to 0 + */ + public void stopSpinner() { + masterMotor.set(0); + } + + /** + * Use this to do any fun stuff you want to do, for spinnerSubsystemPeriodic + */ + protected void userPeriodic() { + + } + + /** + * Posts important information about Spinner Subsystem into SmartDashboard + */ + public void postSmartDashboardValues() { + SmartDashboard.putNumber("Spinner Velocity (rotations/s)", feedbackEncoder.getVelocity()); + SmartDashboard.putNumber("Spinner Goal Velocity (rotations/s)", spinnerGoal); + SmartDashboard.putBoolean("Spinner at Goal Speed", atGoal()); + SmartDashboard.putNumber("Spinner Latest Manual Input", latestManualInput); + SmartDashboard.putNumber("Current period of Spinner (ms)", currentPeriod*1000); + switch (currentState) { + case AUTO -> SmartDashboard.putString("spinnerControlState", "Autonomous"); + case MANUAL -> SmartDashboard.putString("spinnerControlState", "Manual"); + case BRAKE -> SmartDashboard.putString("spinnerControlState", "Idling: Brake"); + case COAST -> SmartDashboard.putString("spinnerControlState", "Idling: Coast"); + } + } + + /** + * Checks Subsystem periodic for running under expected time limits, reports inconsitencies to user, also is usable by feedforward + */ + private void checkPeriodic() { + if (pastStamp == 0) { + pastStamp = Timer.getFPGATimestamp(); + return; + } + currentStamp = Timer.getFPGATimestamp(); + currentPeriod = currentStamp - pastStamp; + pastStamp = currentStamp; + if (currentPeriod > 0.02) { + DriverStation.reportWarning("Spinner Periodic is slow: " + currentPeriod *1000 + " ms. Check for any lag reasons", true); + } + } + + private void calculateAccleration() { + double currentVelocity = feedbackEncoder.getVelocity(); + currentAcceleration = (currentVelocity - previousVelocity) / currentPeriod; + previousVelocity = currentVelocity; + } + + /*Overide these methods to customize what the mechanism does in different states */ + protected void handleAuto() { autoSpinner();} + protected void handleManual() { manualSpinner();} + protected void handleBrake() { stopSpinner();} + protected void handleCoast() { stopSpinner();} + + /** + * DO NOT OVERRIDE THIS PERIODIC UNLESS YOU KNOW WHAT YOU ARE DOING! + * Use {@link #userPeriodic()} instead. + */ @Override public void periodic() { - // This method will be called once per scheduler run + switch (currentState) { + case AUTO -> handleAuto(); + case MANUAL -> handleManual(); + case BRAKE -> handleBrake(); + case COAST -> handleCoast(); + } + checkFeedBackEncoder(); + userPeriodic(); + postSmartDashboardValues(); + checkPeriodic(); + calculateAccleration(); } } diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java index 22ec30d..88cf0bc 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java @@ -1,8 +1,224 @@ package org.carlmontrobotics.lib199.SimpleMechs.Spinner; +import org.carlmontrobotics.lib199.MotorConfig; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.DriverStation; + public final class SpinnerConfig { - - public SpinnerConfig() { - + public final int spinnerMasterId; + public final int spinnerFollowId; + public final MotorConfig spinnerMasterMotorType; + public final MotorConfig spinnerFollowMotorType; + public final boolean spinnerMasterInverted; + public final boolean spinnerFollowInverted; + public final double gearReduction; + public final double maxVolts; + public final double maxManualInput; + public final double[] spinnerKPID; + public final double[] spinnerKFeedForward; + public final double spinnerPIDTolerance; + public double spinnerKP; + public double spinnerKI; + public double spinnerKD; + public double spinnerKS; + public double spinnerKV; + public double spinnerKA; + public PIDController spinnerPID; + public SimpleMotorFeedforward spinnerFeedforward; + public boolean spinnerFollowExists; + public boolean spinnerPIDExists; + public boolean spinnerFeedForwardExists; + + /** + * + * @param spinnerMasterId master id + * @param spinnerFollowId follow id, -1 if no follow motor + * @param spinnerMasterMotorType master {@link MotorConfig} + * @param spinnerFollowMotorType follow {@link MotorConfig} null if no follow motor + * @param spinnerMasterInverted master inversed such that whichever is the desired movement of the spinner will be positive + * @param spinnerFollowInverted follow inversed such that whichever is the desired movement of the spinner will be positive, false if no motor + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxVolts maximum volts that the spinner can use autonomously + * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled + * @param spinnerKPID 3 values for kP, kI, and kD + * @param spinnerKFeedForward 3 values for kS, kV, and kA + * @param spinnerPIDTolerance in rotations per second how much should pid care for speed + * @throws IllegalArgumentException if SpinnerFollowMotorType is null when there is provided a spinnerFollowId + * @throws IllegalArgumentException if SpinnerFollowMotorType or SpinnerMasterMotorType is not an expected {@link MotorConfig} + * @throws IllegalArgumentException if spinnerKPID is not three values with each being non negative + * @throws IllegalArgumentException if spinnerKFeedForward is not three values with each being non negative + * @throws IllegalArgumentException if gearReduction is 0 + * @throws IllegalArgumentException if maxVolts is not positive + * @throws IllegalArgumentException if maxManualInput is not positive + */ + public SpinnerConfig(int spinnerMasterId, int spinnerFollowId, MotorConfig spinnerMasterMotorType, MotorConfig spinnerFollowMotorType, boolean spinnerMasterInverted, boolean spinnerFollowInverted, + double gearReduction, double maxVolts, double maxManualInput, double[] spinnerKPID, double[] spinnerKFeedForward, double spinnerPIDTolerance) { + this.spinnerMasterId = spinnerMasterId; + this.spinnerMasterMotorType = spinnerMasterMotorType; + this.spinnerMasterInverted = spinnerMasterInverted; + this.spinnerFollowId = spinnerFollowId; + this.spinnerFollowMotorType = spinnerFollowMotorType; + this.spinnerFollowInverted = spinnerFollowInverted; + this.gearReduction = gearReduction; + this.maxVolts = maxVolts; + this.maxManualInput = maxManualInput; + this.spinnerKPID = spinnerKPID; + this.spinnerKFeedForward = spinnerKFeedForward; + this.spinnerPIDTolerance = spinnerPIDTolerance; + checkRequirements(); + } + + /** + * Creates a simple single NEO spinner with PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxVolts maximum volts that the spinner can use autonomously + * @param spinnerKPID 3 values for kP, kI, and kD + * @param spinnerKPIDTolerance in rotations per second how much should pid care for speed + */ + public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxVolts, double[] spinnerKPID, double spinnerPIDTolerance) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO, null, spinnerMasterInverted, false, gearReduction, maxVolts, maxVolts/12.0, spinnerKPID, null, spinnerPIDTolerance); + } + + /** + * Creates a simple single NEO spinner without PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInversed inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled + * @return + */ + public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxManualInput) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO, null, spinnerMasterInverted, false, gearReduction, 14, maxManualInput, null, null, 0); + } + + /** + * Creates a simple single NEO spinner with PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInversed inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxVolts maximum volts that the spinner can use autonomously + * @param spinnerKPID 3 values for kP, kI, and kD + * @param spinnerKPIDTolerance in rotations per second how much should pid care for speed + */ + public static SpinnerConfig VORTEXSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxVolts, double[] spinnerKPID, double spinnerPIDTolerance) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO_VORTEX, null, spinnerMasterInverted, false, gearReduction, maxVolts, maxVolts/12.0, spinnerKPID, null, spinnerPIDTolerance); + } + + /** + * Creates a simple single NEO spinner without PID + * @param spinnerMasterId id of the motor + * @param spinnerMasterInversed inversed setting such that whichever is the desired movement of the spinner will be positive + * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor + * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled + * @return + */ + public static SpinnerConfig VORTEXSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxManualInput) { + return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO_VORTEX, null, spinnerMasterInverted, false, gearReduction, 14, maxManualInput, null, null, 0); + } + + /** + * Checks the spinnerConfig to not have any issues, and creates certain extra values automatically like {@link #spinnerPID} and {@link #spinnerFeedforward} + */ + private void checkRequirements() { + checkMotorConfigs(); //Prevent future issues with new motors + checkPID(); + checkFeedForward(); + checkGearReduction(); + checkMaxVoltsAndInput(); + } + + /** + * Check that the motor Configs are not any new configs that would not work or null. + */ + private void checkMotorConfigs() { + if (spinnerFollowId != -1) { + spinnerFollowExists = true; + if (spinnerFollowMotorType != MotorConfig.NEO && spinnerFollowMotorType != MotorConfig.NEO_VORTEX && spinnerFollowMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + else { + spinnerFollowExists = false; + } + if (spinnerMasterMotorType != MotorConfig.NEO && spinnerMasterMotorType != MotorConfig.NEO_VORTEX && spinnerMasterMotorType != MotorConfig.NEO_550) { + throw new IllegalArgumentException("What is this config??? If null pls change, if not null you're cooked"); + } + } + + /** + * Checks PID to have 3 values, updates {@link #spinnerPIDExists} and makes sure the values are legitimate + */ + private void checkPID() { + if (spinnerKPID != null) { + if (spinnerKPID.length == 3) { + if (spinnerKPID[0] < 0 || spinnerKPID[1] < 0 || spinnerKPID[2] < 0) { + throw new IllegalArgumentException("PID values have to be non negative"); + } + spinnerKP = spinnerKPID[0]; + spinnerKI = spinnerKPID[1]; + spinnerKD = spinnerKPID[2]; + spinnerPID = new PIDController(spinnerKP, spinnerKI, spinnerKD); + spinnerPID.setTolerance(spinnerPIDTolerance); + spinnerPID.enableContinuousInput(0, 1); + spinnerPIDExists = true; + return; + } + throw new IllegalArgumentException("Need to have 3 values for PID"); + } + spinnerKP = 0; + spinnerKI = 0; + spinnerKD = 0; + spinnerPIDExists = false; + DriverStation.reportWarning("Spinner PID is off", true); + } + + /** + * Checks FeedForward to have 3 values, updates {@link #spinnerFeedForwardExists} and makes sure the values are legitimate + */ + private void checkFeedForward() { + if (spinnerKFeedForward != null) { + if (spinnerKFeedForward.length == 3) { + if (spinnerKFeedForward[0] < 0|| spinnerKFeedForward[1] < 0 || spinnerKFeedForward[2] < 0) { + throw new IllegalArgumentException("FeedForward values need to have non negative values"); + } + spinnerKS = spinnerKFeedForward[0]; + spinnerKV = spinnerKFeedForward[1]; + spinnerKA = spinnerKFeedForward[2]; + spinnerFeedforward = new SimpleMotorFeedforward(spinnerKS, spinnerKV, spinnerKA); + spinnerFeedForwardExists = true; + return; + } + throw new IllegalArgumentException("Need to have 3 values for Spinner FeedForward: Friction, Voltage, and Acceleration"); + } + spinnerKS = 0; + spinnerKV = 0; + spinnerKA = 0; + spinnerFeedForwardExists = false; + DriverStation.reportWarning("SpinnerFeedForward is off", true); + } + + /** + * Makes sure gear reduction is not 0 + */ + private void checkGearReduction() { + if (gearReduction == 0) { + throw new IllegalArgumentException("Gear reduction cannot be 0"); + } + } + + /** + * Checks for {@link #maxVolts} and {@link #maxManualInput} to be greater than zero + */ + private void checkMaxVoltsAndInput() { + if (maxVolts <= 0) { + throw new IllegalArgumentException("maxVolts needs to be positive"); + } + if (maxManualInput <= 0) { + throw new IllegalArgumentException("maxManualInput needs to be positive"); + } } -} +} \ No newline at end of file diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java index b83c112..7935eae 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModule.java @@ -12,10 +12,14 @@ import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.revrobotics.spark.SparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.spark.SparkBase; import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkFlex; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkBaseConfig; +import com.revrobotics.spark.config.SparkFlexConfig; import com.revrobotics.spark.config.SparkMaxConfig; import edu.wpi.first.math.MathUtil; @@ -45,12 +49,27 @@ public class SwerveModule implements Sendable { public enum ModuleType {FL, FR, BL, BR}; + //constants + private final double neoStallTorqueNewtonMeters = 3.36; + private final double neoFreeCurrentAmps = 1.3; + private final double neoStallCurrentAmps = 166; + private final double neoVortexStallTorqueNewtonMeters = 3.6; + private final double neoVortexFreeCurrentAmps = 3.6; + private final double neoVortexStallCurrentAmps = 211; + private final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; + + private SwerveConfig config; - private SparkMaxConfig turnConfig = new SparkMaxConfig(); - private SparkMaxConfig driveConfig = new SparkMaxConfig(); + private SparkMaxConfig turnMaxConfig = new SparkMaxConfig(); + private SparkMaxConfig driveMaxConfig = new SparkMaxConfig(); + private SparkFlexConfig turnFlexConfig = new SparkFlexConfig(); + private SparkFlexConfig driveFlexConfig = new SparkFlexConfig(); private ModuleType type; - private SparkMax drive, turn; + private SparkMax maxDrive, maxTurn; + private SparkFlex flexDrive, flexTurn; + private RelativeEncoder driveRelEnc, turnRelEnc; + private boolean useFlexDrive, useFlexTurn; //false for max true for flex private CANcoder turnEncoder; private PIDController drivePIDController; private ProfiledPIDController turnPIDController; @@ -67,35 +86,125 @@ public enum ModuleType {FL, FR, BL, BR}; private double maxTurnVelocityWithoutTippingRps; public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkMax turn, CANcoder turnEncoder, int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { - //SmartDashboard.putNumber("Target Angle (deg)", 0.0); String moduleString = type.toString(); this.timer = new Timer(); timer.start(); - // SmartDashboard.putNumber("num periods",1); - // SmartDashboard.putNumber("maxOverShootDegree",1); this.config = config; this.type = type; - this.drive = drive; + this.maxDrive = drive; + this.useFlexDrive = false; + this.driveRelEnc = maxDrive.getEncoder(); + + driveMaxConfig.inverted(config.driveInversion[arrIndex]); + turnMaxConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveMaxConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor); + + maxControllableAccerlationRps2 = 0; + double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; + double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; + double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps); + // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); + driveMaxConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + + this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], + config.kForwardVels[arrIndex], + config.kForwardAccels[arrIndex]); + this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], + config.kBackwardVels[arrIndex], + config.kBackwardAccels[arrIndex]); + + drivePIDController = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); + + /* offset for 1 relative encoder count */ + drivetoleranceMPerS = (1.0 + / (double)(maxDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(maxDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * maxDrive.configAccessor.encoder.getQuadratureAverageDepth()); + drivePIDController.setTolerance(drivetoleranceMPerS); + + + this.maxTurn = turn; + this.useFlexTurn = false; + this.turnRelEnc = maxTurn.getEncoder(); + + this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], + config.turnkV[arrIndex], + config.turnkA[arrIndex]); + + // Voltage = kS + kV * velocity + kA * acceleration + // Assume cruising at maximum velocity --> 12 = kS + kV * max velocity + kA * 0 --> max velocity = (12 - kS) / kV + // Limit the velocity by a factor of 0.5 + maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); + maxTurnVelocityWithoutTippingRps = maxAchievableTurnVelocityRps; + // Assume accelerating while at limited speed --> 12 = kS + kV * limited speed + kA * acceleration + // acceleration = (12 - kS - kV * limiedSpeed) / kA + turnToleranceRot = Units.degreesToRotations(3 * 360/4096.0); /* degree offset for 3 CANCoder counts */ + maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); + + turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); + lastAngle = 0.0; + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); + turnPIDController.enableContinuousInput(-.5, .5); + turnPIDController.setTolerance(turnToleranceRot); + + this.driveModifier = config.driveModifier; + this.reversed = config.reversed[arrIndex]; + this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); + + turnPIDController.reset(getModuleAngle()); + + this.rollDegSupplier = rollDegSupplier; + this.pitchDegSupplier = pitchDegSupplier; + + SendableRegistry.addLW(this, "SwerveModule", type.toString()); + + //do stuff here + maxDrive.configure(driveMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + maxTurn.configure(turnMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkFlex turn, CANcoder turnEncoder, + int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + String moduleString = type.toString(); + this.timer = new Timer(); + timer.start(); + this.config = config; + this.type = type; + this.maxDrive = drive; + this.useFlexDrive = false; + this.driveRelEnc = maxDrive.getEncoder(); - driveConfig.inverted(config.driveInversion[arrIndex]); - turnConfig.inverted(config.turnInversion[arrIndex]); + driveMaxConfig.inverted(config.driveInversion[arrIndex]); + turnFlexConfig.inverted(config.turnInversion[arrIndex]); double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; - final double driveVelocityFactor = drivePositionFactor / 60;//why by 60? - driveConfig.encoder + final double driveVelocityFactor = drivePositionFactor / 60; + driveMaxConfig.encoder .positionConversionFactor(drivePositionFactor) .velocityConversionFactor(driveVelocityFactor); maxControllableAccerlationRps2 = 0; - final double normalForceNewtons = 83.2 /* lbf */ * 4.4482 /* N/lbf */ / 4 /* numModules */; double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; - final double neoStallTorqueNewtonMeters = 3.36; - final double neoFreeCurrentAmps = 1.3; - final double neoStallCurrentAmps = 166; double currentLimitAmps = neoFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoStallTorqueNewtonMeters * (neoStallCurrentAmps-neoFreeCurrentAmps); // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); - driveConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + driveMaxConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], config.kForwardVels[arrIndex], @@ -110,13 +219,14 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkM /* offset for 1 relative encoder count */ drivetoleranceMPerS = (1.0 - / (double)(drive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) - / Units.millisecondsToSeconds(drive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * drive.configAccessor.encoder.getQuadratureAverageDepth()); + / (double)(maxDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(maxDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * maxDrive.configAccessor.encoder.getQuadratureAverageDepth()); drivePIDController.setTolerance(drivetoleranceMPerS); - //System.out.println("Velocity Constant: " + (positionConstant / 60)); - this.turn = turn; + this.flexTurn = turn; + this.useFlexTurn = true; + this.turnRelEnc = flexTurn.getEncoder(); this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], config.turnkV[arrIndex], @@ -156,29 +266,202 @@ public SwerveModule(SwerveConfig config, ModuleType type, SparkMax drive, SparkM this.rollDegSupplier = rollDegSupplier; this.pitchDegSupplier = pitchDegSupplier; - // SmartDashboard.putNumber(moduleString + " Swerve kS", turnSimpleMotorFeedforward.ks); - // SmartDashboard.putNumber(moduleString + " Swerve kV", turnSimpleMotorFeedforward.kv); - // SmartDashboard.putNumber(moduleString + " Swerve kA", turnSimpleMotorFeedforward.ka); - // SmartDashboard.putNumber(moduleString + " Swerve kP", turnPIDController.getP()); - // SmartDashboard.putNumber(moduleString +" Swerve kD", turnPIDController.getD()); - // SmartDashboard.putNumber(moduleString + " Swerve Turn Tolerance", turnToleranceRot); + SendableRegistry.addLW(this, "SwerveModule", type.toString()); + + //do stuff here + maxDrive.configure(driveMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + flexTurn.configure(turnFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public SwerveModule(SwerveConfig config, ModuleType type, SparkFlex drive, SparkMax turn, CANcoder turnEncoder, + int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + String moduleString = type.toString(); + this.timer = new Timer(); + timer.start(); + this.config = config; + this.type = type; + this.flexDrive = drive; + this.useFlexDrive = true; + this.driveRelEnc = maxDrive.getEncoder(); + + driveFlexConfig.inverted(config.driveInversion[arrIndex]); + turnMaxConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveFlexConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor); + + maxControllableAccerlationRps2 = 0; + double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; + double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; + double currentLimitAmps = neoVortexFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoVortexStallCurrentAmps * (neoVortexStallCurrentAmps-neoVortexFreeCurrentAmps); + // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); + driveFlexConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + + this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], + config.kForwardVels[arrIndex], + config.kForwardAccels[arrIndex]); + this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], + config.kBackwardVels[arrIndex], + config.kBackwardAccels[arrIndex]); + + drivePIDController = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); + + /* offset for 1 relative encoder count */ + drivetoleranceMPerS = (1.0 + / (double)(flexDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(flexDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * flexDrive.configAccessor.encoder.getQuadratureAverageDepth()); + drivePIDController.setTolerance(drivetoleranceMPerS); + + + this.maxTurn = turn; + this.useFlexTurn = false; + this.turnRelEnc = maxTurn.getEncoder(); + + this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], + config.turnkV[arrIndex], + config.turnkA[arrIndex]); + + // Voltage = kS + kV * velocity + kA * acceleration + // Assume cruising at maximum velocity --> 12 = kS + kV * max velocity + kA * 0 --> max velocity = (12 - kS) / kV + // Limit the velocity by a factor of 0.5 + maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); + maxTurnVelocityWithoutTippingRps = maxAchievableTurnVelocityRps; + // Assume accelerating while at limited speed --> 12 = kS + kV * limited speed + kA * acceleration + // acceleration = (12 - kS - kV * limiedSpeed) / kA + turnToleranceRot = Units.degreesToRotations(3 * 360/4096.0); /* degree offset for 3 CANCoder counts */ + maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); + + turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); + lastAngle = 0.0; + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); + turnPIDController.enableContinuousInput(-.5, .5); + turnPIDController.setTolerance(turnToleranceRot); + + this.driveModifier = config.driveModifier; + this.reversed = config.reversed[arrIndex]; + this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); - // SmartDashboard.putNumber(moduleString + " Drive kP", drivePIDController.getP()); - // SmartDashboard.putNumber(moduleString + " Drive kD", drivePIDController.getD()); - // SmartDashboard.putNumber(moduleString + " Drive kS", forwardSimpleMotorFF.ks); - // SmartDashboard.putNumber(moduleString + " Drive kV", forwardSimpleMotorFF.kv); - // SmartDashboard.putNumber(moduleString + " Drive kA", forwardSimpleMotorFF.ka); - // SmartDashboard.putNumber(moduleString + " Drive Tolerance", turnToleranceRot); + turnPIDController.reset(getModuleAngle()); - // SmartDashboard.putData(this); + this.rollDegSupplier = rollDegSupplier; + this.pitchDegSupplier = pitchDegSupplier; SendableRegistry.addLW(this, "SwerveModule", type.toString()); //do stuff here - drive.configure(driveConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); - turn.configure(turnConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + flexDrive.configure(driveFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + maxTurn.configure(turnMaxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + } + + public SwerveModule(SwerveConfig config, ModuleType type, SparkFlex drive, SparkFlex turn, CANcoder turnEncoder, + int arrIndex, Supplier pitchDegSupplier, Supplier rollDegSupplier) { + String moduleString = type.toString(); + this.timer = new Timer(); + timer.start(); + this.config = config; + this.type = type; + this.flexDrive = drive; + this.useFlexDrive = true; + this.driveRelEnc = maxDrive.getEncoder(); + + driveFlexConfig.inverted(config.driveInversion[arrIndex]); + turnFlexConfig.inverted(config.turnInversion[arrIndex]); + + double drivePositionFactor = config.wheelDiameterMeters * Math.PI / config.driveGearing; + final double driveVelocityFactor = drivePositionFactor / 60; + driveFlexConfig.encoder + .positionConversionFactor(drivePositionFactor) + .velocityConversionFactor(driveVelocityFactor); + + maxControllableAccerlationRps2 = 0; + double wheelTorqueLimitNewtonMeters = normalForceNewtons * config.mu * config.wheelDiameterMeters / 2; + double motorTorqueLimitNewtonMeters = wheelTorqueLimitNewtonMeters / config.driveGearing; + double currentLimitAmps = neoVortexFreeCurrentAmps + 2*motorTorqueLimitNewtonMeters / neoVortexStallCurrentAmps * (neoVortexStallCurrentAmps-neoVortexFreeCurrentAmps); + // SmartDashboard.putNumber(type.toString() + " current limit (amps)", currentLimitAmps); + driveFlexConfig.smartCurrentLimit(Math.min(50, (int)currentLimitAmps)); + + this.forwardSimpleMotorFF = new SimpleMotorFeedforward(config.kForwardVolts[arrIndex], + config.kForwardVels[arrIndex], + config.kForwardAccels[arrIndex]); + this.backwardSimpleMotorFF = new SimpleMotorFeedforward(config.kBackwardVolts[arrIndex], + config.kBackwardVels[arrIndex], + config.kBackwardAccels[arrIndex]); + + drivePIDController = new PIDController(config.drivekP[arrIndex], + config.drivekI[arrIndex], + config.drivekD[arrIndex]); + + /* offset for 1 relative encoder count */ + drivetoleranceMPerS = (1.0 + / (double)(flexDrive.configAccessor.encoder.getCountsPerRevolution()) * drivePositionFactor) + / Units.millisecondsToSeconds(flexDrive.configAccessor.signals.getPrimaryEncoderPositionPeriodMs() * flexDrive.configAccessor.encoder.getQuadratureAverageDepth()); + drivePIDController.setTolerance(drivetoleranceMPerS); + + + this.flexTurn = turn; + this.useFlexTurn = true; + this.turnRelEnc = flexTurn.getEncoder(); + + this.turnSimpleMotorFeedforward = new SimpleMotorFeedforward(config.turnkS[arrIndex], + config.turnkV[arrIndex], + config.turnkA[arrIndex]); + + // Voltage = kS + kV * velocity + kA * acceleration + // Assume cruising at maximum velocity --> 12 = kS + kV * max velocity + kA * 0 --> max velocity = (12 - kS) / kV + // Limit the velocity by a factor of 0.5 + maxAchievableTurnVelocityRps = 0.5 * turnSimpleMotorFeedforward.maxAchievableVelocity(12.0, 0); + maxTurnVelocityWithoutTippingRps = maxAchievableTurnVelocityRps; + // Assume accelerating while at limited speed --> 12 = kS + kV * limited speed + kA * acceleration + // acceleration = (12 - kS - kV * limiedSpeed) / kA + turnToleranceRot = Units.degreesToRotations(3 * 360/4096.0); /* degree offset for 3 CANCoder counts */ + maxAchievableTurnAccelerationRps2 = 0.5 * turnSimpleMotorFeedforward.maxAchievableAcceleration(12.0, maxAchievableTurnVelocityRps); + + turnConstraints = new TrapezoidProfile.Constraints(maxAchievableTurnVelocityRps, maxAchievableTurnAccelerationRps2); + lastAngle = 0.0; + turnPIDController = new ProfiledPIDController( + config.turnkP[arrIndex], + config.turnkI[arrIndex], + config.turnkD[arrIndex], + turnConstraints); + turnPIDController.enableContinuousInput(-.5, .5); + turnPIDController.setTolerance(turnToleranceRot); + + this.driveModifier = config.driveModifier; + this.reversed = config.reversed[arrIndex]; + this.turnZeroDeg = config.turnZeroDeg[arrIndex]; + + CANcoderConfiguration CANconfig = new CANcoderConfiguration(); + CANconfig.MagnetSensor.AbsoluteSensorDiscontinuityPoint = .5; + this.turnEncoder = turnEncoder; + // CANconfig.MagnetSensor.MagnetOffset=-turnZeroDeg; //done in getModuleAngle. + this.turnEncoder.getConfigurator().apply(CANconfig); + turnPIDController.reset(getModuleAngle()); + + this.rollDegSupplier = rollDegSupplier; + this.pitchDegSupplier = pitchDegSupplier; + + SendableRegistry.addLW(this, "SwerveModule", type.toString()); + + //do stuff here + flexDrive.configure(driveFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); + flexTurn.configure(turnFlexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kPersistParameters); } public ModuleType getType() { @@ -186,6 +469,7 @@ public ModuleType getType() { } private double prevTurnVelocity = 0; + public void periodic() { drivePeriodic(); updateSmartDashboard(); @@ -216,8 +500,12 @@ public void drivePeriodic() { // SmartDashboard.putNumber(moduleString + " pidVolts", pidVolts); double appliedVoltage = MathUtil.clamp(targetVoltage, -12, 12); // SmartDashboard.putNumber(moduleString + " appliedVoltage", appliedVoltage); - - drive.setVoltage(appliedVoltage); + if (useFlexDrive) { + flexDrive.setVoltage(appliedVoltage); + } + else { + maxDrive.setVoltage(appliedVoltage); + } } public void turnPeriodic() { @@ -251,9 +539,19 @@ public void turnPeriodic() { turnFFVolts = turnSimpleMotorFeedforward.calculate(state.velocity);//(state.velocity-prevTurnVelocity) / period); turnVolts = turnFFVolts + turnSpeedCorrectionVolts; if (!turnPIDController.atGoal()) { - turn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); + if (useFlexTurn) { + flexTurn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); + } + else { + maxTurn.setVoltage(MathUtil.clamp(turnVolts, -12.0, 12.0)); + } } else { - turn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity)); + if (useFlexTurn) { + flexTurn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity)); + } + else { + maxTurn.setVoltage(turnSimpleMotorFeedforward.calculate(goal.velocity)); + } } prevTurnVelocity = state.velocity; } @@ -348,11 +646,11 @@ public SwerveModulePosition getCurrentPosition() { } public double getCurrentDistance() { - return drive.getEncoder().getPosition(); + return driveRelEnc.getPosition(); } public double getCurrentSpeed() { - return drive.getEncoder().getVelocity(); + return driveRelEnc.getVelocity(); } /** @@ -369,7 +667,7 @@ public void updateSmartDashboard() { SmartDashboard.putNumber(moduleString + " Absolute Angle (deg)", Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble())); // Display the module angle as calculated using the absolute encoder. SmartDashboard.putNumber(moduleString + " Turn Measured Pos (deg)", getModuleAngle()); - SmartDashboard.putNumber(moduleString + " Encoder Position", drive.getEncoder().getPosition()); + SmartDashboard.putNumber(moduleString + " Encoder Position", driveRelEnc.getPosition()); // Display the speed that the robot thinks it is travelling at. SmartDashboard.putNumber(moduleString + " Current Speed", getCurrentSpeed()); SmartDashboard.putBoolean(moduleString + " Drive is at Goal", drivePIDController.atSetpoint()); @@ -429,20 +727,40 @@ public void updateSmartDashboard() { } public void toggleMode() { - if (drive.configAccessor.getIdleMode() == IdleMode.kBrake && turn.configAccessor.getIdleMode() == IdleMode.kCoast) coast(); + IdleMode driveIdleMode; + IdleMode turnIdleMode; + if (useFlexDrive) { + driveIdleMode = flexDrive.configAccessor.getIdleMode(); + } + else { + driveIdleMode = maxDrive.configAccessor.getIdleMode(); + } + if (useFlexTurn) { + turnIdleMode = flexTurn.configAccessor.getIdleMode(); + } + else { + turnIdleMode = maxTurn.configAccessor.getIdleMode(); + } + if (driveIdleMode == IdleMode.kBrake && turnIdleMode == IdleMode.kCoast) coast(); else brake(); } public void brake() { - SparkBaseConfig config = new SparkMaxConfig().idleMode(IdleMode.kBrake); - drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - turn .configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + SparkBaseConfig maxConfig = new SparkMaxConfig().idleMode(IdleMode.kBrake); + SparkBaseConfig flexConfig = new SparkFlexConfig().idleMode(IdleMode.kBrake); + if (useFlexDrive) {flexDrive.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxDrive.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + if (useFlexTurn) {flexTurn.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxTurn.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} } public void coast() { - SparkBaseConfig config = new SparkMaxConfig().idleMode(IdleMode.kCoast); - drive.configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); - turn .configure(config, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters); + SparkBaseConfig maxConfig = new SparkMaxConfig().idleMode(IdleMode.kCoast); + SparkBaseConfig flexConfig = new SparkFlexConfig().idleMode(IdleMode.kCoast); + if (useFlexDrive) {flexDrive.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxDrive.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + if (useFlexTurn) {flexTurn.configure(flexConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} + else {maxTurn.configure(maxConfig, ResetMode.kNoResetSafeParameters, PersistMode.kNoPersistParameters);} } /** @@ -464,7 +782,7 @@ public void initSendable(SendableBuilder builder) { builder.addDoubleProperty("Incremental Position", () -> turnEncoder.getPosition().getValueAsDouble(), null); builder.addDoubleProperty("Absolute Angle (deg)", () -> Units.rotationsToDegrees(turnEncoder.getAbsolutePosition().getValueAsDouble()), null); builder.addDoubleProperty("Turn Measured Pos (deg)", this::getModuleAngle, null); - builder.addDoubleProperty("Encoder Position", drive.getEncoder()::getPosition, null); + builder.addDoubleProperty("Encoder Position", driveRelEnc::getPosition, null); // Display the speed that the robot thinks it is travelling at. builder.addDoubleProperty("Current Speed", this::getCurrentSpeed, null); builder.addDoubleProperty("Turn Setpoint Pos (deg)", () -> turnPIDController.getSetpoint().position, null); @@ -492,8 +810,10 @@ public void initSendable(SendableBuilder builder) { */ public SwerveModuleSim createSim(Mass massOnWheel, double turnGearing, double turnMoiKgM2) { double driveMoiKgM2 = massOnWheel.in(Kilogram) * Math.pow(config.wheelDiameterMeters/2, 2); - return new SwerveModuleSim(drive.getDeviceId(), config.driveGearing, driveMoiKgM2, - turn.getDeviceId(), turnEncoder.getDeviceID(), turnGearing, turnMoiKgM2); + int driveId = useFlexDrive ? flexDrive.getDeviceId() : maxDrive.getDeviceId(); + int turnId = useFlexTurn ? flexTurn.getDeviceId() : maxTurn.getDeviceId(); + return new SwerveModuleSim(driveId, config.driveGearing, driveMoiKgM2, + turnId, turnEncoder.getDeviceID(), turnGearing, turnMoiKgM2); } /** diff --git a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java index 4987540..c16ccc0 100644 --- a/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java +++ b/src/main/java/org/carlmontrobotics/lib199/swerve/SwerveModuleSim.java @@ -23,7 +23,7 @@ public class SwerveModuleSim { private Timer timer = new Timer(); /** - * Constructs a SwerveModuleSim that simulates the physics of a swerve module. + * Constructs a SwerveModuleSim that simulates the physics of a swerve module with Neos. * * @param drivePortNum the port of the SparkMax drive motor * @param driveGearing the gearing reduction between the drive motor and the wheel @@ -35,13 +35,61 @@ public class SwerveModuleSim { */ public SwerveModuleSim(int drivePortNum, double driveGearing, double driveMoiKgM2, int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, double turnMoiKgM2) { - driveMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", drivePortNum); - driveEncoderSim = new SimDeviceSim("CANEncoder:CANSparkMax", drivePortNum); + driveMotorSim = new SimDeviceSim("CANMotor:SparkMax", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:SparkMax", drivePortNum); DCMotor dcmotor = DCMotor.getNEO(1); drivePhysicsSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(dcmotor, driveMoiKgM2, driveGearing), dcmotor);//FIXME WHAT DO WE WANT THE MEASUREMENT STDDEVS TO BE? (LAST ARG) this.driveGearing = driveGearing; - turnMotorSim = new SimDeviceSim("CANMotor:CANSparkMax", turnMotorPortNum); + turnMotorSim = new SimDeviceSim("CANMotor:SparkMax", turnMotorPortNum); + turnEncoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", turnEncoderPortNum); + turnPhysicsSim = new DCMotorSim( + LinearSystemId.createDCMotorSystem(dcmotor, turnMoiKgM2, turnGearing), + dcmotor); + } + + /** + * Constructs a SwerveModuleSim that simulates the physics of a swerve module with Neos and Neo Vortexes. + * + * @param drivePortNum the port of the SparkMax drive motor + * @param driveGearing the gearing reduction between the drive motor and the wheel + * @param driveMoiKgM2 the effective moment of inertia around the wheel axle (typciall the mass of the robot divided the number of modules times the square of the wheel radius) + * @param turnMotorPortNum the port of the SparkMax turn motor + * @param turnEncoderPortNum the port of the CANCoder measuring the module's angle + * @param turnGearing the gearing reduction between the turn motor and the module + * @param turnMoiKgM2 the moment of inertia of the part of the module turned by the turn motor (in kg m^2) + * @param useFlexDrive boolean for whether to use Vortex or Neos for drive motors + * @param useFlexTurn boolean for whether to use Vortex or Neos for turn motors + */ + public SwerveModuleSim(int drivePortNum, double driveGearing, double driveMoiKgM2, + int turnMotorPortNum, int turnEncoderPortNum, double turnGearing, double turnMoiKgM2, boolean useFlexDrive, boolean useFlexTurn) { + DCMotor dcmotor; + double[] measurementStdDevs = { + 0.01, // position noise + 0.1 // velocity noise + }; + if (useFlexDrive) { + driveMotorSim = new SimDeviceSim("CANMotor:SparkFlex", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:SparkFlex", drivePortNum); + dcmotor = DCMotor.getNeoVortex(1); + } + else { + driveMotorSim = new SimDeviceSim("CANMotor:SparkMax", drivePortNum); + driveEncoderSim = new SimDeviceSim("CANEncoder:SparkMax", drivePortNum); + dcmotor = DCMotor.getNEO(1); + } + + drivePhysicsSim = new DCMotorSim(LinearSystemId.createDCMotorSystem(dcmotor, driveMoiKgM2, driveGearing), dcmotor, measurementStdDevs); + this.driveGearing = driveGearing; + + if (useFlexTurn) { + turnMotorSim = new SimDeviceSim("CANMotor:SparkFlex", turnMotorPortNum); + dcmotor = DCMotor.getNeoVortex(1); + } + else { + turnMotorSim = new SimDeviceSim("CANMotor:SparkMax", turnMotorPortNum); + dcmotor = DCMotor.getNEO(1); + } turnEncoderSim = new SimDeviceSim("CANDutyCycle:CANCoder", turnEncoderPortNum); turnPhysicsSim = new DCMotorSim( LinearSystemId.createDCMotorSystem(dcmotor, turnMoiKgM2, turnGearing), From cf1d47888990948d684755cdfc8915ae7b0b66ce Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Fri, 2 Jan 2026 18:46:09 -0800 Subject: [PATCH 08/10] Fixed java docs? --- .github/workflows/javadoc.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/javadoc.yml b/.github/workflows/javadoc.yml index 3ff8c19..abf4b65 100644 --- a/.github/workflows/javadoc.yml +++ b/.github/workflows/javadoc.yml @@ -17,13 +17,14 @@ jobs: - name: Generate Javadoc run: ./gradlew javadoc - name: Build Artifact - uses: actions/upload-pages-artifact@v1 + uses: actions/upload-pages-artifact@v4 with: path: ./build/docs/javadoc deploy-javadoc: needs: build-javadoc if: ${{ github.ref == 'refs/heads/master' }} permissions: + actions: read pages: write id-token: write environment: @@ -33,4 +34,4 @@ jobs: steps: - name: Deploy to GitHub Pages id: deployment - uses: actions/deploy-pages@v1 + uses: actions/deploy-pages@v4 \ No newline at end of file From cb9a5916763561c549b0c5c5fb3fc2b06010dee1 Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Fri, 2 Jan 2026 18:58:49 -0800 Subject: [PATCH 09/10] Now java docs should be fixed --- .../java/org/carlmontrobotics/lib199/Limelight.java | 5 +++++ .../SimpleMechs/Drivetrain/SimpleDrivetrain.java | 8 -------- .../lib199/SimpleMechs/Spinner/SpinnerConfig.java | 10 +++++----- .../org/carlmontrobotics/lib199/sim/MockedEncoder.java | 6 +++--- 4 files changed, 13 insertions(+), 16 deletions(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/Limelight.java b/src/main/java/org/carlmontrobotics/lib199/Limelight.java index 0aa06a6..ccd44f6 100644 --- a/src/main/java/org/carlmontrobotics/lib199/Limelight.java +++ b/src/main/java/org/carlmontrobotics/lib199/Limelight.java @@ -214,6 +214,11 @@ public Pose3d getTransform(Transform transform) { return new Pose3d(rawData[0], rawData[1], rawData[2], new Rotation3d(Math.toRadians(rawData[3]), Math.toRadians(rawData[4]), Math.toRadians(rawData[5]))); } + /** + * Gets Limelight Network Tables + * @param key id of the NT + * @return the NT + */ public NetworkTableEntry getNTEntry(String key) { return NetworkTableInstance.getDefault().getTable(config.ntName).getEntry(key); } diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java index ec64e62..24ce196 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Drivetrain/SimpleDrivetrain.java @@ -726,14 +726,6 @@ public void initSendable(SendableBuilder builder) { // #region Drive Methods - /** - * Drives the robot using the given x, y, and rotation speed - * - * @param forward The desired forward speed, in m/s. Forward is positive. - * @param strafe The desired strafe speed, in m/s. Left is positive. - * @param rotation The desired rotation speed, in rad/s. Counter clockwise is - * positive - */ public void setExtraSpeedMult(double set) { extraSpeedMult=set; } diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java index 88cf0bc..3ca35db 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Spinner/SpinnerConfig.java @@ -77,7 +77,7 @@ public SpinnerConfig(int spinnerMasterId, int spinnerFollowId, MotorConfig spinn * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor * @param maxVolts maximum volts that the spinner can use autonomously * @param spinnerKPID 3 values for kP, kI, and kD - * @param spinnerKPIDTolerance in rotations per second how much should pid care for speed + * @param spinnerPIDTolerance in rotations per second how much should pid care for speed */ public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxVolts, double[] spinnerKPID, double spinnerPIDTolerance) { return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO, null, spinnerMasterInverted, false, gearReduction, maxVolts, maxVolts/12.0, spinnerKPID, null, spinnerPIDTolerance); @@ -86,7 +86,7 @@ public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinne /** * Creates a simple single NEO spinner without PID * @param spinnerMasterId id of the motor - * @param spinnerMasterInversed inversed setting such that whichever is the desired movement of the spinner will be positive + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled * @return @@ -98,11 +98,11 @@ public static SpinnerConfig NEOSpinnerConfig(int spinnerMasterId, boolean spinne /** * Creates a simple single NEO spinner with PID * @param spinnerMasterId id of the motor - * @param spinnerMasterInversed inversed setting such that whichever is the desired movement of the spinner will be positive + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor * @param maxVolts maximum volts that the spinner can use autonomously * @param spinnerKPID 3 values for kP, kI, and kD - * @param spinnerKPIDTolerance in rotations per second how much should pid care for speed + * @param spinnerPIDTolerance in rotations per second how much should pid care for speed */ public static SpinnerConfig VORTEXSpinnerConfig(int spinnerMasterId, boolean spinnerMasterInverted, double gearReduction, double maxVolts, double[] spinnerKPID, double spinnerPIDTolerance) { return new SpinnerConfig(spinnerMasterId, -1, MotorConfig.NEO_VORTEX, null, spinnerMasterInverted, false, gearReduction, maxVolts, maxVolts/12.0, spinnerKPID, null, spinnerPIDTolerance); @@ -111,7 +111,7 @@ public static SpinnerConfig VORTEXSpinnerConfig(int spinnerMasterId, boolean spi /** * Creates a simple single NEO spinner without PID * @param spinnerMasterId id of the motor - * @param spinnerMasterInversed inversed setting such that whichever is the desired movement of the spinner will be positive + * @param spinnerMasterInverted inversed setting such that whichever is the desired movement of the spinner will be positive * @param gearReduction 1 revolution of the spinner / how many revolution is needed by the motor * @param maxManualInput maximum voltage percentage that the spinner can use while being manually controlled * @return diff --git a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java index e4f8fc7..06c4c86 100644 --- a/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java +++ b/src/main/java/org/carlmontrobotics/lib199/sim/MockedEncoder.java @@ -44,7 +44,7 @@ public class MockedEncoder implements AbsoluteEncoder, AnalogInput, AutoCloseabl * @param analog Whether the encoder is an analog sensor * @param absolute Whether the encoder is an absolute encoder. This flag caps the position to * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables - * {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + * {@link #setPosition(double)}, and enables setZeroOffset(double). * * {@link #getVelocity()} will return a value in rpm. */ @@ -59,7 +59,7 @@ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, * @param analog Whether the encoder is an analog sensor * @param absolute Whether the encoder is an absolute encoder. This flag caps the position to * one rotation via. {@link MathUtil#inputModulus(double, double, double)}, disables - * {@link #setPosition(double)}, and enables {@link #setZeroOffset(double)}. + * {@link #setPosition(double)}, and enables setZeroOffset(double). //FIXME where is this so called setZero * @param useRps Whether getVelocity() should return rps instead of rpm. */ public MockedEncoder(SimDevice device, int countsPerRev, boolean analog, @@ -106,7 +106,7 @@ public REVLibError setPosition(double newPosition) { // } /** - * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and {@link #setZeroOffset(double)}) + * @return The current position of the encoder, not accounting for the position offset ({@link #setPosition(double)} and setZeroOffset(double) */ public double getRawPosition() { double rotationsOrVolts = voltage != null ? voltage.get() : position.get(); From a38a5de5c1d3004b920315941917c17eb8eed94a Mon Sep 17 00:00:00 2001 From: Niosocket11 Date: Fri, 2 Jan 2026 19:04:37 -0800 Subject: [PATCH 10/10] Added curly bracket so tests will pass Hopefully it works --- .../org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java index 82fa97e..0cf591b 100644 --- a/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java +++ b/src/main/java/org/carlmontrobotics/lib199/SimpleMechs/Arm/ArmConfig.java @@ -107,7 +107,7 @@ public ArmConfig( /** * Create a single motor arm config with PID, no FeedForward * @param armMasterMotorId master id - * @param armMasterMotorType @link MotorConfig} of the master motor + * @param armMasterMotorType {@link MotorConfig} of the master motor * @param armMasterInversed inverse config such that positive direction for the motor will result in the arm moving up * @param armKPID double array with 3 values, kP, kI, kD * @param bottomLimit lowest angle in degrees the arm can safely achieve with 0 being horizontal