From 6216815f51211b3f08d9bfac0e6d0b676c71e973 Mon Sep 17 00:00:00 2001 From: cuttestkittensrule Date: Wed, 24 Jan 2024 19:49:45 -0800 Subject: [PATCH] add motors to shooter subsystem --- .../lib2813/control/motors/TalonFXWrapper.java | 2 -- .../team2813/lib2813/subsystems/MotorSubsystem.java | 4 ++-- Robot2024/src/main/java/com/team2813/Constants.java | 3 ++- .../main/java/com/team2813/subsystems/Shooter.java | 11 ++++++++++- 4 files changed, 14 insertions(+), 6 deletions(-) diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java index d0bc6f6..c2afd37 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/control/motors/TalonFXWrapper.java @@ -127,8 +127,6 @@ public void set(ControlMode controlMode, double demand, double feedForward) { case MOTION_MAGIC: demand = Units2813.motorRevsToTicks(demand, 2048); break; - case DUTY_CYCLE: - break; default: break; } diff --git a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java index 413e880..953e6b8 100644 --- a/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java +++ b/Robot2024/lib/src/main/java/com/team2813/lib2813/subsystems/MotorSubsystem.java @@ -40,8 +40,8 @@ public static class MotorSubsystemConfiguration { * The default starting position if one is not defined */ public static final double DEFAULT_STARTING_POSITION = 0.0; - private Motor motor; - private Encoder encoder; + protected Motor motor; + protected Encoder encoder; private PIDController controller; private double acceptableError; private double startingPosition; diff --git a/Robot2024/src/main/java/com/team2813/Constants.java b/Robot2024/src/main/java/com/team2813/Constants.java index 1785760..3bde732 100644 --- a/Robot2024/src/main/java/com/team2813/Constants.java +++ b/Robot2024/src/main/java/com/team2813/Constants.java @@ -51,7 +51,8 @@ public static class DriverConstants { //Mechanism CAN IDs public static final int INTAKE = 14; public static final int KICKER = 15; - public static final int SHOOTER = 16; + public static final int SHOOTER_1 = 16; + public static final int SHOOTER_2 = -1; public static final int SHOOTER_PIVOT= 17; public static final int SHOOTER_ENCODER = 18; public static final int CLIMBER = 19; diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java index 7a03198..1678fc8 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -1,15 +1,24 @@ package com.team2813.subsystems; +import com.team2813.lib2813.control.Motor; import com.team2813.lib2813.control.motors.TalonFXWrapper; import com.team2813.lib2813.subsystems.MotorSubsystem; import static com.team2813.Constants.*; +import com.ctre.phoenix.motorcontrol.FollowerType; import com.ctre.phoenix.motorcontrol.TalonFXInvertType; public class Shooter extends MotorSubsystem { + Motor shooterMotor; public Shooter() { // TODO: fix invert type - super(new MotorSubsystemConfiguration(new TalonFXWrapper(0, TalonFXInvertType.Clockwise))); + super(new MotorSubsystemConfiguration( + new TalonFXWrapper(SHOOTER_PIVOT, TalonFXInvertType.Clockwise), + null //TODO: write encoder class (embarassing wow) + )); + TalonFXWrapper m = new TalonFXWrapper(SHOOTER_1, TalonFXInvertType.Clockwise); + m.addFollower(SHOOTER_2, TalonFXInvertType.OpposeMaster); + shooterMotor = m; } public static enum Angle implements MotorSubsystem.Position { TEST(0.0);