testing chganges

grt192 · Feb 6, 2024 · 7cc48a4 · 7cc48a4
1 parent 1ffe51f
commit 7cc48a4
Show file tree

Hide file tree

Showing 13 changed files with 45 additions and 19 deletions.
diff --git a/src/main/deploy/choreo/2X1MCurve.traj → src/main/deploy/choreo/Curve.traj b/src/main/deploy/choreo/2X1MCurve.traj → src/main/deploy/choreo/Curve.traj
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -27,14 +27,14 @@ public static class ElevatorConstants {
     public static final double GROUND_POSITION = 0;
     public static final double SPEAKER_POSITION = 0;
     public static final double AMP_POSITION = Units.inchesToMeters(28);
-    public static final double CHUTE_POSITION = Units.inchesToMeters(25);
+    public static final double CHUTE_POSITION = Units.inchesToMeters(1);
     public static final double TRAP_POSITION = Units.inchesToMeters(30);
     public static final double START_POSITION = 0; 
 
     public static final double EXTENSION_P= 3.5;
     public static final double EXTENSION_I= 0;
     public static final double EXTENSION_D= 0;
-    public static final double EXTENSION_TOLERANCE= 0.003;
+    public static final double EXTENSION_TOLERANCE= 0.3;
 
     public static final double POSITION_CONVERSION_FACTOR = Units.inchesToMeters(30.)/63.5;
     public static final double VELOCITY_CONVERSION_FACTOR = 1;
@@ -83,7 +83,7 @@ public static class IntakeConstants {
     //public static final int intakeencoderID = 3;
 
     public static final double rollersclockwise = 1;
-    public static final double rollerscounterclockwise = -1; 
+    public static final double rollerscounterclockwise = 1; 
     public static final double sensorreached = .3;
     public static final double pivotclockwise = 1;
     public static final double pivotcounterclockwise = -1;

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -17,6 +17,7 @@
 import frc.robot.commands.climb.ClimbLowerCommand;
 import frc.robot.commands.climb.ClimbRaiseCommand;
 import frc.robot.commands.elevator.ElevatorToAMPCommand;
+import frc.robot.commands.elevator.ElevatorToChuteCommand;
 import frc.robot.commands.elevator.ElevatorToGroundCommand;
 import frc.robot.commands.intake.roller.IntakeRollerFeedCommand;
 import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand;
@@ -51,9 +52,11 @@
 import edu.wpi.first.wpilibj2.command.button.Trigger;
 import static frc.robot.Constants.SwerveConstants.*;
 
+import edu.wpi.first.cscore.MjpegServer;
+import edu.wpi.first.cscore.UsbCamera;
 public class RobotContainer {
   // The robot's subsystems and commands are defined here...
-    private final BaseDriveController driveController = new XboxDriveController();
+    private final BaseDriveController driveController = new DualJoystickDriveController();
     private final BaseSwerveSubsystem baseSwerveSubsystem;
 
     private final IntakePivotSubsystem intakePivotSubsystem;
@@ -75,6 +78,8 @@ public class RobotContainer {
     private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value);
     private final JoystickButton yButton = new JoystickButton(mechController, XboxController.Button.kY.value);
 
+    private UsbCamera camera1;
+    private MjpegServer mjpgserver1;
     //private final JoystickButton xButton = new JoystickButton(mechController, XboxController.Button.kX.value);
 
     ChoreoTrajectory traj;
@@ -100,11 +105,19 @@ public RobotContainer() {
 
         // Configure the trigger bindings
         configureBindings();
+
+        camera1 = new UsbCamera("camera1", 0);
+        camera1.setFPS(60);
+        camera1.setBrightness(45);
+        camera1.setResolution(176, 144);
+        mjpgserver1 = new MjpegServer("m1", 1181);
+        mjpgserver1.setSource(camera1);
   }
 
 
   private void configureBindings() {
-        aButton.onTrue(new IntakeRollerIntakeCommand(intakeRollerSubsystem));
+        aButton.onTrue(new ElevatorToChuteCommand(elevatorSubsystem).andThen(
+                       new IntakeRollerIntakeCommand(intakeRollerSubsystem)));
 
         bButton.onTrue(new IdleCommand(intakePivotSubsystem, intakeRollerSubsystem, 
                                        elevatorSubsystem, 

diff --git a/src/main/java/frc/robot/commands/IdleCommand.java b/src/main/java/frc/robot/commands/IdleCommand.java
@@ -21,15 +21,15 @@ public IdleCommand(IntakePivotSubsystem intakePivotSubsystem,
                        ShooterFeederSubsystem shooterFeederSubsystem,
                        ShooterFlywheelSubsystem shooterFlywheelSubsystem,
                        ClimbSubsystem climbSubsystem){
-        addRequirements(intakePivotSubsystem, intakeRollersSubsystem,
-                        elevatorSubsystem,
-                        shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem,
-                        climbSubsystem
-        );
+        // addRequirements(intakePivotSubsystem, intakeRollersSubsystem,
+        //                 elevatorSubsystem,
+        //                 shooterPivotSubsystem, shooterFeederSubsystem, shooterFlywheelSubsystem,
+        //                 climbSubsystem
+        // );
 
-        addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0)),
+        addCommands(new InstantCommand(() -> intakeRollersSubsystem.setAllRollSpeed(0, 0), intakeRollersSubsystem),
                     new ElevatorToGroundCommand(elevatorSubsystem),
-                    new InstantCommand(() -> shooterFeederSubsystem.setFeederMotorSpeed(0)),
+                    new InstantCommand(() -> shooterFeederSubsystem.setFeederMotorSpeed(0), shooterFeederSubsystem),
                     new ShooterFlywheelStopCommand(shooterFlywheelSubsystem)//,
                     // new ClimbLowerCommand(climbSubsystem) // NOT USING CLIMB FOR NOW
         );

diff --git a/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java b/src/main/java/frc/robot/commands/elevator/ElevatorToGroundCommand.java
@@ -32,6 +32,7 @@ public void execute(){
 
     @Override
     public boolean isFinished(){
+        System.out.println("TO GROUND RUNNING");
         return elevatorSubsystem.atState(ElevatorState.GROUND);
     }
 }
diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerFeedCommand.java
@@ -15,12 +15,13 @@ public class IntakeRollerFeedCommand extends Command{
     public IntakeRollerFeedCommand(IntakeRollersSubsystem intakeSubsystem){
         this.intakeSubsystem = intakeSubsystem;
         timer = new TrackingTimer();
+        addRequirements(intakeSubsystem);
     }
 
     @Override
     public void initialize() {
         // TODO Auto-generated method stub
-        intakeSubsystem.setAllRollSpeed(rollerscounterclockwise,rollersclockwise);
+        intakeSubsystem.setAllRollSpeed(rollerscounterclockwise, rollersclockwise);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerIntakeCommand.java
@@ -12,6 +12,7 @@ public class IntakeRollerIntakeCommand extends Command{
 
     public IntakeRollerIntakeCommand(IntakeRollersSubsystem intakeSubsystem){
         this.intakeSubsystem = intakeSubsystem;
+        addRequirements(intakeSubsystem);
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java b/src/main/java/frc/robot/commands/intake/roller/IntakeRollerOutakeCommand.java
@@ -16,11 +16,13 @@ public class IntakeRollerOutakeCommand extends Command{
     public IntakeRollerOutakeCommand(IntakeRollersSubsystem intakeSubsystem){
         this.intakeSubsystem = intakeSubsystem;
         timer = new TrackingTimer();
+        addRequirements(intakeSubsystem);
     }
 
     @Override
     public void initialize() {
-        intakeSubsystem.setRollSpeed(rollersclockwise,rollerscounterclockwise); 
+        timer.reset();
+        intakeSubsystem.setRollSpeed(-1,-1); 
     }
 
     @Override

diff --git a/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java b/src/main/java/frc/robot/commands/sequences/ShootModeSequence.java
@@ -19,7 +19,7 @@ public class ShootModeSequence extends SequentialCommandGroup {
 
     /*
      *                    START
-     *              /     |     `---------.
+     *                /   |     `---------.
      *  Angle shooter    lower         Start Flywheels   
      *  for intaking    elevator          |
      *             \  /                  |

diff --git a/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java b/src/main/java/frc/robot/commands/shooter/pivot/ShooterPivotSetAngleCommand.java
@@ -14,12 +14,14 @@ public ShooterPivotSetAngleCommand(ShooterPivotSubsystem pivotSubsystem, double
 
     @Override
     public void initialize() {
+        System.out.println("SET ANGLE");
         pivotSubsystem.setAutoAimBoolean(false);
         pivotSubsystem.setAngle(angle);
     }
 
     @Override
     public boolean isFinished() {
+        System.out.println("NOT FINISHED");
         return (Math.abs(pivotSubsystem.getPosition() - angle) < pivotSubsystem.ERRORTOLERANCE);
     }
 

diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java b/src/main/java/frc/robot/subsystems/intake/IntakeRollersSubsystem.java
@@ -64,6 +64,7 @@ public void setRollersInwards(Boolean pressedB){
 
   @Override
   public void periodic() {
+    // System.out.println(sensor.get());
     // This method will be called once per scheduler run
   }
   @Override

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterFeederSubsystem.java
@@ -10,9 +10,9 @@
 public class ShooterFeederSubsystem extends SubsystemBase{
 
     //finals
-    public final double FEEDER_MOTOR_SPEED = 0.1;
+    public final double FEEDER_MOTOR_SPEED = .8;
     public final int NO_NOTE_TOLERANCE = 500; //must test with no note in front of sensor
-    public final int TOLERANCE = 7000; //represents the value when half note is in front of sensor
+    public final int TOLERANCE = 1000; //represents the value when half note is in front of sensor
 
     //motors
     private final TalonSRX feederMotor; 
@@ -36,12 +36,17 @@ public void setFeederMotorSpeed(double speed){
     }
 
     public int getRed(){
-        System.out.println("proximity: " + shooterSensor.getProximity());
+        // System.out.println("proximity: " + shooterSensor.getProximity());
         return shooterSensor.getRed();
     }
 
     public ColorSensorV3 getSensor(){
         System.out.println("returning shooter sensor");
         return shooterSensor;
     }
+
+    @Override
+    public void periodic() {
+        System.out.println(getRed());
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java
@@ -218,7 +218,7 @@ public void resetDriverHeading() {
     }
 
     private Rotation2d getGyroHeading() {
-        return Rotation2d.fromDegrees(ahrs.getAngle());
+        return Rotation2d.fromDegrees(-ahrs.getAngle());
     }
 
     public Rotation2d getDriverHeading() {