From 4452529be059c5fbe4aaf9190f25cfc734715f01 Mon Sep 17 00:00:00 2001 From: kartofen Date: Thu, 7 Mar 2024 06:05:41 +0200 Subject: orks --- src/main/java/frc/robot/Constants.java | 51 ++++--- src/main/java/frc/robot/Robot.java | 3 + src/main/java/frc/robot/RobotContainer.java | 75 +++++++--- src/main/java/frc/robot/subsystems/Arm.java | 164 +++++++++++++++++++-- src/main/java/frc/robot/subsystems/Chain.java | 22 ++- src/main/java/frc/robot/subsystems/Drivetrain.java | 1 - src/main/java/frc/robot/subsystems/Hooks.java | 25 +++- src/main/java/frc/robot/subsystems/Intake.java | 22 ++- src/main/java/frc/robot/subsystems/Shooter.java | 10 +- src/main/java/frc/robot/util/IMUGyro.java | 2 +- 10 files changed, 305 insertions(+), 70 deletions(-) (limited to 'src') 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 m_voltage = mutable(Volts.of(0)); + private final MutableMeasure m_angle = mutable(Radians.of(0)); + private final MutableMeasure> 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 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; -- cgit v1.2.3