diff --git a/src/main/java/org/team1540/elmo/Constants.java b/src/main/java/org/team1540/elmo/Constants.java index be0c7b2..01ccf34 100644 --- a/src/main/java/org/team1540/elmo/Constants.java +++ b/src/main/java/org/team1540/elmo/Constants.java @@ -40,4 +40,13 @@ public final class Constants { public static final int DEPLOY_INNER_CHANNEL = 6; // COMPRESSOR public static final int COMPRESSOR = 0; + + // APRILTAGS + public static final int TOWER_APRILTAG_ID = 15; + public static final String CAMERA_NAME = "Microsoft_LifeCam_HD-3000"; + + // SELF MEASUREMENTS + public static final double CAMERA_HEIGHT_METERS = 2; + public static final double CAMERA_PITCH_RADIANS = 0; // from the horizontal plane >0=up + public static final double TARGET_HEIGHT_METERS = 2; } diff --git a/src/main/java/org/team1540/elmo/RobotContainer.java b/src/main/java/org/team1540/elmo/RobotContainer.java index 04ece6a..4c9c6cd 100644 --- a/src/main/java/org/team1540/elmo/RobotContainer.java +++ b/src/main/java/org/team1540/elmo/RobotContainer.java @@ -11,17 +11,12 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; -import java.util.concurrent.atomic.AtomicBoolean; - -import org.team1540.elmo.subsystems.drivetrain.ArcadeDrive; -import org.team1540.elmo.subsystems.drivetrain.DriveTrain; -import org.team1540.elmo.subsystems.drivetrain.TankDrive; +import org.team1540.elmo.subsystems.drivetrain.*; import org.team1540.elmo.subsystems.ejectors.*; +import org.team1540.elmo.utils.ChickenPhotonCamera; /** @@ -40,6 +35,7 @@ public class RobotContainer // Misc Components private final Compressor compressor = new Compressor(Constants.COMPRESSOR, PneumaticsModuleType.CTREPCM); + private final ChickenPhotonCamera camera = new ChickenPhotonCamera(Constants.CAMERA_NAME); // Control Devices private XboxController driver = new XboxController(Constants.DRIVER_CONTROLLER_PORT); @@ -68,32 +64,62 @@ private void configureButtonBindings() Trigger eitherBumper = new JoystickButton(driver,XboxController.Button.kLeftBumper.value).or(new JoystickButton(driver,XboxController.Button.kRightBumper.value)); Trigger neitherBumper = eitherBumper.negate(); + + //////////////////// SUCTION ///////////////////// + ///// Eject ///// // left side - new JoystickButton(driver,XboxController.Button.kX.value).and(neitherBumper).whenActive( + new JoystickButton(driver,XboxController.Button.kX.value).and(eitherBumper).whenActive( new EjectInnerCommand(innerEjector) ); // right side - new JoystickButton(driver,XboxController.Button.kB.value).and(neitherBumper).whenActive( + new JoystickButton(driver,XboxController.Button.kB.value).and(eitherBumper).whenActive( new EjectOuterCommand(outerEjector) ); // upper - new JoystickButton(driver,XboxController.Button.kY.value).and(neitherBumper).whenActive( + new JoystickButton(driver,XboxController.Button.kY.value).and(eitherBumper).whenActive( new EjectUpperCommand(upperEjector) ); // toggle individual suction when either bumper and X:inner / B:outer / Y:upper are pressed - new JoystickButton(driver,XboxController.Button.kX.value).and(eitherBumper).toggleWhenActive( + new JoystickButton(driver,XboxController.Button.kX.value).and(neitherBumper).toggleWhenActive( new ResetEjectorAndWaitForeverCommand(innerEjector,true) ); - new JoystickButton(driver,XboxController.Button.kB.value).and(eitherBumper).toggleWhenActive( + new JoystickButton(driver,XboxController.Button.kB.value).and(neitherBumper).toggleWhenActive( new ResetEjectorAndWaitForeverCommand(outerEjector,true) ); - new JoystickButton(driver,XboxController.Button.kY.value).and(eitherBumper).toggleWhenActive( + new JoystickButton(driver,XboxController.Button.kY.value).and(neitherBumper).toggleWhenActive( new ResetEjectorAndWaitForeverCommand(upperEjector,true) ); - driveTrain.setDefaultCommand(new TankDrive(driveTrain, driver)); - // driveTrain.setDefaultCommand(new ArcadeDrive(driveTrain, driver)); + //////////////////// AUTO DRIVE ///////////////////// + Trigger bothBumpers = new JoystickButton(driver,XboxController.Button.kLeftBumper.value).and(new JoystickButton(driver,XboxController.Button.kRightBumper.value)); + Trigger leftJoy = new JoystickButton(driver,XboxController.Button.kLeftStick.value); + Trigger rightJoy = new JoystickButton(driver,XboxController.Button.kRightStick.value); + Trigger bothJoys = leftJoy.and(rightJoy); + Trigger eitherJoy = leftJoy.or(rightJoy); + // reset gyro + bothBumpers.whenActive( + new InstantCommand(driveTrain::resetGyro) + ); + rightJoy.and(bothJoys.negate()).whileActiveContinuous( + new DriveForwardAndTurnCommand(45, .5, .3, driveTrain) + ); + leftJoy.and(bothJoys.negate()).whileActiveContinuous( + new DriveForwardAndTurnCommand(0, .5, .3, driveTrain) + ); + +// driveTrain.setDefaultCommand(new TankDrive(driveTrain, driver)); +// driveTrain.setDefaultCommand(new BobaTankDrive(driveTrain, driver)); + driveTrain.setDefaultCommand(new ArcadeDrive(driveTrain, driver)); + +// new JoystickButton(driver,XboxController.Button.kA.value).whileActiveOnce( +// new DriveToAprilTagPIDCommand(Constants.TOWER_APRILTAG_ID,0.1,driveTrain,camera) +// ); + new JoystickButton(driver,XboxController.Button.kA.value).toggleWhenPressed( + new SegmentedAuto(driveTrain,upperEjector,outerEjector,innerEjector,camera) + ); + + // Add button to command mappings here. // See https://docs.wpilib.org/en/stable/docs/software/commandbased/binding-commands-to-triggers.html @@ -108,6 +134,6 @@ private void configureButtonBindings() public Command getAutonomousCommand() { // An ExampleCommand will run in autonomous - return null; + return new SegmentedAuto(driveTrain,upperEjector,outerEjector,innerEjector,camera); } } diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/ArcadeDrive.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/ArcadeDrive.java index e3095d7..7877fec 100644 --- a/src/main/java/org/team1540/elmo/subsystems/drivetrain/ArcadeDrive.java +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/ArcadeDrive.java @@ -1,7 +1,8 @@ package org.team1540.elmo.subsystems.drivetrain; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; public class ArcadeDrive extends CommandBase { @@ -16,23 +17,21 @@ public ArcadeDrive(DriveTrain driveTrain, XboxController driver) { addRequirements(driveTrain); } + @Override + public void initialize() { + Shuffleboard.selectTab("Testies"); +// Shuffleboard.getTab("Testies").addNumber("ArcadeDrive/Spin",0); + } + @Override public void execute() { - double spinPercent = driver.getRightX(); + double spinPercent = -driver.getLeftX()/1.8; double forwardPercent = driver.getLeftY(); - - - double leftPercent = -spinPercent + forwardPercent; - double rightPercent = spinPercent + forwardPercent; - // Any calculated output is greater than 1, scale both coefficients down so that the max is 1 - double maxScalarPercent = Math.max(Math.abs(leftPercent),Math.abs(rightPercent)); - if(maxScalarPercent > 1) { - double coefficient = 1 / maxScalarPercent; - leftPercent *= coefficient; - rightPercent *= coefficient; - } + // all shit handled in drivetrain functions +// driveTrain.setSpeedAndSpin(forwardPercent, spinPercent); + SmartDashboard.putNumber("ArcadeDrive/Spin",spinPercent); - driveTrain.setPercent(leftPercent, rightPercent); + driveTrain.setSpeedAndSpinRamped(forwardPercent, spinPercent); } } diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/Auto.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/Auto.java new file mode 100644 index 0000000..68b8a0f --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/Auto.java @@ -0,0 +1,47 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import org.team1540.elmo.Constants; +import org.team1540.elmo.subsystems.ejectors.EjectInnerCommand; +import org.team1540.elmo.subsystems.ejectors.EjectOuterCommand; +import org.team1540.elmo.subsystems.ejectors.EjectUpperCommand; +import org.team1540.elmo.subsystems.ejectors.Ejector; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class Auto extends SequentialCommandGroup { + public Auto(DriveTrain driveTrain, Ejector.UpperEjector upperEjector, Ejector.OuterEjector outerEjector, Ejector.InnerEjector innerEjector, ChickenPhotonCamera camera) { + super( + // drive forward until can see april tag + deadline( + new WaitUntilCanSeeAprilTagCommand(Constants.TOWER_APRILTAG_ID,camera), + new DriveForwardRampedForeverCommand(-0.8,driveTrain) + ), + // drive fast (w/ ramping) pointing to tag until close to april tag + deadline( + new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,0.4,camera), + new DriveToAprilTagPIDCommand(Constants.TOWER_APRILTAG_ID,0.1,driveTrain,camera) + ), + // drive forward slowly for last few seconds + deadline( + new WaitCommand(1), + new DriveForwardRampedForeverCommand(0.1,driveTrain) + ), + // stop drivetrain (right now no ramping, possibly add ramping?) + new InstantCommand(driveTrain::stop,driveTrain), + // eject all the tubes! + parallel( + new EjectUpperCommand(upperEjector), + new EjectInnerCommand(innerEjector), + new EjectOuterCommand(outerEjector) + ), + // back up from the tower for a few seconds to give better position for driver to take over + deadline( + new DriveForwardRampedForeverCommand(-0.3,driveTrain), + new WaitCommand(1.5) + ), + new InstantCommand(driveTrain::stopRamped,driveTrain) + ); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/BobaTankDrive.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/BobaTankDrive.java new file mode 100644 index 0000000..f3e91fe --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/BobaTankDrive.java @@ -0,0 +1,58 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.CommandBase; +import org.team1540.elmo.utils.SignedSlewRateLimiter; + +/** + * Standard drive, left stick controls left side of drivetrain, right stick controls right side. + */ +public class BobaTankDrive extends CommandBase { + + private final DriveTrain drivetrain; + private final XboxController controller; + private final double deadzone = 0.15; + + private final SignedSlewRateLimiter speedRateLimiter = new SignedSlewRateLimiter(2,2); + private final SignedSlewRateLimiter spinRateLimiter = new SignedSlewRateLimiter(3,3); + + public BobaTankDrive(DriveTrain drivetrain, XboxController controller) { + this.drivetrain = drivetrain; + this.controller = controller; + addRequirements(drivetrain); + } + + @Override + public void execute() { + double left = Math.abs(controller.getLeftY()) > deadzone ? controller.getLeftY() : 0; + double right = Math.abs(controller.getRightY()) > deadzone ? controller.getRightY() : 0; +// double forward = Math.abs(controller.getRightTriggerAxis()) > deadzone ? Math.pow(controller.getRightTriggerAxis(), 3) : 0; +// double backward = Math.abs(controller.getLeftTriggerAxis()) > deadzone ? Math.pow(controller.getLeftTriggerAxis(), 3) : 0; +// double multiplier = 0.5 - ((0.3 * elevator.getRotations())/ElevatorConstants.ELEVATOR_ROTS_TO_TOP); +// speedRateLimiter.setAccelerationLimit(2-(elevatorPercentHeight*1)); // at bottom = 2-0%*1, at top: 2-100%*1 + + double speed = (left + right) / 2; + double spin = left - speed; // this changes sign lol + /// + speed = Math.pow(speed,2) * Math.signum(speed); + + spin = Math.pow(spin,2) * Math.signum(spin); + spin = spin/2; + /// + double rateLimitedSpeed = speedRateLimiter.calculate(speed); + double rateLimitedSpin = spinRateLimiter.calculate(spin); + +// double rateLimitedLeft = speed + spin; +// double rateLimitedRight = speed - spin; + double rateLimitedLeft = rateLimitedSpeed + rateLimitedSpin; + double rateLimitedRight = rateLimitedSpeed - rateLimitedSpin; + +// drivetrain.setPercent(multiplier*(-1*left + forward - backward), multiplier*(-1*right + forward - backward)); + drivetrain.setPercent(rateLimitedLeft,rateLimitedRight); + } + + @Override + public void end(boolean isInterrupted) { + drivetrain.stop(); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveForwardAndTurnCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveForwardAndTurnCommand.java new file mode 100644 index 0000000..41edb9a --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveForwardAndTurnCommand.java @@ -0,0 +1,31 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.PIDCommand; + +public class DriveForwardAndTurnCommand extends PIDCommand { + public DriveForwardAndTurnCommand(double setPoint, double rotationSpeed, double driveSpeed, DriveTrain driveTrain) { + super(new PIDController(rotationSpeed, rotationSpeed/7, 0), + ()->driveTrain.getAngle()%360, + ()->setPoint, + (double output)->{ // output from -180 to 180 + // put output to smartdashboard + SmartDashboard.putNumber("DriveForwardAndTurn/PID-Output", output); + + // void useOutput(double output) { + output=output/180; + double leftOutput = driveSpeed; + double rightOutput = driveSpeed; + if(output > 0) { + rightOutput -= output; + } else { + leftOutput -= output; + } + driveTrain.setPercent(leftOutput, rightOutput); + // } + }, + driveTrain); + this.m_controller.enableContinuousInput(0, 360); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveForwardRampedForeverCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveForwardRampedForeverCommand.java new file mode 100644 index 0000000..741aa9b --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveForwardRampedForeverCommand.java @@ -0,0 +1,19 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class DriveForwardRampedForeverCommand extends CommandBase { + private DriveTrain driveTrain; + private double motorPercent; + + public DriveForwardRampedForeverCommand(double motorPercent, DriveTrain driveTrain) { + addRequirements(driveTrain); + this.driveTrain = driveTrain; + this.motorPercent = motorPercent; + } + + @Override + public void execute() { + driveTrain.setPercentRamped(motorPercent,motorPercent); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagCommand.java new file mode 100644 index 0000000..3fb799e --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagCommand.java @@ -0,0 +1,34 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class DriveToAprilTagCommand extends CommandBase { + + private int tagId; + private double driveSpeedPercent; + private double turnSpeedMaxPercent; + private DriveTrain driveTrain; + private ChickenPhotonCamera camera; + + public DriveToAprilTagCommand(int tagId, double driveSpeedPercent, double turnSpeedFactor, DriveTrain driveTrain, ChickenPhotonCamera camera) { + this.tagId = tagId; + this.driveSpeedPercent = driveSpeedPercent; + this.turnSpeedMaxPercent = turnSpeedFactor; + this.driveTrain = driveTrain; + this.camera = camera; + } + + @Override + public void execute() { + PhotonTrackedTarget target = camera.getTargetById(tagId); + if(target==null) return; + + final double yaw = target.getYaw(); // in degrees + double turnSpeedPercent = yaw/180*turnSpeedMaxPercent; + + // this function handles ramping and shit + driveTrain.setSpeedAndSpinRamped(driveSpeedPercent, turnSpeedPercent); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand.java new file mode 100644 index 0000000..efdeab9 --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand.java @@ -0,0 +1,38 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand extends CommandBase { + + private int tagId; + private double driveSpeedPercent; + private double turnSpeedMaxPercent; + private DriveTrain driveTrain; + private ChickenPhotonCamera camera; + + public DriveToAprilTagNotUsingFancyDrivetrainMethodsCommand(int tagId, double driveSpeedPercent, double turnSpeedMaxPercent, DriveTrain driveTrain, ChickenPhotonCamera camera) { + this.tagId = tagId; + this.driveSpeedPercent = driveSpeedPercent; + this.turnSpeedMaxPercent = turnSpeedMaxPercent; + this.driveTrain = driveTrain; + this.camera = camera; + } + + @Override + public void execute() { + PhotonTrackedTarget target = camera.getTargetById(tagId); + if(target==null) return; + + final double yaw = target.getYaw(); // in degrees + double turnSpeedPercent = yaw/180*turnSpeedMaxPercent; + + double leftOutput = driveSpeedPercent; + double rightOutput = driveSpeedPercent; + leftOutput += turnSpeedPercent/2d; + rightOutput -= turnSpeedPercent/2d; + + driveTrain.setPercent(leftOutput, rightOutput); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagPIDCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagPIDCommand.java new file mode 100644 index 0000000..791b5ed --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveToAprilTagPIDCommand.java @@ -0,0 +1,61 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.CommandBase; +import org.photonvision.PhotonUtils; +import org.photonvision.targeting.PhotonTrackedTarget; +import org.team1540.elmo.Constants; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class DriveToAprilTagPIDCommand extends CommandBase { + + DriveTrain driveTrain; + int tagId; + ChickenPhotonCamera camera; + + PIDController pid = new PIDController(0.5,0,0.05); + + public DriveToAprilTagPIDCommand(int tagId, double targetDistance, DriveTrain driveTrain, ChickenPhotonCamera camera) { + this.tagId = tagId; + this.driveTrain = driveTrain; + this.camera = camera; + + pid.setSetpoint(targetDistance); + + SmartDashboard.putData(pid); + } + + public DriveToAprilTagPIDCommand(int tagId, DriveTrain driveTrain, ChickenPhotonCamera camera) { + this(tagId,0,driveTrain,camera); + } + + @Override + public void execute() { + PhotonTrackedTarget target = camera.getTargetById(tagId); + if(target==null) { + System.out.println(getName() + ": NO TARGET FOUND with id " + tagId); + return; + } +// var measurement = target.getBestCameraToTarget(); + double distance = PhotonUtils.calculateDistanceToTargetMeters( + Constants.CAMERA_HEIGHT_METERS, + Constants.TARGET_HEIGHT_METERS, + Constants.CAMERA_PITCH_RADIANS, + target.getPitch() + ); + double pitch = target.getPitch(); + + System.out.println("DRIVE PID COMMAND"); + System.out.print("Distance"); + System.out.println(distance); + + double speed = -pid.calculate(distance); + double spin = pitch/180; + + System.out.print("speed"); + System.out.println(speed); + + driveTrain.setSpeedAndSpin(speed,spin); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveTrain.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveTrain.java index f1abf86..8b9d3d3 100644 --- a/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveTrain.java +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/DriveTrain.java @@ -3,16 +3,21 @@ import com.ctre.phoenix.motorcontrol.ControlMode; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.kauailabs.navx.frc.AHRS; -import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.I2C.Port; +import edu.wpi.first.wpilibj.drive.Vector2d; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import org.team1540.elmo.Constants; +import org.team1540.elmo.utils.SignedSlewRateLimiter; public class DriveTrain extends SubsystemBase { private TalonSRX frontRight = new TalonSRX(Constants.RIGHT_FRONT_MOTOR_CAN_ID); private TalonSRX frontLeft = new TalonSRX(Constants.LEFT_FRONT_MOTOR_CAN_ID); private TalonSRX backRight = new TalonSRX(Constants.RIGHT_BACK_MOTOR_CAN_ID); private TalonSRX backLeft = new TalonSRX(Constants.LEFT_BACK_MOTOR_CAN_ID); + private AHRS navx = new AHRS(Port.kMXP); public DriveTrain() { backLeft.follow(frontLeft); @@ -26,12 +31,170 @@ public DriveTrain() { frontLeft.setNeutralMode(NeutralMode.Brake); } + @Override + public void periodic() { + super.periodic(); + SmartDashboard.putNumber(getName()+"/navx-angle", getAngle()); + + rampingPeriodic(); + } + public void stop() { setPercent(0, 0); } + public void stopRamped() { + setPercentRamped(0, 0); + } - public void setPercent(double leftPercent, double rightPercent) { + ///////////// RAMPING //////////// + private enum DriveRampingMode { + NO_RAMP_SS_SET, // no ramping but spin speed ratelimiters are reset + NO_RAMP_D_SET, // no ramping but differential ratelimiters are reset + SPEED_SPIN, + DIFFERENTIAL, + } + private DriveRampingMode driveRampingMode = DriveRampingMode.DIFFERENTIAL; + + private void rampingPeriodic() { + if(driveRampingMode.equals(DriveRampingMode.SPEED_SPIN)) { + setSpeedSpinRaw(speedRateLimiter.calculate(speedRampedSetpoint),spinRateLimiter.calculate(spinRampedSetpoint)); + SmartDashboard.putNumber("DriveTrain/SpinCalced",spinRateLimiter.calculate(spinRampedSetpoint)); + SmartDashboard.putNumber("ArcadeDrive/SpinSetPoint",spinRampedSetpoint); + } + else if(driveRampingMode.equals(DriveRampingMode.DIFFERENTIAL)) { + frontLeft.set(ControlMode.PercentOutput, leftRateLimiter.calculate(leftRampedSetpoint)); + frontRight.set(ControlMode.PercentOutput, rightRateLimiter.calculate(rightRampedSetpoint)); + } + } + + ///////////////// Differential Drive Mode ///////////////// + private void setPercentRaw(double leftPercent, double rightPercent) { frontLeft.set(ControlMode.PercentOutput, leftPercent); frontRight.set(ControlMode.PercentOutput, rightPercent); } + public double getLeftMotorPercent() { return frontLeft.getMotorOutputPercent(); } + public double getRightMotorPercent() { return frontRight.getMotorOutputPercent(); } + + public void setPercent(double leftPercent, double rightPercent) { + // set + setPercentRaw(leftPercent, rightPercent); + + // ramping + driveRampingMode = DriveRampingMode.NO_RAMP_D_SET; + leftRateLimiter.reset(leftPercent); + rightRateLimiter.reset(rightPercent); + } + + ///////////////// Differential Drive Mode Ramping ///////////////// + private final double defaultAccLimit = 4; + private final double defaultDecLimit = 3; + private double leftRampedSetpoint = 0; + private double rightRampedSetpoint = 0; + private SignedSlewRateLimiter leftRateLimiter = new SignedSlewRateLimiter( + SmartDashboard.getNumber(getName() + "/AccelerateLimit", defaultAccLimit), + SmartDashboard.getNumber(getName() + "/DecelerateLimit", defaultDecLimit) + ); + private SignedSlewRateLimiter rightRateLimiter = new SignedSlewRateLimiter( + SmartDashboard.getNumber(getName() + "/AccelerateLimit", defaultAccLimit), + SmartDashboard.getNumber(getName() + "/DecelerateLimit", defaultDecLimit) + ); + + public void setPercentRamped(double leftPercent, double rightPercent) { + // if setting right after using speed-spin ramped mode, translate units + if(driveRampingMode.equals(DriveRampingMode.SPEED_SPIN) || driveRampingMode.equals(DriveRampingMode.NO_RAMP_SS_SET)) { + double left = speedRateLimiter.getPrevVal() + spinRateLimiter.getPrevVal(); + double right = speedRateLimiter.getPrevVal() - spinRateLimiter.getPrevVal(); + leftRateLimiter.reset(left); + rightRateLimiter.reset(right); + } + + leftRampedSetpoint = leftPercent; + rightRampedSetpoint = rightPercent; + rampingPeriodic(); + } + public void setAccelerationRateLimit(double rateLimit) { + leftRateLimiter.setAccelerationLimit(rateLimit); + rightRateLimiter.setAccelerationLimit(rateLimit); + } + public void setDecelerationRateLimit(double rateLimit) { + leftRateLimiter.setDecelerationLimit(rateLimit); + rightRateLimiter.setDecelerationLimit(rateLimit); + } + + ///////////////// Speed/Rotate Mode ///////////////// + private static Vector2d preprocessSpeedSpin(double speedPercent, double spinPercent) { + // decrease drive percent as much as possible (to zero if needed) if max percent exceeds 1 + double maxScalarPercent = Math.abs(speedPercent)+Math.abs(spinPercent); + speedPercent -= Math.signum(speedPercent) * Math.max(0,maxScalarPercent-1); + + // cap percents at 1 by multiplying by a scale factor if method above didn't full work + maxScalarPercent = Math.abs(speedPercent)+Math.abs(spinPercent); + if(maxScalarPercent > 1) { + double coefficient = 1 / maxScalarPercent; + speedPercent *= coefficient; + spinPercent *= coefficient; + } + + // x: drive percent, y: spinPercent + return new Vector2d(speedPercent,spinPercent); + } + + private void setSpeedSpinRaw(double speedPercent, double spinPercent) { + double leftPercent, rightPercent; + leftPercent = rightPercent = speedPercent; + leftPercent += spinPercent; + rightPercent -= spinPercent; + setPercentRaw(leftPercent,rightPercent); + } + + public void setSpeedAndSpin(double speedPercent, double spinPercent) { + var processed = preprocessSpeedSpin(speedPercent,spinPercent); + setSpeedSpinRaw(processed.x,processed.y); + + // ramping + driveRampingMode = DriveRampingMode.NO_RAMP_SS_SET; + speedRateLimiter.reset(speedPercent); + spinRateLimiter.reset(spinPercent); + } + + ///////////////// Speed/Rotate Mode Ramping ///////////////// + private final double defaultSpinAccLimit = 3.5; + private final double defaultSpinDecLimit = 6; + private final double defaultSpeedAccLimit = 5; + private final double defaultSpeedDecLimit = 3; + private double speedRampedSetpoint = 0; + private double spinRampedSetpoint = 0; + private SignedSlewRateLimiter speedRateLimiter = new SignedSlewRateLimiter(defaultSpeedAccLimit,defaultSpeedDecLimit +// SmartDashboard.getNumber(getName() + "/SpeedAccLimit", defaultSpeedAccLimit), +// SmartDashboard.getNumber(getName() + "/SpeedDecLimit", defaultSpeedDecLimit) + ); + private SignedSlewRateLimiter spinRateLimiter = new SignedSlewRateLimiter(defaultSpinAccLimit,defaultSpinDecLimit +// SmartDashboard.getNumber(getName() + "/SpinAccLimit", defaultSpinAccLimit), +// SmartDashboard.getNumber(getName() + "/SpinDecLimit", defaultSpinDecLimit) + ); + + public void setSpeedAndSpinRamped(double speedPercent, double spinPercent) { + // if setting right after using differential ramped mode, translate units + if(driveRampingMode.equals(DriveRampingMode.DIFFERENTIAL) || driveRampingMode.equals(DriveRampingMode.NO_RAMP_D_SET)) { + double speed = (leftRateLimiter.getPrevVal() + rightRateLimiter.getPrevVal()) / 2; + double spin = (leftRateLimiter.getPrevVal() - speed); + speedRateLimiter.reset(speed); + spinRateLimiter.reset(spin); + } + + var processed = preprocessSpeedSpin(speedPercent,spinPercent); + speedRampedSetpoint = processed.x; + spinRampedSetpoint = processed.y; + + driveRampingMode = DriveRampingMode.SPEED_SPIN; + rampingPeriodic(); + } + + /////// SENSORS ////// + public double getAngle() { + return navx.getAngle(); + } + public void resetGyro() { + navx.reset(); + } } \ No newline at end of file diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/SegmentedAuto.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/SegmentedAuto.java new file mode 100644 index 0000000..5f6a108 --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/SegmentedAuto.java @@ -0,0 +1,54 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; +import org.team1540.elmo.Constants; +import org.team1540.elmo.subsystems.ejectors.EjectInnerCommand; +import org.team1540.elmo.subsystems.ejectors.EjectOuterCommand; +import org.team1540.elmo.subsystems.ejectors.EjectUpperCommand; +import org.team1540.elmo.subsystems.ejectors.Ejector; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class SegmentedAuto extends SequentialCommandGroup { + public SegmentedAuto(DriveTrain driveTrain, Ejector.UpperEjector upperEjector, Ejector.OuterEjector outerEjector, Ejector.InnerEjector innerEjector, ChickenPhotonCamera camera) { + super( + // drive forward until can see april tag + deadline( + new WaitUntilCanSeeAprilTagCommand(Constants.TOWER_APRILTAG_ID,camera), + new DriveForwardRampedForeverCommand(-0.8,driveTrain) + ), + // drive fast (w/ ramping) pointing to tag until close to april tag + deadline( + new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,2,camera), + new DriveToAprilTagCommand(Constants.TOWER_APRILTAG_ID,0.7,1,driveTrain,camera) + ), + // drive slow (w/ ramping) pointing to tag until very close to april tag + deadline( + new WaitUntilAprilTagIsWithinXMetersCommand(Constants.TOWER_APRILTAG_ID,0.1,camera), + new DriveToAprilTagCommand(Constants.TOWER_APRILTAG_ID,0.1,1,driveTrain,camera) + ), + // drive forward slowly for last few seconds + deadline( + new WaitCommand(1), + new DriveForwardRampedForeverCommand(0.1,driveTrain) + ), + // stop drivetrain (right now no ramping, possibly add ramping?) + new InstantCommand(driveTrain::stop,driveTrain), + // eject all tubes! + parallel( + new EjectUpperCommand(upperEjector), + new EjectInnerCommand(innerEjector), + new EjectOuterCommand(outerEjector) + ), + // wait for tubes to fully eject + new WaitCommand(2), + // back up from the tower for a few seconds to give better position for driver to take over + deadline( + new DriveForwardRampedForeverCommand(-0.3,driveTrain), + new WaitCommand(1.5) + ), + new InstantCommand(driveTrain::stopRamped,driveTrain) + ); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/TankDrive.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/TankDrive.java index 9884935..be4c4e8 100644 --- a/src/main/java/org/team1540/elmo/subsystems/drivetrain/TankDrive.java +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/TankDrive.java @@ -63,12 +63,13 @@ public void execute() { // double leftOutputPercent = totalOutputPercent * leftPercent / totalPercent; // proportion of left to total // double rightOutputPercent = totalOutputPercent * rightPercent / totalPercent; // proportion of - double leftOutPercent = leftRateLimiter.calculate(leftPercent); - double rightOutPercent = rightRateLimiter.calculate(rightPercent); +// double leftOutPercent = leftRateLimiter.calculate(leftPercent); +// double rightOutPercent = rightRateLimiter.calculate(rightPercent); +// driveTrain.setPercentRamped(leftOutPercent,rightOutPercent); +// SmartDashboard.putNumber(getName()+"/leftPercent", leftOutPercent); +// SmartDashboard.putNumber(getName()+"/rightPercent", rightOutPercent); - driveTrain.setPercent(leftOutPercent,rightOutPercent); + driveTrain.setPercentRamped(leftPercent,rightPercent); - SmartDashboard.putNumber(getName()+"/leftPercent", leftOutPercent); - SmartDashboard.putNumber(getName()+"/rightPercent", rightOutPercent); } } \ No newline at end of file diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilAprilTagIsWithinXMetersCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilAprilTagIsWithinXMetersCommand.java new file mode 100644 index 0000000..32f52bf --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilAprilTagIsWithinXMetersCommand.java @@ -0,0 +1,27 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import org.photonvision.PhotonUtils; +import org.team1540.elmo.Constants; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class WaitUntilAprilTagIsWithinXMetersCommand extends WaitUntilCommand { + + public WaitUntilAprilTagIsWithinXMetersCommand(int apriltagId, double meters, ChickenPhotonCamera camera) { + super(() -> + camera.getTargetById(apriltagId) != null + && PhotonUtils.calculateDistanceToTargetMeters( + Constants.CAMERA_HEIGHT_METERS, + Constants.TARGET_HEIGHT_METERS, + Constants.CAMERA_PITCH_RADIANS, + camera.getTargetById(apriltagId).getPitch()) < meters + ); + + } + + @Override + public void end(boolean interrupted) { + System.out.println("ENDED!"); + super.end(interrupted); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilCanSeeAprilTagCommand.java b/src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilCanSeeAprilTagCommand.java new file mode 100644 index 0000000..e90558d --- /dev/null +++ b/src/main/java/org/team1540/elmo/subsystems/drivetrain/WaitUntilCanSeeAprilTagCommand.java @@ -0,0 +1,11 @@ +package org.team1540.elmo.subsystems.drivetrain; + +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import org.team1540.elmo.utils.ChickenPhotonCamera; + +public class WaitUntilCanSeeAprilTagCommand extends WaitUntilCommand { + + public WaitUntilCanSeeAprilTagCommand(int apriltagId, ChickenPhotonCamera camera) { + super(()->camera.getTargetById(apriltagId)!=null); + } +} diff --git a/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectInnerCommand.java b/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectInnerCommand.java index 4db3ff4..a0d887a 100644 --- a/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectInnerCommand.java +++ b/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectInnerCommand.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import org.team1540.elmo.utils.WaitDashboardVarCommand; public class EjectInnerCommand extends SequentialCommandGroup { @@ -11,8 +12,8 @@ public EjectInnerCommand(Ejector.InnerEjector innerEjector) { addCommands( new InstantCommand(()->innerEjector.setSuction(true)), - new InstantCommand(innerEjector::extend), - new WaitDashboardVarCommand(0.15, "extend", this), // wait for extendermabober to start extending + new InstantCommand(()->innerEjector.extend(1.6)), + new WaitCommand(1.3), // wait for extendermabober to start extending new InstantCommand(()->innerEjector.setSuction(false)) ); } diff --git a/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectOuterCommand.java b/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectOuterCommand.java index a3bd8b9..f8cb81f 100644 --- a/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectOuterCommand.java +++ b/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectOuterCommand.java @@ -2,6 +2,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import org.team1540.elmo.utils.WaitDashboardVarCommand; public class EjectOuterCommand extends SequentialCommandGroup { @@ -11,8 +12,10 @@ public EjectOuterCommand(Ejector.OuterEjector outerEjector) { addCommands( new InstantCommand(()->outerEjector.setSuction(false)), - new WaitDashboardVarCommand(0.1,"unsuck",this), // wait for suction to fully unsucc. - new InstantCommand(outerEjector::extend) +// new WaitDashboardVarCommand(0.1,"unsuck",this), // wait for suction to fully unsucc. + new WaitCommand(0.3), + new InstantCommand(()->outerEjector.extend(0.5)), + new WaitCommand(0.5) ); } diff --git a/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectUpperCommand.java b/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectUpperCommand.java index 02162c6..e3120df 100644 --- a/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectUpperCommand.java +++ b/src/main/java/org/team1540/elmo/subsystems/ejectors/EjectUpperCommand.java @@ -3,14 +3,15 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; import org.team1540.elmo.utils.WaitDashboardVarCommand; public class EjectUpperCommand extends SequentialCommandGroup { public EjectUpperCommand(Ejector.UpperEjector upperEjector) { addCommands( - new InstantCommand(()->{upperEjector.setSuction(true);}), - new InstantCommand(upperEjector::extend), - new WaitDashboardVarCommand(.3,"extend",this), + new InstantCommand(()->upperEjector.setSuction(true)), + new InstantCommand(()->upperEjector.extend(2)), + new WaitCommand(2), new InstantCommand(()->upperEjector.setSuction(false)) ); } diff --git a/src/main/java/org/team1540/elmo/subsystems/ejectors/Ejector.java b/src/main/java/org/team1540/elmo/subsystems/ejectors/Ejector.java index 6cfdd75..fb4eb00 100644 --- a/src/main/java/org/team1540/elmo/subsystems/ejectors/Ejector.java +++ b/src/main/java/org/team1540/elmo/subsystems/ejectors/Ejector.java @@ -26,7 +26,11 @@ public void setExtended(boolean extended) { eject.set(extended); } public void extend() { - eject.setPulseDuration(SmartDashboard.getNumber(getName() + "/extendSecs",1)); + extend(0.4); +// extend(SmartDashboard.getNumber(getName() + "/extendSecs",1)); + } + public void extend(double seconds) { + eject.setPulseDuration(seconds); eject.startPulse(); } diff --git a/src/main/java/org/team1540/elmo/utils/ChickenPhotonCamera.java b/src/main/java/org/team1540/elmo/utils/ChickenPhotonCamera.java new file mode 100644 index 0000000..b9d283b --- /dev/null +++ b/src/main/java/org/team1540/elmo/utils/ChickenPhotonCamera.java @@ -0,0 +1,29 @@ +package org.team1540.elmo.utils; + +import edu.wpi.first.networktables.NetworkTableInstance; +import org.photonvision.PhotonCamera; +import org.photonvision.common.hardware.VisionLEDMode; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +import java.util.Optional; + +public class ChickenPhotonCamera extends PhotonCamera { + + public ChickenPhotonCamera(String cameraName) { + super(cameraName); + this.setLED(VisionLEDMode.kBlink); + this.setDriverMode(true); + } + + public PhotonTrackedTarget getTargetById(int id) { +// this.takeInputSnapshot(); + PhotonPipelineResult result = getLatestResult(); + var allTags = result.getTargets(); + System.out.println(allTags.toString()); + + Optional target = result.getTargets().stream().filter((targ)->targ.getFiducialId() == id).findFirst(); + return target.orElseGet(()->null); +// return getLatestResult().getBestTarget(); // testing + } +} diff --git a/src/main/java/org/team1540/elmo/utils/SignedSlewRateLimiter.java b/src/main/java/org/team1540/elmo/utils/SignedSlewRateLimiter.java index 858234d..a1c0f7d 100644 --- a/src/main/java/org/team1540/elmo/utils/SignedSlewRateLimiter.java +++ b/src/main/java/org/team1540/elmo/utils/SignedSlewRateLimiter.java @@ -14,31 +14,44 @@ * edu.wpi.first.math.trajectory.TrapezoidProfile} instead. */ public class SignedSlewRateLimiter { - private final double m_rateLimitAcc; - private final double m_rateLimitDecc; + private double m_rateLimitAcc; + private double m_rateLimitDecc; private double m_prevVal; private double m_prevTime; /** * Creates a new SlewRateLimiter with the given rate limit and initial value. * - * @param rateLimit The rate-of-change limit, in units per second. + * @param rateLimitAcc The rate-of-increase limit, in units per second. + * @param rateLimitDec The rate-of-decrease limit, in units per second. * @param initialValue The initial value of the input. */ - public SignedSlewRateLimiter(double rateLimitAcc, double rateLimitDecc, double initialValue) { + public SignedSlewRateLimiter(double rateLimitAcc, double rateLimitDec, double initialValue) { m_rateLimitAcc = rateLimitAcc; - m_rateLimitDecc = rateLimitDecc; + m_rateLimitDecc = rateLimitDec; m_prevVal = initialValue; m_prevTime = WPIUtilJNI.now() * 1e-6; } + public double getPrevVal() { + return m_prevVal; + } + + public void setAccelerationLimit(double rateLimitAcc) { + this.m_rateLimitAcc = rateLimitAcc; + } + public void setDecelerationLimit(double rateLimitDecc) { + this.m_rateLimitDecc = rateLimitDecc; + } + /** * Creates a new SlewRateLimiter with the given rate limit and an initial value of zero. * - * @param rateLimit The rate-of-change limit, in units per second. + * @param rateLimitAcc The rate-of-increase limit, in units per second. + * @param rateLimitDec The rate-of-decrease limit, in units per second. */ - public SignedSlewRateLimiter(double rateLimitAcc, double rateLimitDecc) { - this(rateLimitAcc, rateLimitDecc, 0); + public SignedSlewRateLimiter(double rateLimitAcc, double rateLimitDec) { + this(rateLimitAcc, rateLimitDec, 0); } /**