diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 38581a6..fc0fd23 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -18,11 +18,13 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.AimCommand; +import frc.robot.commands.ArmCommand; import frc.robot.commands.DefaultDrive; import frc.robot.commands.ElevatorCommand; import frc.robot.commands.FlywheelCommand; import frc.robot.commands.LifterCommand; import frc.robot.commands.StraightCommand; +import frc.robot.subsystems.ArmSubsystem; import frc.robot.subsystems.CameraSubsystem; import frc.robot.subsystems.DriveSubsystem; import frc.robot.subsystems.DriverCameraSubsystem; @@ -63,6 +65,7 @@ public class RobotContainer { new LifterSubsystem(Constants.CANConstants.MOTORLIFTERRIGHTID); private final DriverCameraSubsystem m_DriverCameraSubsystem = new DriverCameraSubsystem(); private final ElevatorSubsystem m_ElevatorSubsystem = new ElevatorSubsystem(); + private final ArmSubsystem m_ArmSubsystem = new ArmSubsystem(); // The robots commands are defined here.. // private final ExampleCommand m_autoCommand = new ExampleCommand(m_exampleSubsystem); @@ -75,6 +78,7 @@ public class RobotContainer { private final LifterCommand m_LeftLifterCommand = new LifterCommand(m_leftLifterSubsystem); private final LifterCommand m_RightLifterCommand = new LifterCommand(m_rightLifterSubsystem); private final ElevatorCommand m_ElevatorCommand = new ElevatorCommand(m_ElevatorSubsystem); + private final ArmCommand m_ArmCommand = new ArmCommand(m_ArmSubsystem); // EX: these commands are used by autonomous only // private final AimAmpCommand m_AimAmpCommand = new AimAmpCommand(m_armSubsystem, diff --git a/src/main/java/frc/robot/commands/ArmCommand.java b/src/main/java/frc/robot/commands/ArmCommand.java new file mode 100644 index 0000000..a7412cf --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -0,0 +1,39 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.ArmSubsystem; + +/** An Arm command that uses the Arm subsystem. */ +public class ArmCommand extends Command { + private final ArmSubsystem m_ArmSubsystem; + + /** + * Create a new ArmCommand. + * + * @param a_Subsystem The subsystem used by this command. + */ + public ArmCommand(ArmSubsystem a_Subsystem) { + m_ArmSubsystem = a_Subsystem; + + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(a_Subsystem); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // 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 false; + } +} diff --git a/src/main/java/frc/robot/subsystems/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/ArmSubsystem.java new file mode 100644 index 0000000..e9113ae --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ArmSubsystem.java @@ -0,0 +1,211 @@ +package frc.robot.subsystems; + +import static edu.wpi.first.units.Units.Volts; + +import com.revrobotics.RelativeEncoder; +import com.revrobotics.sim.SparkMaxSim; +import com.revrobotics.sim.SparkRelativeEncoderSim; +import com.revrobotics.spark.SparkBase; +import com.revrobotics.spark.SparkBase.PersistMode; +import com.revrobotics.spark.SparkBase.ResetMode; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.system.plant.DCMotor; +import edu.wpi.first.units.measure.Voltage; +import edu.wpi.first.wpilibj.RobotController; +import edu.wpi.first.wpilibj.simulation.BatterySim; +import edu.wpi.first.wpilibj.simulation.RoboRioSim; +import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import frc.robot.Constants.CANConstants; +import frc.robot.DriveConstants; + +public class ArmSubsystem extends SubsystemBase { + // Decalare Motor + private final SparkMax m_Motor; + // Declare Simulated Motor + private final DCMotor m_simGearbox; + private final SparkMaxSim m_simMotor; + + // Declare Motor Configs + private final SparkMaxConfig m_MotorConfig = new SparkMaxConfig(); + // Declare PID + private final SparkClosedLoopController m_ArmMainPIDController; + // Declare Encoder + private RelativeEncoder m_ArmEncoder; + // Declare Simulated Encoder + private final SparkRelativeEncoderSim m_ArmEncoderSim; + // Declare Arm Physics Engine + private final SingleJointedArmSim m_ArmSim; + + // TODO: Update to accurate values + private final double kP, kI, kD, kIz, kMaxOutput, kMinOutput; + // general drive constants + // https://www.chiefdelphi.com/t/encoders-velocity-to-m-s/390332/2 + // https://sciencing.com/convert-rpm-linear-speed-8232280.html + private final double kWheelDiameter = 1; // meters TODO: Figure out ratio + private final double kGearRatio = 16; // TBD + // basically converted from rotations to to radians to then meters using the wheel diameter. + // the diameter is already *2 so we don't need to multiply by 2 again. + private final double kPositionConversionRatio = (Math.PI * kWheelDiameter) / kGearRatio; + private final double kVelocityConversionRatio = kPositionConversionRatio / 60; + + // setup feedforward + private final double kS = 0.1; // Static Friction (Volts) + private final double kG = 0.1; // Inertia (Volts) + private final double kV = 0.1; // Mass Volts/(rad/s) + private final double kA = 0.1; // Acceleration Volts/(rad/s^2) + + // other constants + private final double kMaxAngleRads = 1.0; // TODO: Update + private final double kMinAngleRads = 0.0; + private final double kStartingAngleRads = kMinAngleRads + 0.0; + private final double kArmLengthMeters = 0.0; + private final double kjKgMetersSquared = + 0.0; // The moment of inertia of the arm; can be calculated from CAD software. + + ArmFeedforward m_ArmFeedforward = new ArmFeedforward(kS, kG, kV, kA); + + // setup trapezoidal motion profile + private final double kMaxVelocity = 0.1; // M/S + private final double kMaxAcceleration = 0.01; // M/S^2 + private final double kAllowedClosedLoopError = 0.01; // Meters + + // setup SysID for auto profiling + private final SysIdRoutine m_sysIdRoutine; + + // current limit + private final int k_CurrentLimit = 60; + + public ArmSubsystem() { + // Create Arm motor + m_Motor = new SparkMax(CANConstants.MOTORARMMAINID, SparkMax.MotorType.kBrushless); + + // Create Simulated Motors + m_simGearbox = DCMotor.getNEO(1); + m_simMotor = new SparkMaxSim(m_Motor, m_simGearbox); + + // Create Simulated encoder + m_ArmEncoderSim = m_simMotor.getRelativeEncoderSim(); + + // Create Simulated Physics Engine + m_ArmSim = + new SingleJointedArmSim( + m_simGearbox, + kGearRatio, + kjKgMetersSquared, + kArmLengthMeters, + kMinAngleRads, + kMaxAngleRads, + true, + kStartingAngleRads, + 0.01, + 0.0); + + // Set idle mode to coast + m_MotorConfig.idleMode(IdleMode.kBrake); + // Set current limit + m_MotorConfig.smartCurrentLimit(k_CurrentLimit); + + // Connect to built in PID controller + m_ArmMainPIDController = m_Motor.getClosedLoopController(); + + // Allow us to read the encoder + m_ArmEncoder = m_Motor.getEncoder(); + + m_MotorConfig.encoder.positionConversionFactor(kPositionConversionRatio); + m_MotorConfig.encoder.velocityConversionFactor(kVelocityConversionRatio); + + // PID coefficients + kP = 0.0; + kI = 0; + kD = 0; + kIz = 0; + kMaxOutput = 0.8; + kMinOutput = -0.8; + // set PID coefficients + m_MotorConfig.closedLoop.pid(kP, kI, kD, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.iZone(kIz, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.outputRange( + kMinOutput, kMaxOutput, DriveConstants.kDrivetrainPositionPIDSlot); + // Smart Control Config + m_MotorConfig.closedLoop.maxMotion.maxVelocity( + kMaxVelocity, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.maxMotion.maxAcceleration( + kMaxAcceleration, DriveConstants.kDrivetrainPositionPIDSlot); + m_MotorConfig.closedLoop.maxMotion.allowedClosedLoopError( + kAllowedClosedLoopError, DriveConstants.kDrivetrainPositionPIDSlot); + // setup SysID for auto profiling + m_sysIdRoutine = + new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (voltage) -> this.setVoltage(voltage), + null, // No log consumer, since data is recorded by URCL + this)); + + m_Motor.configure( + m_MotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); + } + + public void setVoltage(Voltage voltage) { + m_Motor.setVoltage(voltage.in(Volts)); + } + + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.quasistatic(direction); + } + + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return m_sysIdRoutine.dynamic(direction); + } + + /* + * Move Arm to a specific angle + */ + public void SetAngle(double radians) { + m_ArmMainPIDController.setReference( + radians, + SparkBase.ControlType.kMAXMotionPositionControl, + DriveConstants.kDrivetrainPositionPIDSlot, + m_ArmFeedforward.calculate(radians, m_ArmEncoder.getVelocity())); + } + + /* + * Lower the Arm + */ + public void LowerArm() { + SetAngle(0); + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + } + + @Override + public void simulationPeriodic() { + // This method will be called once per scheduler run during simulation + // Update the simulation of our Arm, set inputs + m_ArmSim.setInput(m_simMotor.getAppliedOutput() * RobotController.getBatteryVoltage()); + + // update simulation (20ms) + m_ArmSim.update(0.020); + + // Iterate PID loops + m_simMotor.iterate(m_ArmSim.getVelocityRadPerSec(), RoboRioSim.getVInVoltage(), 0.02); + + // add load to battery + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(m_simMotor.getMotorCurrent())); + + // update encoder + m_ArmEncoderSim.setPosition(m_ArmSim.getAngleRads()); + m_ArmEncoderSim.setVelocity(m_ArmSim.getVelocityRadPerSec()); + } +}