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.java67
-rw-r--r--src/main/java/frc/robot/subsystems/Chain.java6
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java23
-rw-r--r--src/main/java/frc/robot/subsystems/Hooks.java18
-rw-r--r--src/main/java/frc/robot/subsystems/Intake.java12
-rw-r--r--src/main/java/frc/robot/subsystems/MainMech.java84
-rw-r--r--src/main/java/frc/robot/subsystems/Shooter.java1
7 files changed, 157 insertions, 54 deletions
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 75e8ad2..0b8814d 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -8,7 +8,6 @@ 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;
@@ -16,7 +15,6 @@ 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;
@@ -105,15 +103,9 @@ public class Arm extends SubsystemBase
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);
+ public double pos() {
+ return Math.min(m_encoderL.getPosition(), m_encoderR.getPosition()) * 180 / Math.PI;
}
-
// in rad
public void atAngle(double angle)
@@ -130,26 +122,36 @@ public class Arm extends SubsystemBase
m_feedforward.calculate(m_pidR.getSetpoint().position, m_pidR.getSetpoint().velocity)
);
}
+
+ 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));
+ }
+
// in deg
- public void atAngleDegrees(double angle)
+ public void incrementAngleDegrees(double amount)
{
- atAngle(angle * Math.PI / 180.0);
+ incrementAngle(amount * Math.PI / 180.0);
}
// in rad
- public boolean hasClearedAngle(double angle)
+ public void incrementAngle(double amount)
{
- if(m_encoderL.getPosition() > angle
- && m_encoderR.getPosition() > angle)
- return true;
-
- return false;
+ atAngle(m_setpoint + amount);
}
- public boolean hasClearedAngleDegree(double angle)
+ // in deg
+ public void atAngleDegrees(double angle)
{
- return hasClearedAngle(angle * Math.PI/180);
+ atAngle(angle * Math.PI / 180.0);
}
+
// in rad
public boolean isAtAngle(double angle)
{
@@ -160,21 +162,27 @@ public class Arm extends SubsystemBase
return false;
}
+ // in deg
public boolean isAtAngleDegree(double angle)
{
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);
+ // 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);
+ // }
- m_armL.setVoltage(MathUtil.clamp(leftVoltsPID + leftVoltsFFW, -4, 4));
- m_armR.setVoltage(MathUtil.clamp(rightVoltsPID + rightVoltsFFW, -4, 4));
- }
@Override
public void periodic()
@@ -183,6 +191,7 @@ public class Arm extends SubsystemBase
Logger.recordOutput(getName() + "/positionR", m_encoderR.getPosition() * 180/Math.PI);
}
+ // Sysid
public Command sysIdQausistatic(SysIdRoutine.Direction direction)
{
return m_sysIdRoutine.quasistatic(direction);
diff --git a/src/main/java/frc/robot/subsystems/Chain.java b/src/main/java/frc/robot/subsystems/Chain.java
index 40235fe..3ee7108 100644
--- a/src/main/java/frc/robot/subsystems/Chain.java
+++ b/src/main/java/frc/robot/subsystems/Chain.java
@@ -20,10 +20,6 @@ public class Chain extends SubsystemBase
public Chain()
{
- // TODO: figure out invertedness
-
- // m_motor.setInverted(true);
-
m_encoder.setDistancePerPulse(1.0/Constants.ChainEncoderScale);
m_encoder.reset();
}
@@ -36,9 +32,9 @@ public class Chain extends SubsystemBase
// perc between 0 and 1;
public void atPercentage(double 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));
}
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index aee9597..66d1ba6 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -1,6 +1,7 @@
package frc.robot.subsystems;
import com.revrobotics.CANSparkMax;
+import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
@@ -40,15 +41,15 @@ public class Drivetrain extends SubsystemBase
{
private IMUGyro m_gyro = Constants.gyro;
- private CANSparkMax m_motorL_leader = Constants.motorL_leader;
- private CANSparkMax m_motorL_follower = Constants.motorL_follower;
- private CANSparkMax m_motorR_leader = Constants.motorR_leader;
- private CANSparkMax m_motorR_follower = Constants.motorR_follower;
+ private CANSparkMax m_motorL_leader = new CANSparkMax(Constants.motorL_leader_ID, MotorType.kBrushed);
+ private CANSparkMax m_motorL_follower = new CANSparkMax(Constants.motorL_follower_ID, MotorType.kBrushed);
+ private CANSparkMax m_motorR_leader = new CANSparkMax(Constants.motorR_leader_ID, MotorType.kBrushed);
+ private CANSparkMax m_motorR_follower = new CANSparkMax(Constants.motorR_follower_ID, MotorType.kBrushed);
- private Encoder m_encoderL = Constants.encoderL;
- private Encoder m_encoderR = Constants.encoderR;
+ private Encoder m_encoderL = new Encoder(Constants.encoderL_PINS[0], Constants.encoderL_PINS[1]);
+ private Encoder m_encoderR = new Encoder(Constants.encoderR_PINS[0], Constants.encoderR_PINS[1]);
- private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward;
+ private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(Constants.MotorkS, Constants.MotorkV);
private final double kP = 0.363;
@@ -116,8 +117,8 @@ public class Drivetrain extends SubsystemBase
m_odometry = new DifferentialDriveOdometry(
m_gyro.toRotation2d(),
- Constants.encoderL.getDistance(),
- Constants.encoderR.getDistance());
+ m_encoderL.getDistance(),
+ m_encoderR.getDistance());
}
// speed - linear velocity in m/s
@@ -168,8 +169,8 @@ public class Drivetrain extends SubsystemBase
m_odometry = new DifferentialDriveOdometry(
m_gyro.toRotation2d(),
- Constants.encoderL.getDistance(),
- Constants.encoderR.getDistance());
+ m_encoderL.getDistance(),
+ m_encoderR.getDistance());
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/Hooks.java b/src/main/java/frc/robot/subsystems/Hooks.java
index b6c26c5..6e42322 100644
--- a/src/main/java/frc/robot/subsystems/Hooks.java
+++ b/src/main/java/frc/robot/subsystems/Hooks.java
@@ -1,25 +1,29 @@
package frc.robot.subsystems;
-import java.lang.constant.Constable;
+import org.littletonrobotics.junction.Logger;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
import frc.robot.Constants;
-
-
+import edu.wpi.first.hal.FRCNetComm.tResourceType;
+import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Hooks extends SubsystemBase
{
+ // initial angle 237
private final CANSparkMax m_hookL = new CANSparkMax(Constants.hookL_ID, MotorType.kBrushed);
private final CANSparkMax m_hookR = new CANSparkMax(Constants.hookR_ID, MotorType.kBrushed);
+ private final Encoder m_hookL_encoder = new Encoder(Constants.hookL_PINS[0], Constants.hookL_PINS[1]);
+ private final Encoder m_hookR_encoder = new Encoder(Constants.hookR_PINS[0], Constants.hookR_PINS[1]);
+
public Hooks()
{
// TODO: possibly invert to move forward upon positive votage
- // TODO: invert one hook
m_hookL.setInverted(true);
+ m_hookL_encoder.setReverseDirection(true);
}
public void atPercentage(double percL, double percR)
@@ -28,4 +32,10 @@ public class Hooks extends SubsystemBase
m_hookR.set(percR);
}
+ @Override
+ public void periodic()
+ {
+ Logger.recordOutput(getName() + "/encoderL", m_hookL_encoder.getDistance());
+ Logger.recordOutput(getName() + "/encoderR", m_hookR_encoder.getDistance());
+ }
}
diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java
index 41620bb..036aae0 100644
--- a/src/main/java/frc/robot/subsystems/Intake.java
+++ b/src/main/java/frc/robot/subsystems/Intake.java
@@ -11,8 +11,8 @@ import frc.robot.Constants;
public class Intake extends SubsystemBase
{
- 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 CANSparkMax m_intakeT = new CANSparkMax(Constants.intakeT_ID, MotorType.kBrushless);
+ private final CANSparkMax m_intakeD = new CANSparkMax(Constants.intakeD_ID, MotorType.kBrushless);
private final RelativeEncoder m_intakeD_encoder = m_intakeD.getEncoder();
public Intake()
@@ -26,17 +26,19 @@ public class Intake extends SubsystemBase
atPercentage(perc, perc);
}
+ // perc is between 0.0 and 1.0
public void atPercentage(double upPerc, double downPerc)
{
m_intakeT.set(upPerc);
m_intakeD.set(downPerc);
}
+
public void shoot()
{
- if (m_intakeD_encoder.getVelocity() > 2000) {
- m_intakeT.set(1);
+ if (Math.abs(m_intakeD_encoder.getVelocity()) > 2000) {
+ m_intakeT.set(-0.5);
}
- m_intakeD.set(1);
+ m_intakeD.set(-0.5);
}
@Override
diff --git a/src/main/java/frc/robot/subsystems/MainMech.java b/src/main/java/frc/robot/subsystems/MainMech.java
new file mode 100644
index 0000000..1e31892
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/MainMech.java
@@ -0,0 +1,84 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants;
+import frc.robot.commands.IntakeIntermediate;
+import frc.robot.util.MechState;
+
+public class MainMech extends SubsystemBase
+{
+ public final Arm m_arm = new Arm();
+ public final Chain m_chain = new Chain();
+
+ public enum MechCmd {
+ INTAKE,
+ SPEAKER,
+ AMP,
+ DEFAULT,
+ MANUAL
+ }
+
+ private double deadzone(double low, double high, double val)
+ {
+ if(val > low && val < high) return 0.0;
+ return ((val > 0.0) ? val - high : val - low);
+ }
+ public MechCmd current_state = MechCmd.SPEAKER;
+
+ public Command m_manual_cmd = this.run(() -> {
+
+ });
+
+ public MechState states[] = {
+ new MechState(0.95, -40),
+ new MechState(0, -42),
+ new MechState(0.95, 54.5),
+ new MechState(0.5, -20),
+ new MechState(0.5, 20)
+ };
+
+ public MainMech() { }
+
+ public void setDefaultCommands(Command arm_command, Command chain_command)
+ {
+ m_arm.setDefaultCommand(arm_command);
+ m_chain.setDefaultCommand(chain_command);
+ }
+
+ public Command command(MechState state) {
+
+ return Commands.parallel(
+ m_arm.run(() -> m_arm.atAngleDegrees(state.m_arm_pos)),
+ m_chain.run(() -> m_chain.atPercentage(state.m_chain_pos)));
+ }
+
+ public Command toState(MechCmd state)
+ {
+ MechState next = states[state.ordinal()];
+ if (next.isValid()) {
+ Command next_cmd = command(next);
+ if(state == MechCmd.INTAKE || current_state == MechCmd.INTAKE) {
+ current_state = state;
+ return new IntakeIntermediate(m_arm, m_chain).andThen(next_cmd);
+ }
+ if (state == MechCmd.MANUAL) {
+ current_state = state;
+ double arm_pos = m_arm.pos() + deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2));
+ double chain_pos = Constants.controller2.getRawAxis(0) * (-0.5) + 0.5;
+ MechState manual_state = new MechState(chain_pos, arm_pos);
+ if (manual_state.isValid()) {
+ return command(manual_state);
+ }
+ return Commands.none();
+ }
+ current_state = state;
+ return next_cmd;
+ }
+ return Commands.none();
+ }
+
+ @Override
+ public void periodic() { }
+}
diff --git a/src/main/java/frc/robot/subsystems/Shooter.java b/src/main/java/frc/robot/subsystems/Shooter.java
index 677073c..273e5bd 100644
--- a/src/main/java/frc/robot/subsystems/Shooter.java
+++ b/src/main/java/frc/robot/subsystems/Shooter.java
@@ -15,6 +15,7 @@ public class Shooter extends SubsystemBase
public Shooter() {}
+ // perc is betwen 0.0 and 1.0
public void atPercentage(double perc)
{
atPercentage(perc, perc);
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 01:33:08 +0000