diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b539b2f..e7cfc1b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -60,7 +60,7 @@ public class RobotContainer { private final IntakeSubsystem m_intake = new IntakeSubsystem(); - // private final ShooterSubsystem m_shooter = new ShooterSubsystem(); + // private final ShooterSubsystem m_shooter = new ShooterSubsystem(); private final SwerveSubsystem swerveSubsystem = new SwerveSubsystem(); private final XboxController driverJoytick = new XboxController(1); private final XboxController subJoytick = new XboxController(3); @@ -95,6 +95,7 @@ private void configureBindings() { // new JoystickButton(driverJoytick, 5).whileTrue(new RunTillSwitch(m_intake,false,m_feeder,m_joystick,m_shooter)); new JoystickButton(driverJoytick, 5).whileFalse(new InstantCommand(()->m_intake.StopNoteMotor())); + // new JoystickButton(subJoytick, 3).whileTrue(new ShooterSetDegree(m_shooter, ()->70.0)); // new JoystickButton(driverJoytick, 6).whileTrue(new FedX(m_intake, m_feeder, m_joystick,m_shooter)); @@ -226,8 +227,23 @@ public Command getAutonomousCommand() { TrajectoryGenerator.generateTrajectory( new Pose2d(0, 0, new Rotation2d(0)), List.of(new Translation2d(-1.28, 0),new Translation2d(-0.56, -0.91)), - new Pose2d(-1.28, -1.44, new Rotation2d(0)), + new Pose2d(-1.28, -1.44, new Rotation2d(3.1)), trajectoryConfig); + + var trajectoryTwo = + TrajectoryGenerator.generateTrajectory( + new Pose2d(-1.28, -1.44, new Rotation2d(0)), + List.of(new Translation2d(-1.28, 0),new Translation2d(0.5, -0.5)), + new Pose2d(-0,0, new Rotation2d(0)), + trajectoryConfig); + + var trajectoryThree = + TrajectoryGenerator.generateTrajectory( + new Pose2d(0, 0, new Rotation2d(0)), + List.of(new Translation2d(1, 0),new Translation2d(1, -1)), + new Pose2d(0,0, new Rotation2d(2)), + trajectoryConfig); + // var trajectoryOne = // TrajectoryGenerator.generateTrajectory( @@ -255,7 +271,27 @@ public Command getAutonomousCommand() { swerveSubsystem::OtosetModuleStates, swerveSubsystem); - return swerveControllerCommand; + SwerveControllerCommand swerveControllerCommand2 = new SwerveControllerCommand( + trajectoryTwo, + swerveSubsystem::getPose, + DriveConstants.kDriveKinematics, + xController, + yController, + thetaController, + swerveSubsystem::OtosetModuleStates, + swerveSubsystem); + + SwerveControllerCommand swerveControllerCommand3 = new SwerveControllerCommand( + trajectoryThree, + swerveSubsystem::getPose, + DriveConstants.kDriveKinematics, + xController, + yController, + thetaController, + swerveSubsystem::OtosetModuleStates, + swerveSubsystem); + + return swerveControllerCommand.andThen(swerveControllerCommand2).andThen(swerveControllerCommand3); // PathPlannerPath path = PathPlannerPath.fromPathFile("New New Path"); // Create a path following command using AutoBuilder. This will also trigger event markers. // return swerveSubsystem.autobuilder.followPath(path); diff --git a/src/main/java/frc/robot/commands/feeder/FeederRunTillSwitch.java b/src/main/java/frc/robot/commands/feeder/FeederRunTillSwitch.java index d407107..c122cc8 100644 --- a/src/main/java/frc/robot/commands/feeder/FeederRunTillSwitch.java +++ b/src/main/java/frc/robot/commands/feeder/FeederRunTillSwitch.java @@ -13,15 +13,13 @@ public class FeederRunTillSwitch extends Command { public FeederRunTillSwitch(FeederSubsystem m_feeder, boolean dursunmu) { - this.m_feeder = m_feeder; - } // Called when the command is initially scheduled. @Override public void initialize() { - + } // Called every time the scheduler runs while the command is scheduled. @@ -29,28 +27,22 @@ public void initialize() { public void execute() { - boolean ilkswitch; +if(m_feeder.varmi){ +if(m_feeder.detector()){ +m_feeder.backward2(); +} +else{ + m_feeder.stop(); + m_feeder.varmifalse(); - ilkswitch = m_feeder.detector(); +}} -if(m_feeder.varmi){ -if(!dursunmu){ - if(ilkswitch){ - - m_feeder.backward2(); - - }else{ - m_feeder.varmifalse(); - m_feeder.stop(); - } - } -} - } +} // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/subsystems/FeederSubsystem.java b/src/main/java/frc/robot/subsystems/FeederSubsystem.java index 97e5a2c..2a39989 100644 --- a/src/main/java/frc/robot/subsystems/FeederSubsystem.java +++ b/src/main/java/frc/robot/subsystems/FeederSubsystem.java @@ -1,5 +1,4 @@ package frc.robot.subsystems; - import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.DigitalInput; //import edu.wpi.first.wpilibj.DigitalInput; @@ -10,18 +9,17 @@ import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX; public class FeederSubsystem extends SubsystemBase { - - // public WPI_VictorSPX feedmotor = new WPI_VictorSPX(7); - //public DigitalInput detector = new DigitalInput(6); + public WPI_VictorSPX feedmotor = new WPI_VictorSPX(7); + public DigitalInput detector = new DigitalInput(6); public boolean magstatus; public boolean count; IntakeSubsystem m_intake; boolean tekcalisma; boolean dolumu = false; public boolean varmi = true; - + //public AnalogInput intechedetector = new AnalogInput(0); - + public FeederSubsystem() { @@ -39,46 +37,31 @@ public void varmifalse(){ } public boolean detector() { - //return detector.get(); - return false; - } - - - public void forward() { - // feedmotor.set(0.3); + return detector.get(); } public void backward2() { - //feedmotor.set(-0.7); + feedmotor.set(-0.7); } public void backward() { - // feedmotor.set(-0.9); + feedmotor.set(-0.9); } - - public void runtillswitch(){ -/*while((detector.get())){ - feedmotor.set(-0.6); - - - while ((detector.get())) { - feedmotor.set(-0.8); - } - feedmotor.set(0); - -}*/ + public void runtillswitch(){ + if(detector.get()){ + feedmotor.set(-0.6); + }else{ + feedmotor.set(0.0); - } + } + } //while((detector.getValue() > 250)&&(intechedetector.getValue() < 2000)){ //feedmotor.set(-0.8); - // } // feedmotor.set(0); - // } - // public boolean ismagfilled(){ // if (detector.getValue() >){ // return false; @@ -88,14 +71,12 @@ public void runtillswitch(){ // } // } + public void stop() { - //feedmotor.set(0); + feedmotor.set(0); } - @Override public void periodic() { - - //SmartDashboard.putBoolean("bakacaz", detector.get()); - + SmartDashboard.putBoolean("feeder Switch", detector.get()); } -} +} \ No newline at end of file