Workshop 10/13

src/main/java/frc/team3128/Constants.java

@@ -288,6 +288,11 @@ public static class VisionConstants {
                                                             new Translation2d(Units.inchesToMeters(5.43), Units.inchesToMeters(11.7)), 
                                                                 Rotation2d.fromDegrees(0)));
 
+        public static final Camera BACK_RIGHT = new Camera("BACK_RIGHT", true, 0, 0, 0, 
+                                                        new Transform2d(
+                                                            new Translation2d(Units.inchesToMeters(6.57), Units.inchesToMeters(-11.7)), 
+                                                                Rotation2d.fromDegrees(180)));
+
         public static final PIDController xController = new PIDController(1, 0, 0);
         public static final PIDController yController = new PIDController(1, 0, 0);
         public static final PIDController rController = new PIDController(1, 0, 0);
@@ -512,8 +517,8 @@ public static class ManipulatorConstants{
         public static final double STALL_POWER_CUBE = 0.1;
 
 
-        public static final double CURRENT_THRESHOLD_CONE = 25;
-        public static final double CURRENT_THRESHOLD_CUBE = 20;
+        public static final double CURRENT_THRESHOLD_CONE = 30;
+        public static final double CURRENT_THRESHOLD_CUBE = 25;
     }
 
     public static class ElevatorConstants {

src/main/java/frc/team3128/PositionConstants.java

@@ -6,14 +6,14 @@ public static enum Position {
         HIGH_CUBE(48.5, 20, false),
         MID_CONE(25.5, 28, true),
         MID_CUBE(25, 30, false),
-        LOW(3, 45, true),
+        LOW(3, 0, true),
 
         SHELF_CONE(53, -30, true),
         SHELF_CUBE(37, 22, false),
         CHUTE_CONE(10, 55, true),
         CHUTE_CUBE(10, 45, false),
 
-        GROUND_CONE(10, -18, true),
+        GROUND_CONE(8, -18, true),
         GROUND_CUBE(2.5, 0, false),
 
         NEUTRAL(3, 80, false);

src/main/java/frc/team3128/Robot.java

@@ -59,7 +59,7 @@ public void autonomousInit() {
     @Override
     public void autonomousPeriodic() {
         CommandScheduler.getInstance().run();
-        if (timer.hasElapsed(14.75) && Math.abs(Swerve.getInstance().getPitch()) > 6) {
+        if (timer.hasElapsed(14.75)) {
             new RunCommand(()->Swerve.getInstance().xlock(), Swerve.getInstance()).schedule();
         }
     }

src/main/java/frc/team3128/RobotContainer.java

@@ -95,8 +95,8 @@ private void configureButtonBindings() {
         controller.getButton("B").onTrue(new InstantCommand(()-> swerve.resetEncoders()));
         controller.getButton("Y").onTrue(runOnce(()-> swerve.throttle = 1)).onFalse(runOnce(()-> swerve.throttle = 0.8));
         controller.getButton("Start").onTrue(resetSwerve());
-        controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true));
-        controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true));
+        controller.getButton("RightBumper").onTrue(pickup(Position.GROUND_CUBE, true)).onFalse(retract(Position.NEUTRAL));
+        controller.getButton("LeftBumper").onTrue(pickup(Position.GROUND_CONE, true)).onFalse(retract(Position.NEUTRAL));
 
         controller.getUpPOVButton().onTrue(runOnce(()-> {
             CmdSwerveDrive.rSetpoint = DriverStation.getAlliance() == Alliance.Red ? 180 : 0;

src/main/java/frc/team3128/autonomous/AutoPrograms.java

@@ -4,8 +4,12 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import static edu.wpi.first.wpilibj2.command.Commands.*;
+import frc.team3128.PositionConstants.Position;
 import frc.team3128.common.narwhaldashboard.NarwhalDashboard;
 import frc.team3128.subsystems.Swerve;
+import static frc.team3128.commands.CmdManager.*;
 
 /**
  * Class to store information about autonomous routines.
@@ -50,10 +54,17 @@ public Command getAutonomousCommand() {
         String selectedAutoName = "b_cable_1Cone+2Cube+Climb"; //uncomment and change this for testing without opening Narwhal Dashboard
         SmartDashboard.putString(selectedAutoName, selectedAutoName);
         if (selectedAutoName == null) {
-            return null;
+            return score(Position.HIGH_CONE, true).beforeStarting(resetAuto());
         }
 
-        return Trajectories.get(selectedAutoName,selectedAutoName.contains("Climb"));
+        return Trajectories.get(selectedAutoName,selectedAutoName.contains("Climb")).beforeStarting(resetAuto());
+    }
+
+    public CommandBase resetAuto() {
+        return sequence(
+            resetAll(),
+            retract(Position.NEUTRAL)
+        );
     }
 
     // /** 

src/main/java/frc/team3128/commands/CmdManager.java

@@ -156,4 +156,11 @@ public static CommandBase resetWrist() {
     public static CommandBase resetSwerve() {
         return runOnce(()-> swerve.zeroGyro());
     }
+
+    public static CommandBase resetAll() {
+        return sequence(
+            resetWrist(),
+            resetElevator()
+        );
+    }
 }

src/main/java/frc/team3128/commands/CmdTrajectory.java

@@ -89,7 +89,7 @@ public void initialize() {
         trajCommand = generateAuto();
         trajCommand.schedule();
         CmdSwerveDrive.enabled = false;
-        endPoint = flip(END_POINTS[index]);
+        endPoint = allianceFlip(END_POINTS[index]);
     }
 
     @Override

src/main/java/frc/team3128/subsystems/Elevator.java

@@ -28,6 +28,7 @@ public Elevator() {
         configMotors();
         initShuffleboard(kS, kV, kG);
         m_controller.setTolerance(ELV_TOLERANCE);
+        resetEncoder();
     }
 
     public void startPID(Position position) {

src/main/java/frc/team3128/subsystems/Manipulator.java

@@ -31,7 +31,7 @@ public Manipulator(){
 
     private void configMotor(){
         m_roller = new NAR_TalonSRX(ROLLER_MOTOR_ID);
-        m_roller.setInverted(true);
+        m_roller.setInverted(false);
         m_roller.setNeutralMode(NeutralMode.Brake);
     }
 

src/main/java/frc/team3128/subsystems/Vision.java

@@ -37,6 +37,7 @@ public Vision() {
         cameras = new HashMap<String,NAR_Camera>();
         cameras.put(FRONT_LEFT.hostname, new NAR_Camera(FRONT_LEFT));
         cameras.put(FRONT_RIGHT.hostname, new NAR_Camera(FRONT_RIGHT));
+        cameras.put(BACK_RIGHT.hostname, new NAR_Camera(BACK_RIGHT));
     }
 
     public Pose2d targetPos(String name, Pose2d robotPos) {

src/main/java/frc/team3128/subsystems/Wrist.java

@@ -43,7 +43,7 @@ private void configMotor() {
         m_wrist.setInverted(false);
         m_wrist.setIdleMode(IdleMode.kCoast);
         m_wrist.setSmartCurrentLimit(40);
-        // resetEncoder();
+        resetEncoder();
     }
 
     @Override
@@ -57,7 +57,7 @@ public double getMeasurement() {
     }
 
     public void resetEncoder() {
-        m_wrist.setSelectedSensorPosition(0);
+        m_wrist.setSelectedSensorPosition(90 * GEAR_RATIO / ROTATION_TO_DEGREES);
     }
 
     public void set(double power) {