Arm in Elevator's Image. lisarm al gaib

src/main/java/frc/robot/OperatorInput.java

@@ -42,16 +42,16 @@ public class OperatorInput {
   final Trigger manualElevator = operator.axisMagnitudeGreaterThan(XboxController.Axis.kLeftY.value, 0.1);
   final Trigger resetElevatorEncoder = operator.start();
 
-  //final Trigger manualArmCommand = operator.axisMagnitudeGreaterThan(XboxController.Axis.kRightY.value, 0.1);
-  final Trigger manualPivotCommand = operator.axisMagnitudeGreaterThan(XboxController.Axis.kRightY.value, 0.1);
-  final Trigger openClaw = driver.leftStick();
-  final Trigger closeClaw = driver.rightStick();
+  final Trigger manualArm = operator.axisMagnitudeGreaterThan(XboxController.Axis.kRightY.value, 0.1);
+  final Trigger manualPivot = operator.axisMagnitudeGreaterThan(XboxController.Axis.kRightY.value, 0.1);
+  // final Trigger openClaw = driver.leftStick();
+  // final Trigger closeClaw = driver.rightStick();
 
   final Trigger rollersIn = operator.povRight();
   final Trigger rollersOut = operator.povLeft();
 
-  // final Trigger armbendup = operator.povUp();
-  // final Trigger armbenddown = operator.povDown();
+  final Trigger armSetpointUp = operator.povUp();
+  final Trigger armSetpointDown = operator.povDown();
 
 
 

src/main/java/frc/robot/RobotContainer.java

@@ -30,17 +30,18 @@
 //import frc.robot.commands.ResetEncoderCommand;
 
 import frc.robot.commands.ArmCommands.ArmHoverCommand;
+import frc.robot.commands.ArmCommands.BendCommand;
 import frc.robot.commands.BaseDriveCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorGoToOriginCommand;
 import frc.robot.commands.ElevatorCommands.ElevatorSetPointCommand;
 import frc.robot.commands.ElevatorCommands.ManualElevatorCommand;
 import frc.robot.commands.ElevatorCommands.ResetElevatorEncoderCommand;
 import frc.robot.commands.IntakeCommands.IntakeSetPivotCommand;
 import frc.robot.commands.IntakeCommands.IntakeSetRollersCommand;
+import frc.robot.constants.ArmConstants;
 import frc.robot.constants.Constants;
 import frc.robot.constants.ElevatorConstants;
 import frc.robot.constants.DriveConstants;
-import frc.robot.constants.IntakeConstants;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.IntakeSubsystem.IntakeIO;
 import frc.robot.subsystems.IntakeSubsystem.IntakeSubsystem;
@@ -71,22 +72,18 @@
 public class RobotContainer {
   // Subsystems
   public final DriveSubsystem drive;
-
-  // private final Flywheel flywheel;
   public final OperatorInput operatorInput;
   private final ElevatorSubsystem elevatorSubsystem;
   private final ClawSubsystem clawSubsystem;
-  private final IntakeSubsystem groundIntakeSubsystem;
+  private final IntakeSubsystem intakeSubsystem;
   private final LEDSubsystem ledSubsystem;
   private final ArmSubsystem armSubsystem;
   private final VisionSubsystem visionSubsystem;
   public static SwerveDriveSimulation driveSimulation = null;
   private final AutoSubsystem autoSubsystem;
   private AutoChooser autoChooser;
 
-
-
-  // Controller
+  // Controllers
   private final CommandXboxController driveController = new CommandXboxController(0);
   private final CommandXboxController operatorController = new CommandXboxController(1);
 
@@ -115,18 +112,17 @@ public RobotContainer() {
                         new ModuleIOHybrid(1, TunerConstants.FrontRight),
                         new ModuleIOHybrid(2, TunerConstants.BackLeft),
                         new ModuleIOHybrid(3, TunerConstants.BackRight));
-
         elevatorSubsystem =
                 new ElevatorSubsystem(new ElevatorIOSparkMax(),
-                        new ElevatorEncoderIOThroughbore()); //TODO
+                        new ElevatorEncoderIOThroughbore());
         clawSubsystem =
                 new ClawSubsystem(new EndEffectorIOSparkMax());
-        groundIntakeSubsystem =
-                new IntakeSubsystem(new IntakeIOSparkMax()); //TODO replace placeholder
+        intakeSubsystem =
+                new IntakeSubsystem(new IntakeIOSparkMax());
         ledSubsystem =
                 new LEDSubsystem(); //TODO
         armSubsystem =
-                new ArmSubsystem(new ArmIOTalonFX(), new ArmEncoderIO() {}); //TODO
+                new ArmSubsystem(new ArmIOTalonFX(), new ArmEncoderIOThroughbore());
         visionSubsystem =
                 new VisionSubsystem(new VisionIOPhotonVision()); //TODO
         autoSubsystem = new AutoSubsystem(clawSubsystem, drive);
@@ -152,7 +148,7 @@ public RobotContainer() {
 
         clawSubsystem =
                 new ClawSubsystem(new EndEffectorIOSim());
-        groundIntakeSubsystem =
+        intakeSubsystem =
                 new IntakeSubsystem(new IntakeIOSim(driveSimulation)); //TODO
         ledSubsystem =
                 new LEDSubsystem(); //TODO
@@ -183,7 +179,7 @@ public RobotContainer() {
                 new ElevatorSubsystem(new ElevatorIO() {}, new ElevatorEncoderIO() {}); //TODO
         clawSubsystem =
                 new ClawSubsystem(new EndEffectorIO() {});
-        groundIntakeSubsystem =
+        intakeSubsystem =
                 new IntakeSubsystem(new IntakeIO() {});
         ledSubsystem =
                 new LEDSubsystem(); //TODO
@@ -198,9 +194,6 @@ public RobotContainer() {
         break;
     }
 
-//    // Set up auto routines
-   //TODO FIX CLAW TO END EFFECTOR
-
     loadCommands();
   }
 
@@ -212,34 +205,46 @@ public RobotContainer() {
    */
   void loadCommands() {
 
-    armSubsystem.setDefaultCommand(new ArmHoverCommand(armSubsystem));
+
+    // elevator stuff
     operatorInput.elevatorSetpoint1.whileTrue(new ElevatorSetPointCommand(elevatorSubsystem, ElevatorConstants.L1));
     operatorInput.elevatorSetpoint2.whileTrue(new ElevatorSetPointCommand(elevatorSubsystem, ElevatorConstants.L3));
     operatorInput.elevatorSetpoint3.whileTrue(new ElevatorSetPointCommand(elevatorSubsystem, ElevatorConstants.L4));
-
+    operatorInput.resetElevatorEncoder.onTrue(new ResetElevatorEncoderCommand(elevatorSubsystem));
     operatorInput.elevatorGoToOrigin.onTrue(new ElevatorGoToOriginCommand(elevatorSubsystem));
 
-    // operatorInput.armbendup.whileTrue(new BendCommand(singleJointedArmSubsystem, Math.PI/2));
-    // operatorInput.armbenddown.whileTrue(new BendCommand(singleJointedArmSubsystem, -Math.PI/2));
+    //arm stuff
+    armSubsystem.setDefaultCommand(new ArmHoverCommand(armSubsystem));
+    operatorInput.armSetpointUp.whileTrue(new BendCommand(armSubsystem, ArmConstants.setpointUp));
+    operatorInput.armSetpointDown.whileTrue(new BendCommand(armSubsystem, ArmConstants.setpointDown));
 
-    // operatorInput.manualArmCommand.whileTrue(new ManualArmCommand(singleJointedArmSubsystem, elevatorAndArmController::getRightY));
 
-    operatorInput.resetElevatorEncoder.onTrue(new ResetElevatorEncoderCommand(elevatorSubsystem));
-    operatorInput.manualPivotCommand.whileTrue(new IntakeSetPivotCommand(groundIntakeSubsystem, operatorController::getRightY));
+    // manual commands
+
+    // uncomment and comment which ever one is needed
+    // operatorInput.manualArmCommand.whileTrue(new ManualArmCommand(singleJointedArmSubsystem, elevatorAndArmController::getRightY));
+    operatorInput.manualPivot.whileTrue(new IntakeSetPivotCommand(intakeSubsystem, operatorController::getRightY));
     operatorInput.manualElevator.whileTrue(new ManualElevatorCommand(elevatorSubsystem, operatorController::getLeftY));
-    operatorInput.rollersIn.whileTrue(new IntakeSetRollersCommand(groundIntakeSubsystem, false));
-    operatorInput.rollersOut.whileTrue(new IntakeSetRollersCommand(groundIntakeSubsystem, true));
+
+    // rollers and pivot, algae intake stuff
+    operatorInput.rollersIn.whileTrue(new IntakeSetRollersCommand(intakeSubsystem, false));
+    operatorInput.rollersOut.whileTrue(new IntakeSetRollersCommand(intakeSubsystem, true));
+
+    // claw stuff
     // operatorInput.openClaw.onTrue(new OpenClawCommand(clawSubsystem));
     // operatorInput.closeClaw.onTrue(new CloseClawCommand(clawSubsystem));
 
+
+    // base drive command
     operatorInput.movementDesired.whileTrue(
-            new BaseDriveCommand.basedrivecommand(
+            new BaseDriveCommand(
                 drive,
                 () -> -driveController.getLeftY() * -0.5,
                 () -> -driveController.getLeftX() * -0.5,
                 () -> -driveController.getRightX()));
   }
 
+
   /**
    * Use this to pass the autonomous command to the main {@link Robot} class.
    *

src/main/java/frc/robot/commands/ArmCommands/ManualArmCommand.java

@@ -30,7 +30,7 @@ public void execute() {
     @Override
     public void end(boolean interrupted) {
         if (interrupted) {
-            double calculatedVolts = ArmConstants.kG;
+            double calculatedVolts = ArmConstants.kG * Math.cos(armSubsystem.getCurrentAngle());
             Logger.recordOutput("ArmSubsystem/target_voltage", calculatedVolts);
             this.armSubsystem.setArmVoltage(calculatedVolts);
         }

src/main/java/frc/robot/commands/BaseDriveCommand.java

@@ -12,14 +12,13 @@
 
 import java.util.function.DoubleSupplier;
 
-public class BaseDriveCommand {
-    public static class basedrivecommand extends Command {
+public class BaseDriveCommand extends Command {
         private final DriveSubsystem drive;
         private final DoubleSupplier xSupplier;
         private final DoubleSupplier ySupplier;
         private final DoubleSupplier omegaSupplier;
 
-        public basedrivecommand(DriveSubsystem drive,
+        public BaseDriveCommand(DriveSubsystem drive,
                                 DoubleSupplier xSupplier,
                                 DoubleSupplier ySupplier,
                                 DoubleSupplier omegaSupplier) {
@@ -68,5 +67,4 @@ public void execute() {
         public void end(boolean interrupted) {
             drive.stop();
         }
-    }
 }

src/main/java/frc/robot/constants/ArmConstants.java

@@ -1,18 +1,17 @@
 package frc.robot.constants;
+import com.ctre.phoenix6.signals.InvertedValue;
 import edu.wpi.first.math.system.plant.DCMotor;
 
 public class ArmConstants {
 
-    public static final int frontLeftDriveCanId = 1;
-    public static final int MotorNumber = 0;
-    public static final int EncoderNumber = 1;
     public static final double kPSim = 3;
     public static final double kDSim = 2;
     public static final double kISim = 0;
     public static final double kVSim= 0.39;
     public static final double kGSim = 1.696;
     public static final double kSSim = 0;
     public static double kASim = 0;
+
     public static double kS = 0;
     public static double kV = 1;
     public static double kG = 0;
@@ -21,27 +20,33 @@ public class ArmConstants {
     public static double kI = 0;
     public static double kD = 0;
     public static final double kArmToleranceRPS = 0;
-    public static final double kEncoderDistancePerPulse = 0;
-    public static final double MAX_ANGLE = Math.PI;
-    public static final double MIN_ANGLE = -Math.PI;
-    public static double SparkkP = 1.0;
-    public static double SparkkD = 1.0;
+    public static final double MAX_ANGLE = Math.PI / 2;
+    public static final double MIN_ANGLE = -Math.PI / 2;
     public static double gearingRatio = 180.0;
     public static double armLength = 0.3;
-    public static DCMotor armGearbox = DCMotor.getNEO(1);
-    public static int armSparkMotorCurrentLimit = 1;
+    public static DCMotor armGearbox = DCMotor.getKrakenX60(1);
+
     public static double armSparkEncoderPositionFactor;
     public static double armSparkEncoderVelocityFactor;
     public static double maxVelocity = 3;
     public static double maxAcceleration = 3;
     public static double mass = 5;
 
-    public static int arm1CurrentLimit = 0;
-    public static int arm2CurrentLimit = 0;
+    public static int arm1CurrentLimit = 20;
+    public static int arm2CurrentLimit = 20;
 
     public static int arm1TalonID;
     public static int arm2TalonID;
 
-    public static int fullRange = 4;
-    public static int expectedZero = 2;
+    // 0 as a value should be parallel to the floor, think unit circle
+    public static final int encoderChannel = 1;
+
+    // offset in rads, this will be subtracted from the offset reading
+    // so if encoder is 1.9, offset is 1.9 and not -1.9
+    public static final double encoderOffset = 0;
+    public static boolean encoderInverted;
+    public static double setpointUp = Math.PI / 4;
+    public static double setpointDown =  -Math.PI / 4;
+    public static InvertedValue arm1Inverted = InvertedValue.Clockwise_Positive;
+    public static InvertedValue arm2Inverted = InvertedValue.CounterClockwise_Positive;
 }

src/main/java/frc/robot/subsystems/ArmSubsystem/ArmEncoderIO.java

@@ -7,12 +7,10 @@ public interface ArmEncoderIO {
     class ArmEncoderIOInputs {
         double unfilteredArmAngle = 0.0;
         double armAngle = 0.0;
-        double encoderPositionRads = 0;
-        double encoderPositionRots = 0;
-        double encoderVelocityRads = 0;
-        double encoderVelocityRots = 0;
+        double encoderPositionRadsOffset = 0;
+        double encoderPositionRadsNoOffset = 0;
+        double encoderPositionRotsNoOffset = 0;
     }
     public default void updateInputs(ArmEncoderIO.ArmEncoderIOInputs inputs) {}
-    public default void resetEncoderPositionWithArmAngle() {}
 
 }

src/main/java/frc/robot/subsystems/ArmSubsystem/ArmEncoderIOThroughbore.java

@@ -9,22 +9,19 @@ public class ArmEncoderIOThroughbore implements ArmEncoderIO{
     private final DutyCycleEncoder armDCEncoder;
     LinearFilter armFilter = LinearFilter.singlePoleIIR(0.1, 0.02);
 
-    public ArmEncoderIOThroughbore () {
-        armDCEncoder = new DutyCycleEncoder(0, ArmConstants.fullRange, ArmConstants.expectedZero);
+    public ArmEncoderIOThroughbore() {
+        armDCEncoder = new DutyCycleEncoder(ArmConstants.encoderChannel);
+        armDCEncoder.setInverted(ArmConstants.encoderInverted);
     }
 
     @Override
     public void updateInputs(ArmEncoderIO.ArmEncoderIOInputs inputs) {
-        inputs.encoderPositionRots = armDCEncoder.get();
-        inputs.encoderPositionRads =  Units.rotationsToRadians(inputs.encoderPositionRots);
-        inputs.encoderVelocityRots = armDCEncoder.get() * armDCEncoder.getFrequency();
-        inputs.encoderVelocityRads = Units.rotationsToRadians(inputs.encoderVelocityRads);
+        inputs.encoderPositionRotsNoOffset = armDCEncoder.get();
+        inputs.encoderPositionRadsNoOffset =  Units.rotationsToRadians(inputs.encoderPositionRotsNoOffset);
+        inputs.encoderPositionRadsOffset =  Units.rotationsToRadians(inputs.encoderPositionRotsNoOffset) - ArmConstants.encoderOffset;
 
-        inputs.unfilteredArmAngle = inputs.encoderPositionRads / ArmConstants.gearingRatio;
+        inputs.unfilteredArmAngle = inputs.encoderPositionRadsOffset / ArmConstants.gearingRatio;
         inputs.armAngle = armFilter.calculate(inputs.unfilteredArmAngle);
     }
-    @Override
-    public void resetEncoderPositionWithArmAngle() {
-        armDCEncoder.close();
-    }
+
 }

src/main/java/frc/robot/subsystems/ArmSubsystem/ArmIO.java

@@ -24,10 +24,6 @@ class ArmIOInputs {
 
         public double arm1CurrentAmps;
         public double arm2CurrentAmps;
-        public double[] odometryTimestamps;
-        public double[] odometryArmPositionsRad;
-        public double[] odometryArm1PositionsRad;
-        public double[] odometryArm2PositionsRad;
 
         public double encoderPosition;
         public double armVelocity;