diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3862e4f..4b8c518 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -121,7 +121,13 @@ public final class Constants { public static final class GyroConstants { public static final int ID = 14; // TODO: find the actual ID } - + + public static final class ArmConstants { + public static final int MIN_ARM_LEN = 0; + public static final int MAX_ARM_LEN = 1; + public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; + } + public static final class LEDConstants { // public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/subsystems/Arm.java b/src/main/java/frc4388/robot/subsystems/Arm.java new file mode 100644 index 0000000..6367d4d --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Arm.java @@ -0,0 +1,39 @@ +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import frc4388.robot.Constants.ArmConstants; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Arm extends SubsystemBase { + private WPI_TalonFX m_armmotor; + private boolean m_debug; + // Moves arm to distence [dist] then returns new ang + public Arm(WPI_TalonFX armmotor, boolean debug) { + armmotor.configFactoryDefault(); + m_armmotor = armmotor; + } + + public Arm(WPI_TalonFX armmoter) { + this(armmoter, false); + } + + public void armSetLength(double len) { + if (len > 1 || len < 0) return; + // Move arm code + m_armmotor.set(ControlMode.Position, len * (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN) + + ArmConstants.MIN_ARM_LEN); + } + + public double getArmLength() { + return (m_armmotor.getSelectedSensorPosition() - ArmConstants.MIN_ARM_LEN) / + (ArmConstants.MAX_ARM_LEN - ArmConstants.MIN_ARM_LEN); + } + + @Override + public void periodic() { + if (m_debug) + SmartDashboard.putNumber("Arm Motor", m_armmotor.getSelectedSensorPosition()); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Claw.java b/src/main/java/frc4388/robot/subsystems/Claw.java new file mode 100644 index 0000000..37793b1 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Claw.java @@ -0,0 +1,23 @@ +package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.PWM; + +public class Claw { + private PWM m_clawMotor; + private boolean m_open = false; + + // Opens claw + public Claw(PWM m_clawMotor) { + this.m_clawMotor = m_clawMotor; + setClaw(true); + } + + public void setClaw(boolean open) { + // Open claw + m_open = open; + m_clawMotor.setRaw(open ? 0 : 2000); + } + + public boolean isClawOpen() { + return m_open; + } +} \ No newline at end of file