robot testing

.roboRIOTeamNumberSetter/roborioteamnumbersetter.json

@@ -0,0 +1 @@
+{}

src/main/java/frc/robot/Commands/SpeakerShot.java

@@ -8,6 +8,6 @@ import frc.robot.subsystems.Shooter;
 public class SpeakerShot extends ParallelCommandGroup{
     
     SpeakerShot(Indexer indexer, Shooter shooter){
-        addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
+        //addCommands(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0, 0), indexer.shootNote(() -> 1.0));
     }
 }

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

@@ -61,6 +61,8 @@ public class RobotContainer {
     // TODO There's more than one speaker tag, how do we want to handle this?
     private int speakerTag;
 
+    private boolean setAmpAngle;
+
     public RobotContainer() {
         /*try {
          
@@ -83,9 +85,9 @@ public class RobotContainer {
 
         indexer = new Indexer();
 
-        shooter = new Shooter(indexer.getBeamBreak());
+        shooter = new Shooter(indexer::getBeamBreak);
 
-        climber = new Climber(shooter.getShooterAngle());
+        climber = new Climber(shooter::getShooterAngle);
 
         
 
@@ -113,6 +115,8 @@ public class RobotContainer {
             speakerTag = 8;
         }
 
+        setAmpAngle = false;
+
         configureBindings();
         shuffleboardSetup();
     }
@@ -130,20 +134,47 @@ public class RobotContainer {
             drivetrain.teleopCommand(
                 driver::getLeftY, 
                 driver::getLeftX, 
-                driver::getRightX, 
+                () -> { return -driver.getRightX(); }, 
                 OIConstants.kTeleopDriveDeadband
             )
         );
 
         //intake.setDefaultCommand(intake.intakeUpCommand());
-        intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, driver::getLeftTriggerAxis));
+        intake.setDefaultCommand(intake.manualPivot(operator::getLeftY, () -> 0.0));
 
-       // shooter.setDefaultCommand(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterLoadAngle, 0.0, 0.0));
+        shooter.setDefaultCommand(
-        shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
+            shooter.angleSpeedsSetpoints(
+                () -> {
+                    if (setAmpAngle) {
+                        return ShooterConstants.kShooterAmpAngle;
+                    } else {
+                        return ShooterConstants.kShooterLoadAngle;
+                    }
+                },
+                () -> {
+                    if (driver.getLeftTriggerAxis() > .25) {
+                        return -1.0;
+                    }else if(driver.getRightTriggerAxis() > 0.25){
+                        return 1.0;
+                    } else {
+                        return 0;
+                    }
+                }
+            )
+        );
+        //shooter.setDefaultCommand(shooter.manualPivot(operator::getRightY, operator::getLeftTriggerAxis));
 
         climber.setDefaultCommand(climber.stop());
 
-        indexer.setDefaultCommand(indexer.shootNote(operator::getRightTriggerAxis));
+        indexer.setDefaultCommand(indexer.shootNote(
+            () -> {
+                if (driver.getLeftTriggerAxis() > .25) {
+                        return -1.0;
+                    }else {
+                        return 0.0;
+                    }
+            }
+        ));
 
         driver.povCenter().onFalse(
             drivetrain.driveCardinal(
@@ -208,9 +239,18 @@ public class RobotContainer {
         */
         driver.start().onTrue(drivetrain.toggleFieldRelativeControl());
 
-        driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
+        
 
-        operator.y().onTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
+        //driver.leftBumper().toggleOnTrue(intake.intakeDownCommand());
+
+        operator.a().onTrue(new InstantCommand(() -> { setAmpAngle = true; }));
+        operator.a().onFalse(new InstantCommand(() -> { setAmpAngle = false; }));
+        
+        //operator.a().whileTrue(shooter.angleSpeedsSetpoints(ShooterConstants.kShooterAmpAngle, 0, 0));
+
+        operator.y().whileTrue(climber.setSpeedWithSupplier(operator::getRightTriggerAxis));
+
+        driver.a().whileTrue(indexer.shootNote(() -> 1.0));
         
   }
 
@@ -229,10 +269,10 @@ public class RobotContainer {
         .withPosition(0, 0)
         .withSize(2, 1)
         .withWidget(BuiltInWidgets.kBooleanBox);
-    teleopTab.addBoolean("indexer beam break", indexer.getBeamBreak());
+    teleopTab.addBoolean("indexer beam break", indexer::getBeamBreak);
-    teleopTab.addBoolean("shooter beam break", shooter.getBeamBreak());
+    teleopTab.addBoolean("shooter beam break", shooter::getBeamBreak);
-    teleopTab.addDouble("shooter angle", shooter.getShooterAngle());
+    teleopTab.addDouble("shooter angle", shooter::getShooterAngle);
-    teleopTab.addDouble("intake angle", intake.getIntakeAngle());
+    teleopTab.addDouble("intake angle", intake::getIntakeAngle);
   }
 
   public Command getAutonomousCommand() {

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

@@ -47,5 +47,5 @@ public final class DrivetrainConstants {
 
     public static final boolean kGyroReversed = true;
 
-    public static final double kRobotStartOffset = 90;
+    public static final double kRobotStartOffset = 0.0;
 }

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

@@ -12,7 +12,7 @@ public class ShooterConstants {
 
     public static final double kShooterFF = 0.0;
 
-    public static final double kShooterPivotP = 0.0;
+    public static final double kShooterPivotP = 3.0;
     public static final double kShooterPivotI = 0.0;
     public static final double kShooterPivotD = 0.0;
 

src/main/java/frc/robot/subsystems/Climber.java

@@ -16,12 +16,14 @@ public class Climber extends SubsystemBase {
     private DoubleSupplier shooterAngle;
 
     public Climber(DoubleSupplier shooterAngle) {
-        if(shooterAngle.getAsDouble() > Math.toRadians(-10.0)){
+        motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
-            motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
+        motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
-            motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
+        
 
-            motor2.follow(motor1);
+        motor2.follow(motor1);
-        }
+        motor1.setInverted(true);
+        motor2.setInverted(true);
+        this.shooterAngle = shooterAngle;
     }
 
     public Command setSpeedWithSupplier(DoubleSupplier speed) {

src/main/java/frc/robot/subsystems/Indexer.java

@@ -55,7 +55,7 @@ public class Indexer extends SubsystemBase{
         });
     }
 
-    public BooleanSupplier getBeamBreak(){
+    public boolean getBeamBreak(){
-        return indexerBeamBreak::get;
+        return indexerBeamBreak.get();
     }
 }

src/main/java/frc/robot/subsystems/Intake.java

@@ -27,7 +27,7 @@ public class Intake extends SubsystemBase{
         intakeRoller = new CANSparkMax(IntakeConstants.kIntakeRollerID, MotorType.kBrushless);
         
         intakeRoller.setSmartCurrentLimit(60);
-        intakeRoller.setIdleMode(IdleMode.kCoast);
+        intakeRoller.setIdleMode(IdleMode.kBrake);
         intakeRoller.burnFlash();
 
         intakePivot = new CANSparkMax(IntakeConstants.kIntakePivotID, MotorType.kBrushless);
@@ -109,7 +109,7 @@ public class Intake extends SubsystemBase{
         });
     }
 
-    public DoubleSupplier getIntakeAngle(){
+    public double getIntakeAngle(){
-        return intakeEncoder::getPosition;
+        return intakeEncoder.getPosition();
     }
 }

src/main/java/frc/robot/subsystems/Shooter.java

@@ -28,9 +28,6 @@ public class Shooter extends SubsystemBase{
 
     private Encoder pivotEncoder;
 
-    private SparkPIDController topPID;
-    private SparkPIDController bottomPID;
-
     private PIDController shooterPivotPID;
     private ArmFeedforward shooterPivotFF;
 
@@ -41,15 +38,19 @@ public class Shooter extends SubsystemBase{
     public Shooter(BooleanSupplier indexerBeamBreak){
         topShooter = new CANSparkMax(ShooterConstants.kTopShooterID, MotorType.kBrushless);
         bottomShooter = new CANSparkMax(ShooterConstants.kBottomShooterID, MotorType.kBrushless);
+
         shooterPivot = new CANSparkMax(ShooterConstants.kShooterPivotID, MotorType.kBrushless);
-        
+        shooterPivot.setInverted(true);
+
         topEncoder = topShooter.getEncoder();
         bottomEncoder = bottomShooter.getEncoder();
 
+        /* 
         topPID = topShooter.getPIDController();
         topPID.setFeedbackDevice(topEncoder);
         bottomPID = bottomShooter.getPIDController();
         bottomPID.setFeedbackDevice(bottomEncoder);
+        */
 
         shooterPivot.setSmartCurrentLimit(50);
         topShooter.setSmartCurrentLimit(40);
@@ -60,46 +61,52 @@ public class Shooter extends SubsystemBase{
 
         shooterBeamBreak = new DigitalInput(ShooterConstants.kShooterBeamBreakChannel);
 
-        topShooter.setIdleMode(IdleMode.kBrake);
+        topShooter.setIdleMode(IdleMode.kCoast);
         bottomShooter.setIdleMode(IdleMode.kCoast);
 
         bottomShooter.burnFlash();
         shooterPivot.burnFlash();
         topShooter.burnFlash();
 
-        topPID.setP(ShooterConstants.kShooterP);
-        topPID.setI(ShooterConstants.kShooterI);
-        topPID.setD(ShooterConstants.kShooterD);
-        topPID.setFF(ShooterConstants.kShooterFF);
-
-        bottomPID.setP(ShooterConstants.kShooterP);
-        bottomPID.setI(ShooterConstants.kShooterI);
-        bottomPID.setI(ShooterConstants.kShooterD);
-        bottomPID.setFF(ShooterConstants.kShooterFF);
-
         shooterPivotPID = new PIDController(
             ShooterConstants.kShooterPivotP,
             ShooterConstants.kShooterPivotI,
             ShooterConstants.kShooterPivotD
         );
+        shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF, ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
 
     }
 
+    /*
     public Command angleSpeedsSetpoints(double setpointAngle, double topRPM, double bottomRPM){
         return run(()-> {
             angleAndSpeedControl(setpointAngle, topRPM, bottomRPM);
         });
+    }*/
+
+    public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, DoubleSupplier speed){
+        return run(()-> {
+            angleAndSpeedControl(setpointAngle.getAsDouble(), speed.getAsDouble(), speed.getAsDouble());
+        });
+    }
+
+    public Command angleSpeedsSetpoints(DoubleSupplier setpointAngle, double topRPM, double bottomRPM){
+        return run(()-> {
+            angleAndSpeedControl(setpointAngle.getAsDouble(), topRPM, bottomRPM);
+        });
     }
 
     private void angleAndSpeedControl(double setpointAngle, double topRPM, double bottomRPM){
         shooterPivot.setIdleMode(IdleMode.kBrake);
 
             shooterPivot.setVoltage(
-                shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + 
+                shooterPivotPID.calculate(getShooterAngle(), setpointAngle) + 
                 shooterPivotFF.calculate(setpointAngle, 0.0));
 
-            topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
+            //topPID.setReference(topRPM, CANSparkMax.ControlType.kVelocity);
-            bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
+            //bottomPID.setReference(bottomRPM, CANSparkMax.ControlType.kVelocity);
+            topShooter.set(topRPM);
+            bottomShooter.set(bottomRPM);
     }
 
     public Command ampHandoff(){
@@ -107,11 +114,11 @@ public class Shooter extends SubsystemBase{
             shooterPivot.setIdleMode(IdleMode.kBrake);
 
             shooterPivot.setVoltage(
-                shooterPivotPID.calculate(pivotEncoder.getDistance(), ShooterConstants.kShooterLoadAngle) + 
+                shooterPivotPID.calculate(getShooterAngle(), ShooterConstants.kShooterLoadAngle) + 
                 shooterPivotFF.calculate(ShooterConstants.kShooterLoadAngle, 0.0));
             
             if(shooterBeamBreak.get() || indexerBeamBreak){
-                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 1000.0, 1000.0);
+                angleAndSpeedControl(ShooterConstants.kShooterLoadAngle, 0.5, 0.5);
             }else{
                 angleAndSpeedControl(ShooterConstants.kShooterAmpAngle, 0, 0);
             }
@@ -125,12 +132,12 @@ public class Shooter extends SubsystemBase{
         });
     }
 
-    public DoubleSupplier getShooterAngle(){
+    public double getShooterAngle(){
-        return () -> {return pivotEncoder.getDistance();};
+        return pivotEncoder.getDistance() + ShooterConstants.kShooterLoadAngle;
     }
 
-    public BooleanSupplier getBeamBreak(){
+    public Boolean getBeamBreak(){
-        return shooterBeamBreak::get;
+        return shooterBeamBreak.get();
     }
 
     public Command manualPivot(DoubleSupplier pivotPower, DoubleSupplier spinny){
@@ -141,4 +148,5 @@ public class Shooter extends SubsystemBase{
         });
     }
 
+
 }