summaryrefslogtreecommitdiff
path: root/src/main
diff options
context:
space:
mode:
Diffstat (limited to 'src/main')
-rw-r--r--src/main/java/frc/robot/Constants.java51
-rw-r--r--src/main/java/frc/robot/Robot.java3
-rw-r--r--src/main/java/frc/robot/RobotContainer.java75
-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
-rw-r--r--src/main/java/frc/robot/util/IMUGyro.java2
10 files changed, 305 insertions, 70 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index c4c2329..efff2b8 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -3,7 +3,6 @@ package frc.robot;
import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
-import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.math.util.Units;
@@ -16,6 +15,8 @@ import frc.robot.util.IMUGyro;
public class Constants
{
public static final GenericHID controller = new GenericHID(0);
+ public static final GenericHID controller2 = new GenericHID(1);
+
public static final double InputSmoothing = 0.1;
public static final double DriveMaxSpeed = 5; // m/s
@@ -45,36 +46,50 @@ public class Constants
public static final Encoder encoderR = new Encoder(4, 5);
// for Intake
- public static final CANSparkMax intakeT = null;//new CANSparkMax(41, MotorType.kBrushless);
- public static final CANSparkMax intakeD = null;//new CANSparkMax(42, MotorType.kBrushless);
- // for Shooter
- public static final CANSparkMax shooterT = null;//new CANSparkMax(61, MotorType.kBrushless);
- public static final CANSparkMax shooterD = null;//new CANSparkMax(62, MotorType.kBrushless);
+ public static final int intakeT_ID = 32;
+ public static final int intakeD_ID = 31;
+
+ // for Shooter CommandScheduler.getInstance().run();
- // for Arm]
+ public static final int shooterT_ID = 21;
+ public static final int shooterD_ID = 22;
+
+ // for Arm
public static final double ArmGearReduction = 60;
- public static final ArmFeedforward arm_feedforward = new ArmFeedforward(0, 0, 0);
+ public static final double ArmEpsilon = 3 * Math.PI/180;
+
+ public static final double ArmkS = 0.4;
+ public static final double ArmkG = 0.495;
+ public static final double ArmkV = 0;
+
+ public static final double arm_initial_position = -43.0 * Math.PI / 180.0;
public static final TrapezoidProfile.Constraints arm_constraints =
- new TrapezoidProfile.Constraints(0, 0);
+ new TrapezoidProfile.Constraints(7, 5);
- public static final double ArmkP = 0.0;
+ public static final double ArmkP = 11.5;
public static final double ArmkI = 0.0;
- public static final double ArmkD = 0.0;
+ public static final double ArmkD = 0.4;
- public static final CANSparkMax armL = null;//new CANSparkMax(51, MotorType.kBrushless);
- public static final CANSparkMax armR = null;//new CANSparkMax(52, MotorType.kBrushless);
+ public static final int armL_ID = 41;
+ public static final int armR_ID = 42;
// for Chain
- public static final double ChainEncoderScale = 2600;
+ public static final double ChainEncoderScale = 3000;
+
+ public static final double ChainEpsilon = 0.05;
public static final TrapezoidProfile.Constraints chain_constraints =
- new TrapezoidProfile.Constraints(0.5, 0.5);
+ new TrapezoidProfile.Constraints(5, 5);
public static final double ChainkP = 45.0;
public static final double ChainkI = 0.0;
public static final double ChainkD = 0.0;
-
- public static final CANSparkMax chain = new CANSparkMax(50, MotorType.kBrushed);
- public static final Encoder chain_encoder = new Encoder(8, 9);
+
+ public static final int chain_ID = 50;
+ public static final int[] chain_encoder_PINS = {8, 9};
+
+ // Hooks
+ public static final int hookL_ID = 62;
+ public static final int hookR_ID = 61;
}
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0a5123a..2fc88fa 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -8,6 +8,7 @@ import org.littletonrobotics.junction.LoggedRobot;
import org.littletonrobotics.junction.Logger;
import org.littletonrobotics.junction.networktables.NT4Publisher;
+import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -21,6 +22,8 @@ public class Robot extends LoggedRobot {
Logger.addDataReceiver(new NT4Publisher());
Logger.start();
+ CameraServer.startAutomaticCapture();
+
m_robotContainer = new RobotContainer();
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 87c4e60..88e493c 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,26 +4,28 @@
package frc.robot;
-import java.util.List;
+import com.fasterxml.jackson.databind.deser.std.ContainerDeserializerBase;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.trajectory.Trajectory;
-import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
import edu.wpi.first.wpilibj2.command.button.Trigger;
-import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
-
+import frc.robot.subsystems.Arm;
+import frc.robot.subsystems.Chain;
import frc.robot.subsystems.Drivetrain;
+import frc.robot.subsystems.Hooks;
+import frc.robot.subsystems.Intake;
+import frc.robot.subsystems.Shooter;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
- // private final Chain m_chain = new Chain();
-
+ private final Chain m_chain = new Chain();
+ private final Arm m_arm = new Arm();
+ private final Intake m_intake = new Intake();
+ private final Shooter m_shooter = new Shooter();
+ private final Hooks m_hooks = new Hooks();
+
private final GenericHID m_controller = Constants.controller;
public RobotContainer()
@@ -31,7 +33,7 @@ public class RobotContainer
configureBindings();
}
- private double[] correctInputs(double in1, double in2)
+ private double[] correctInputs(double in1, double in2)
{
double p1 = processInput(in1);
double p2 = processInput(in2);
@@ -59,6 +61,19 @@ public class RobotContainer
private void configureBindings()
{
+ m_shooter.setDefaultCommand(m_shooter.run(() -> m_shooter.atPercentage(0.0)));
+ m_intake.setDefaultCommand(m_intake.run(() -> m_intake.atPercentage(0.0)));
+ m_arm.setDefaultCommand(m_arm.run(() -> m_arm.incrementAngleDegrees(
+ deadzone(-0.05, 0.05, Constants.controller2.getRawAxis(2)))));
+ m_hooks.setDefaultCommand(m_hooks.run(() -> m_hooks.atPercentage(
+ deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(4)),
+ deadzone(-0.1, 0.1, Constants.controller2.getRawAxis(5)))));
+ m_chain.setDefaultCommand(m_chain.run(() ->
+ m_chain.atPercentage(
+ Constants.controller2.getRawAxis(0)
+ * (-0.5) + 0.5
+ )));
+
m_drive.setDefaultCommand(m_drive.run(() -> {
double[] inputs = correctInputs(
m_controller.getRawAxis(2),
@@ -68,25 +83,39 @@ public class RobotContainer
inputs[1] * Constants.DriveMaxSpeed);
}));
- Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
- new Pose2d(0, 0, new Rotation2d(0)),
- // List.of(),
- List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
- new Pose2d(3, 0, new Rotation2d(0)),
- m_drive.trajectoyConfig);
- new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
+ // Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+ // new Pose2d(0, 0, new Rotation2d(0)),
+ // List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+ // new Pose2d(3, 0, new Rotation2d(0)), CameraServer camere = CameraServer.startAutomaticCapture();
- // m_chain.setDefaultCommand(m_chain.run(() ->
- // m_chain.atPercentage(
- // processInput(m_controller.getRawAxis(2))
- // )));
+ // m_drive.trajectoyConfig);
+ // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
- // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
+ new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(0.3)));
+ new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(-0.1)));
+ new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_shooter.run(() -> m_shooter.atPercentage(1)));
+ new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_intake.run(() -> m_intake.atPercentage(1)));
+
+ new Trigger(() -> Constants.controller2.getRawAxis(6) > 0).whileTrue(
+ Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(0)),
+ Commands.waitUntil(() -> m_arm.hasClearedAngle(-20)).andThen(m_chain.run(() -> m_chain.atPercentage(0.9)))
+ .until(() -> m_chain.isAtPosition(0.9)).andThen(Commands.run(() -> m_arm.atAngleDegrees((-40))))));//.whileTrue(
+ // Commands.parallel(m_arm.run(() -> m_arm.atAngleDegrees(-30)), Commands.waitUntil(() -> m_arm.hasClearedAngleDegree(-20))
+ // .andThen(() ->
+ // m_chain.run(() -> m_chain.atPercentage(0.9)).until(() ->
+ // m_chain.atPosition(0)).andThen(m_arm.run(() -> m_arm.atAngleDegrees(-40))))
+ // ));
+ // Commands.parallel(arm (chain at 0.waitUntil(arm > -30)).andThen());
// new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
// new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
// new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
+ // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kForward));
+ // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_arm.sysIdQausistatic(Direction.kReverse));
+ // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kForward));
+ // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_arm.sysIdDynamic(Direction.kReverse));
+
}
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)
{
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 580af8b..585fdbd 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -23,7 +23,7 @@ public class IMUGyro
private final int PWR_MGMT_1 = 0x6B;
private final int CONFIG_ADDR = 0x1A;
- private final int SMPLRT_DIV = 0x19;
+ // private final int SMPLRT_DIV = 0x19;
private final int GYRO_ADDR = 0x43;
private final int GYRO_CONFIG = 0x1B;
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 06:30:19 +0000