summaryrefslogtreecommitdiff
path: root/src/main/java/frc/robot/subsystems
diff options
context:
space:
mode:
Diffstat (limited to 'src/main/java/frc/robot/subsystems')
-rw-r--r--src/main/java/frc/robot/subsystems/Arm.java164
-rw-r--r--src/main/java/frc/robot/subsystems/Chain.java22
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java1
-rw-r--r--src/main/java/frc/robot/subsystems/Hooks.java25
-rw-r--r--src/main/java/frc/robot/subsystems/Intake.java22
-rw-r--r--src/main/java/frc/robot/subsystems/Shooter.java10
6 files changed, 216 insertions, 28 deletions
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index dca7ebd..75e8ad2 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -4,56 +4,192 @@ import org.littletonrobotics.junction.Logger;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.units.Angle;
+import edu.wpi.first.units.Measure;
+import edu.wpi.first.units.MutableMeasure;
+import edu.wpi.first.units.Velocity;
+import edu.wpi.first.units.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.PIDCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.Constants;
+import static edu.wpi.first.units.MutableMeasure.mutable;
+import static edu.wpi.first.units.Units.Volts;
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Seconds;
+
public class Arm extends SubsystemBase
{
- private final CANSparkMax m_armL = Constants.armL;
- private final CANSparkMax m_armR = Constants.armR;
+ // rad
+ private double m_setpoint = Constants.arm_initial_position;
+ private final CANSparkMax m_armL = new CANSparkMax(Constants.armL_ID, MotorType.kBrushless);
+ private final CANSparkMax m_armR = new CANSparkMax(Constants.armR_ID, MotorType.kBrushless);
private final RelativeEncoder m_encoderL = m_armL.getEncoder();
private final RelativeEncoder m_encoderR = m_armR.getEncoder();
- private final ArmFeedforward m_feedforward = Constants.arm_feedforward;
+ private ArmFeedforward m_feedforward = new ArmFeedforward(Constants.ArmkS, Constants.ArmkG, Constants.ArmkV);
- private final ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
- private final ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+ private ProfiledPIDController m_pidL = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+ private ProfiledPIDController m_pidR = new ProfiledPIDController(Constants.ArmkP, Constants.ArmkI, Constants.ArmkD, Constants.arm_constraints);
+ // SYSID
+ private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
+ private final MutableMeasure<Angle> m_angle = mutable(Radians.of(0));
+ private final MutableMeasure<Velocity<Angle>> m_velocity = mutable(RadiansPerSecond.of(0));
+
+ private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
+ new SysIdRoutine.Config(
+ Volts.per(Seconds).of(0.5),
+ Volts.of(3),
+ Seconds.of(10)),
+ new SysIdRoutine.Mechanism(
+ (Measure<Voltage> volts) -> {
+ m_armL.setVoltage(volts.in(Volts));
+ m_armR.setVoltage(volts.in(Volts));
+ },
+ log -> {
+ log.motor("arm-left")
+ .voltage(
+ m_voltage.mut_replace(
+ m_armL.getAppliedOutput() * m_armL.getBusVoltage(), Volts)
+ )
+ .angularPosition(m_angle.mut_replace(m_encoderL.getPosition(), Radians))
+ .angularVelocity(m_velocity.mut_replace(m_encoderL.getVelocity(), RadiansPerSecond));
+ log.motor("arm-right")
+
+ .voltage(
+ m_voltage.mut_replace(
+ m_armR.getAppliedOutput() * m_armR.getBusVoltage(), Volts)
+ )
+ .angularPosition(m_angle.mut_replace(m_encoderR.getPosition(), Radians))
+ .angularVelocity(m_velocity.mut_replace(m_encoderR.getVelocity(), RadiansPerSecond));
+
+ }, this));
+
public Arm()
{
+ m_armL.setInverted(false);
m_armR.setInverted(true);
m_encoderL.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
m_encoderR.setPositionConversionFactor(2*Math.PI / Constants.ArmGearReduction);
+ reset();
+ }
+
+ // public Arm(double kP, double kI, double kD, double kS, double kG, double kV)
+ // {
+ // this();
+
+ // m_feedforward = new ArmFeedforward(kS, kG, kV);
+ // m_pidL = new ProfiledPIDController(kP, kI, kD, Constants.arm_constraints);
+ // m_pidR = new ProfiledPIDController(kP, kI, kD, Constants.arm_constraints);
+ // }
+
+ public void reset()
+ {
+ m_setpoint = Constants.arm_initial_position;
+ m_encoderL.setPosition(Constants.arm_initial_position);
+ m_encoderR.setPosition(Constants.arm_initial_position);
+
+ m_pidL.reset(Constants.arm_initial_position);
+ m_pidR.reset(Constants.arm_initial_position);
}
+
+ public void incrementAngleDegrees(double amount) {
+ incrementAngle(amount * Math.PI / 180.0);
+ }
+
+ public void incrementAngle(double amount) {
+ atAngle(m_setpoint + amount);
+ }
+
+
// in rad
public void atAngle(double angle)
{
+ m_setpoint = angle;
+ m_pidL.setGoal(m_setpoint);
+ m_pidR.setGoal(m_setpoint);
+
voltage(
- m_pidL.calculate(m_encoderL.getPosition())
- + m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidR.getSetpoint().velocity),
+ m_pidL.calculate(m_encoderL.getPosition()),
+ m_feedforward.calculate(m_pidL.getSetpoint().position, m_pidL.getSetpoint().velocity),
- m_pidR.calculate(m_encoderR.getPosition())
- + m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
+ m_pidR.calculate(m_encoderR.getPosition()),
+ m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
);
}
+ // in deg
+ public void atAngleDegrees(double angle)
+ {
+ atAngle(angle * Math.PI / 180.0);
+ }
+
+ // in rad
+ public boolean hasClearedAngle(double angle)
+ {
+ if(m_encoderL.getPosition() > angle
+ && m_encoderR.getPosition() > angle)
+ return true;
+
+ return false;
+ }
+
+ public boolean hasClearedAngleDegree(double angle)
+ {
+ return hasClearedAngle(angle * Math.PI/180);
+ }
+ // in rad
+ public boolean isAtAngle(double angle)
+ {
+ if(Math.abs(m_encoderL.getPosition() - angle) < Constants.ArmEpsilon
+ && Math.abs(m_encoderR.getPosition() - angle) < Constants.ArmEpsilon)
+ return true;
+ return false;
+ }
- public void voltage(double leftVolts, double rightVolts)
+ public boolean isAtAngleDegree(double angle)
{
- m_armL.setVoltage(leftVolts);
- m_armR.setVoltage(rightVolts);
+ return isAtAngle(angle * Math.PI/180);
+ }
+
+ public void voltage(double leftVoltsPID, double leftVoltsFFW, double rightVoltsPID, double rightVoltsFFW)
+ {
+ Logger.recordOutput(getName() + "/leftPID", leftVoltsPID);
+ Logger.recordOutput(getName() + "/rightPID", rightVoltsPID);
+ Logger.recordOutput(getName() + "/leftFFW", leftVoltsFFW);
+ Logger.recordOutput(getName() + "/rightFFW", rightVoltsFFW);
+
+ m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
+ m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
}
@Override
public void periodic()
{
- Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition());
- Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition());
+ Logger.recordOutput(getName() + "/positionL", m_encoderL.getPosition() * 180/Math.PI);
+ Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
+ }
+
+ public Command sysIdQausistatic(SysIdRoutine.Direction direction)
+ {
+ return m_sysIdRoutine.quasistatic(direction);
+ }
+
+ public Command sysIdDynamic(SysIdRoutine.Direction direction)
+ {
+ return m_sysIdRoutine.dynamic(direction);
}
}
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 2485739..40235fe 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -3,7 +3,9 @@ package frc.robot.subsystems;
import org.littletonrobotics.junction.Logger;
import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -11,15 +13,16 @@ import frc.robot.Constants;
public class Chain extends SubsystemBase
{
- private final CANSparkMax m_motor = Constants.chain;
- private final Encoder m_encoder = Constants.chain_encoder;
+ private final CANSparkMax m_motor = new CANSparkMax(Constants.chain_ID, MotorType.kBrushed);
+ private final Encoder m_encoder = new Encoder(Constants.chain_encoder_PINS[0], Constants.chain_encoder_PINS[1]);
private final ProfiledPIDController m_pid = new ProfiledPIDController(Constants.ChainkP, Constants.ChainkI, Constants.ChainkD, Constants.chain_constraints);
public Chain()
{
+ // TODO: figure out invertedness
+
// m_motor.setInverted(true);
- // m_encoder.setReverseDirection(true);
m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
m_encoder.reset();
@@ -33,7 +36,18 @@ public class Chain extends SubsystemBase
// perc between 0 and 1;
public void atPercentage(double perc)
{
- m_motor.setVoltage(m_pid.calculate(m_encoder.getDistance(), perc));
+ // System.out.println(perc);
+ Logger.recordOutput(getName() + "/perc", perc);
+ double pid_out = m_pid.calculate(m_encoder.getDistance(), perc);
+ Logger.recordOutput(getName() + "/pidOUt", pid_out);
+ m_motor.setVoltage(MathUtil.clamp(pid_out, -6, 6));
+ }
+
+ public boolean isAtPosition(double pos)
+ {
+ if(Math.abs(pos - m_encoder.getDistance()) < Constants.ChainEpsilon)
+ return true;
+ return false;
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5f34119..aee9597 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -13,7 +13,6 @@ import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
import edu.wpi.first.math.trajectory.TrajectoryConfig;
-import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index eb9c4f7..b6c26c5 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -1,8 +1,31 @@
package frc.robot.subsystems;
+import java.lang.constant.Constable;
+
+import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
+
+import frc.robot.Constants;
+
+
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Hooks extends SubsystemBase
{
-
+ private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
+ private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);
+
+ public Hooks()
+ {
+ // TODO: possibly invert to move forward upon positive votage
+ // TODO: invert one hook
+ m_hookL.setInverted(true);
+ }
+
+ public void atPercentage(double percL, double percR)
+ {
+ m_hookL.set(percL);
+ m_hookR.set(percR);
+ }
+
}
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 85cbbf8..41620bb 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -1,17 +1,25 @@
package frc.robot.subsystems;
import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
+import com.revrobotics.RelativeEncoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class Intake extends SubsystemBase
{
- private final CANSparkMax m_intakeT = Constants.intakeT;
- private final CANSparkMax m_intakeD = Constants.intakeD;
+ public static final CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
+ public static final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
+ private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();
- public Intake() { }
+ public Intake()
+ {
+ m_intakeT.setInverted(true);
+ m_intakeD.setInverted(true);
+ }
public void atPercentage(double perc)
{
@@ -23,12 +31,18 @@ public class Intake extends SubsystemBase
m_intakeT.set(upPerc);
m_intakeD.set(downPerc);
}
+ public void shoot()
+ {
+ if (m_intakeD_encoder.getVelocity() > 2000) {
+ m_intakeT.set(1);
+ }
+ m_intakeD.set(1);
+ }
@Override
public void periodic()
{
Logger.recordOutput(getName() + "/speedT", m_intakeT.getEncoder().getVelocity());
Logger.recordOutput(getName() + "/speedD", m_intakeD.getEncoder().getVelocity());
-
}
}
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index f171049..677073c 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -1,17 +1,19 @@
package frc.robot.subsystems;
import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.CANSparkLowLevel.MotorType;
import com.revrobotics.CANSparkMax;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import frc.robot.Constants;
public class Shooter extends SubsystemBase
-{
- private final CANSparkMax m_shooterT = Constants.shooterT;
- private final CANSparkMax m_shooterD = Constants.shooterD;
+{
+ public static final CANSparkMax m_shooterT = new CANSparkMax(Constants.shooterT_ID, MotorType.kBrushless);
+ public static final CANSparkMax m_shooterD = new CANSparkMax(Constants.shooterD_ID, MotorType.kBrushless);
- public Shooter() { }
+ public Shooter() {}
public void atPercentage(double perc)
{
generated by cgit v1.2.3 (git 2.25.1) at 2025-01-23 05:09:31 +0000