fixed amp shooting (#45)

* new docs, now implemented multitag

* rumble subsystem + redside supplier

* moved all arm commands to ArmToAngle factories

* deleted unused files that caused chaos and confusion

* more deletion for those who cause chaos

* return if interrupted command

* ratchette disengage

* ritvik ratchette (RR)

* bundt shot (rsetty + alu)

* Bundt Shot working (yes)

* intake returns back up

* auton start pos and current limits

* intermap change + removing unnecessary comments

* remove rumble

* kalman tweak (trust vision more)

* changed from xboxcontroller to our own implementation

* reduces cpu usage

* updated defualt arm angle to 56.12 deg

* updated rachette and added rumble back

* updated intake current lim (as per Jeff's request)

* got ratchette working and added timeout to intake for auton

* changed intermap and introduced logging for arm angle

* updated arm constants to reflect updated weight

* added constant to improve arm aiming

* updated arm angle and logging

* commit for the intermap +

* intermap changes (working from far)

* fixed arm alignment shooting issue. WE ARE CONSISTIENT (except when odo is off)

* added commented constant voltage control for preshooter mtoro

* added square drive and changed preshooter motor to constant voltage for shoot

* updated preshooter wheel voltage

* added reset arm position and fixed square input

* added SignalLogger Logs

* changed signallogger

* changed mihir controls to change arm angles to pov buttons

* spotless and removed fireauton error

* Added multitag and multiple cameras

* simplified intermap code

* added comment to shooting speed

* added LED code

* organized code

* initial thing with enum

* fixed broken import

* got rid of unused code

* explicitly state shooter direction

* fixed amp

* added first rotation to mnovetotarget

* got rid of movetotarget changes

* cleaned auton logging

* changed kg to 0.2, made ks higher at 0.15 from 0.1

* change piper constants

* added square control to turning

* vision w multitag

* preshooter is now right direction on piper

* renaming MotorConstants factories

* deleted newpath

* spotless check

* Removed side camera pose bcz apparently we arent getting a second camera as promised.

* added intermap for piper

* used yajwin's eq to shoot

* new constants for kg/ks

* using the intermap

* yes

* yes

* yes

* 4 note working

* 4 note not working working ish

* intake update

* tol change

* converged voltage to velocity for intake

* updated wpi and spotless did its thing

* intake works! added intermap angle change

* swapped alterarmvalues to make more sense to the driver

* changed some key bindings

* added break during turning and improved shooting values

* removed lights and got rid of overturn

* readded

* vision revival

* removed rouge clamp

* yes

* yes

* changed constants to set target velocity to proper speed

* changed kv back to original

* revert back to original move to target and rc auton code

* aagrim asked me to

* fixed auto amp

---------

Co-authored-by: Warzone <[email protected]>
Co-authored-by: pastamaple73 <[email protected]>
Co-authored-by: Rawpower9 <[email protected]>
Co-authored-by: chivesonions <[email protected]>
Co-authored-by: ritvik <[email protected]>
Co-authored-by: mahikamaini <[email protected]>
Co-authored-by: Warzone <[email protected]>

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

@@ -199,10 +199,12 @@ private void configureBindings() {
                                             -(Units.inchesToMeters(5)
                                                 + Constants.Swerve.ROBOT_HALF_WIDTH_METERS),
                                             new Rotation2d())))),
-                        new SpinUpShooter(peterSubsystem, true)),
-                    new ShootNoWarmup(peterSubsystem, false),
-                    ArmToAngleCmd.toNeutral(armSubsystem))
-                .withInterruptBehavior(InterruptionBehavior.kCancelSelf));
+                        new SpinUpShooter(peterSubsystem, true))));
+                    // new ShootNoWarmup(peterSubsystem, false),
+                    // ArmToAngleCmd.toNeutral(armSubsystem))
+                // .withInterruptBehavior(InterruptionBehavior.kCancelSelf)
+                // );
+
     // just move arm to amp position
     joystickB
         .a()

src/main/java/frc/robot/commands/PeterCommands/SpinUpShooter.java

@@ -42,6 +42,10 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return peterSubsystem.isShooterReady();
+    if(isAmp){
+      return peterSubsystem.isShooterReadyAmp();
+    } else {
+      return peterSubsystem.isShooterReady();
+    }
   }
 }

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

@@ -145,7 +145,7 @@ public void spinLeftShooterForAmp() {
   }
 
   public void spinRightShooterForAmp() {
-    runLeftShooterAtRPS(Constants.Pooer.SHOOTER.SHOOTER_1.AMP_SPEED_RPS);
+    runRightShooterAtRPS(Constants.Pooer.SHOOTER.SHOOTER_1.AMP_SPEED_RPS);
   }
 
   public void stopRightShooter() {
@@ -195,6 +195,18 @@ public boolean isShooterReady() {
         < 10;
   }
 
+    public boolean isShooterReadyAmp() {
+      SmartDashboard.putNumber(
+        "shooter amp",
+        Constants.Pooer.SHOOTER.SHOOTER_1.AMP_SPEED_RPS * Constants.Pooer.SHOOTER.SHOOTER_1.GEAR_RATIO);
+    return Math.abs(
+            (shooter1.getVelocity().getValueAsDouble()
+                    )
+                - (Constants.Pooer.SHOOTER.SHOOTER_1.AMP_SPEED_RPS
+                    * Constants.Pooer.SHOOTER.SHOOTER_1.GEAR_RATIO))
+        < 10;
+  }
+
   /* private void runShooterAtRPS(double speed) {
     runRightShooterAtRPS(speed);
     runLeftShooterAtRPS(speed);