diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 835dff1..4ae4474 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -58,7 +58,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final int SHOOTER_TIMEOUT_MS = 30; - public static final Gains SHOOTER_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_GAINS = new Gains(0.0, 0.0, 0.0, 0.0542, 0, 1.0); public static final double ENCODER_TICKS_PER_REV = 2048; } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 86be53e..e9527ae 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,7 +15,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; -import frc4388.robot.commands.DistanceElevatorPID; import frc4388.robot.commands.DriveAtVelocityPID; import frc4388.robot.commands.DriveToDistanceMM; import frc4388.robot.commands.DriveToDistancePID; @@ -23,7 +22,6 @@ import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Elevator; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.Shooter; @@ -43,7 +41,6 @@ public class RobotContainer { private final Drive m_robotDrive = new Drive(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); - private final Elevator m_robotElevator = new Elevator(); private final Shooter m_robotShooter = new Shooter(); /* Controllers */ @@ -61,12 +58,9 @@ public class RobotContainer { m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, getDriverController())); // drives motor with input from triggers on the opperator controller m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); - // moves elevator with one-axis input from the driver controller - m_robotElevator.setDefaultCommand(new RunCommand(() -> m_robotElevator.moveElevator( - getOperatorController().getLeftYAxis()), m_robotElevator - )); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(() -> m_robotLED.updateLED(), m_robotLED)); + m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); } /** @@ -97,10 +91,7 @@ public class RobotContainer { .whenPressed(new DriveAtVelocityPID(m_robotDrive, 2000)); new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 2300)); - - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new DistanceElevatorPID(m_robotElevator, 20000)); + .whileHeld(new ShooterVelocityControlPID(m_robotShooter, 3200)); /*new JoystickButton(getDriverJoystick(), XboxController.LEFT_JOYSTICK_BUTTON) .whenPressed(new InstantCommand(null, m_robotDrive));*/ diff --git a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java b/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java deleted file mode 100644 index 10644ca..0000000 --- a/src/main/java/frc4388/robot/commands/DistanceElevatorPID.java +++ /dev/null @@ -1,56 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Elevator; - -public class DistanceElevatorPID extends CommandBase { - Elevator m_elevator; - double m_distance; - double m_target1; - double m_target2; - /** - * Creates a new DistanceElevatorPID. - */ - public DistanceElevatorPID(Elevator subsystem, double distance) { - // Use addRequirements() here to declare subsystem dependencies. - m_elevator = subsystem; - m_distance = distance; - addRequirements(m_elevator); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_target1 = m_elevator.m_talon1.getActiveTrajectoryPosition() + m_distance; - m_target2 = m_elevator.m_talon2.getActiveTrajectoryPosition() + m_distance; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_elevator.runElevatorPositionPID(m_elevator.m_talon1, m_target1); - m_elevator.runElevatorPositionPID(m_elevator.m_talon2, m_target2); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (Math.abs(m_elevator.m_talon1.getActiveTrajectoryPosition() - m_target1) < 100) { - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index 4c7b058..dbd6603 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -31,10 +31,11 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000){ + if (/*m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity() < 1000*/false){ m_shooter.runDrumShooter(0.5); + System.err.println("Less than 1000: " + m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } else { - m_shooter.runDrumShooterVelocityPID(m_targetVel); + m_shooter.runDrumShooterVelocityPID(m_targetVel, m_shooter.m_shooterFalcon.getActiveTrajectoryVelocity()); } } diff --git a/src/main/java/frc4388/robot/subsystems/Elevator.java b/src/main/java/frc4388/robot/subsystems/Elevator.java deleted file mode 100644 index eec7909..0000000 --- a/src/main/java/frc4388/robot/subsystems/Elevator.java +++ /dev/null @@ -1,78 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.ctre.phoenix.motorcontrol.ControlMode; -import com.ctre.phoenix.motorcontrol.InvertType; -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Gains; -import frc4388.robot.Constants.ElevatorConstants; - -public class Elevator extends SubsystemBase { - - public WPI_TalonSRX m_talon1 = new WPI_TalonSRX(ElevatorConstants.TALON_1); - public WPI_TalonSRX m_talon2 = new WPI_TalonSRX(ElevatorConstants.TALON_2); - - public static Gains m_elevatorGains = ElevatorConstants.ELEVATOR_GAINS; - /** - * Creates a new Elevator. - */ - public Elevator() { - //resets motors - m_talon1.configFactoryDefault(); - m_talon2.configFactoryDefault(); - - //config following settings - m_talon2.follow(m_talon1); - - m_talon1.setNeutralMode(NeutralMode.Brake); - m_talon2.setNeutralMode(NeutralMode.Brake); - - m_talon1.setInverted(false); - m_talon2.setInverted(false); - m_talon1.setInverted(InvertType.FollowMaster); - m_talon2.setInverted(InvertType.FollowMaster); - - setElevatorGains(); - - m_talon1.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.setSelectedSensorPosition(0, ElevatorConstants.ELEVATOR_PID_LOOP_IDX, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - - int closedLoopTimeMs = 1; - m_talon1.configClosedLoopPeriod(0, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.configClosedLoopPeriod(1, closedLoopTimeMs, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - public void moveElevator(double speed) { - m_talon1.set(speed); - m_talon2.set(speed); - } - public void setElevatorGains(){ - m_talon1.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon1.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon1.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - - m_talon2.selectProfileSlot(ElevatorConstants.ELEVATOR_SLOT_IDX, ElevatorConstants.ELEVATOR_PID_LOOP_IDX); - m_talon2.config_kF(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kF, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kP(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kP, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kI(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kI, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - m_talon2.config_kD(ElevatorConstants.ELEVATOR_SLOT_IDX, m_elevatorGains.kD, ElevatorConstants.ELEVATOR_TIMEOUT_MS); - } - public void runElevatorPositionPID(WPI_TalonSRX talon, double targetPos) { - talon.set(ControlMode.Position, targetPos); - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 9452170..5fe805a 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -25,6 +25,7 @@ public class Shooter extends SubsystemBase { public static Gains m_shooterGains = ShooterConstants.SHOOTER_GAINS; + double velP; /** * Creates a new Shooter. */ @@ -76,7 +77,13 @@ public class Shooter extends SubsystemBase { * @param falcon Motor to use * @param targetVel Target velocity to run motor at */ - public void runDrumShooterVelocityPID(double targetVel) { - m_shooterFalcon.set(TalonFXControlMode.Velocity, m_targetVel*ShooterConstants.ENCODER_TICKS_PER_REV/600); + public void runDrumShooterVelocityPID(double targetVel, double actualVel) { + velP = actualVel/targetVel; + if(velP < 0.1){ + velP = 0.1; + } + double runSpeed = velP*(1-velP); + System.err.println(runSpeed); + m_shooterFalcon.set(TalonFXControlMode.PercentOutput, runSpeed/*ShooterConstants.ENCODER_TICKS_PER_REV/600*/); } -} +} \ No newline at end of file