diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3408679..642d0da 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,26 +30,33 @@ public final class Constants { public static final int PIGEON_ID = 6; /* Drive Inversions */ - public static final boolean isRightMotorInverted = false; + public static final boolean isRightMotorInverted = true; public static final boolean isLeftMotorInverted = false; public static final boolean isRightArcadeInverted = false; public static final boolean isAuxPIDInverted = false; /* Drive Configuration */ - public static final double OPEN_LOOP_RAMP_RATE = 0.1; + public static final int DRIVE_TIMEOUT_MS = 30; // Use for all motor config + public static final double OPEN_LOOP_RAMP_RATE = 0.1; // Seconds from 0 to full power on motor public static final double NEUTRAL_DEADBAND = 0.04; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = - new SupplyCurrentLimitConfiguration(false, 40, 35, 0.01); - public static final int CLOSED_LOOP_TIME_MS = 1; + new SupplyCurrentLimitConfiguration(false, 60, 40, 2); + public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops + + /* Drive Train Characteristics */ + public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; + public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; + public static final double WHEEL_DIAMETER_INCHES = 6; + public static final double TICKS_PER_GYRO_REV = 8192; + public static final double TICKS_PER_MOTOR_REV = 2048; /* PID Constants Drive*/ - public static final int DRIVE_TIMEOUT_MS = 30; public static final Gains DRIVE_DISTANCE_GAINS_LOW = new Gains(0.1, 0.0, 1.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_LOW = new Gains(0.1, 0.0, 0.2, 0.025, 0, 1.0); - public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.05, 0.0, 0, 0.55); + public static final Gains DRIVE_TURNING_GAINS_LOW = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.55); public static final Gains DRIVE_MOTION_MAGIC_GAINS_LOW = new Gains(0.2, 0.0, 0.0, 0.0, 0, 1.0); - public static final int DRIVE_CRUISE_VELOCITY = 20000; - public static final int DRIVE_ACCELERATION = 7000; + public static final int DRIVE_CRUISE_VELOCITY = 25000; + public static final int DRIVE_ACCELERATION = 21000; public static final Gains DRIVE_DISTANCE_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 0.5); public static final Gains DRIVE_VELOCITY_GAINS_HIGH = new Gains(0.0, 0.0, 0.0, 0.0, 0, 1.0); @@ -78,13 +85,6 @@ public final class Constants { public final static int SLOT_TURNING = 2; public final static int SLOT_MOTION_MAGIC = 3; - /* Drive Train Characteristics */ - public static final double TICKS_PER_MOTOR_REV = 2048; - public static final double MOTOR_ROT_PER_WHEEL_ROT_HIGH = 5.13; - public static final double MOTOR_ROT_PER_WHEEL_ROT_LOW = 15; - public static final double WHEEL_DIAMETER_INCHES = 6; - public static final double TICKS_PER_GYRO_REV = 8192; - /* Ratio Calculation */ public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; public static final double TICK_TIME_TO_SECONDS = 0.1; @@ -103,10 +103,10 @@ public final class Constants { public static final double INCHES_PER_TICK_LOW = 1/TICKS_PER_INCH_LOW; } - public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = -9; - public static final int EXTENDER_SPARK_ID = -10; + public static final class IntakeConstants {; public static final double EXTENDER_SPEED = 0.3; + public static final int INTAKE_SPARK_ID = 12; + public static final int EXTENDER_SPARK_ID = 13; } public static final class ShooterConstants { @@ -176,6 +176,16 @@ public final class Constants { public static final double STORAGE_MIN_OUTPUT = -1.0; public static final Gains STORAGE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 1.0); } + + public static final class PneumaticsConstants { + public static final int PCM_MODULE_ID = 7; + + public static final int SPEED_SHIFT_FORWARD_ID = 0; + public static final int SPEED_SHIFT_REVERSE_ID = 1; + + public static final int COOL_FALCON_FORWARD_ID = 3; + public static final int COOL_FALCON_REVERSE_ID = 2; + } public static final class LEDConstants { public static final int LED_SPARK_ID = 0; diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 1029754..4f894c9 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -105,7 +105,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(true); + //m_robotContainer.setDriveGearState(true); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 674daa8..501aab2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -25,11 +25,15 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RamseteCommand; import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.commands.CalibrateShooter; import frc4388.robot.commands.DrivePositionMPAux; +import frc4388.robot.commands.DriveStraightAtVelocityPID; +import frc4388.robot.commands.DriveStraightToPositionMM; +import frc4388.robot.commands.DriveStraightToPositionPID; import frc4388.robot.commands.DriveWithJoystick; import frc4388.robot.commands.DriveStraightToPositionMM; import frc4388.robot.commands.DriveStraightToPositionPID; @@ -42,6 +46,7 @@ import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; import frc4388.robot.commands.StorageIntake; +import frc4388.robot.commands.GotoCoordinates; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -67,6 +72,12 @@ import frc4388.robot.commands.StorageRun; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.LimeLight; +import frc4388.robot.commands.TurnDegrees; +import frc4388.robot.commands.Wait; +import frc4388.robot.commands.storageOutake; +import frc4388.robot.subsystems.Camera; +import frc4388.robot.subsystems.Leveler; +import frc4388.robot.subsystems.Pneumatics; import frc4388.robot.subsystems.Storage; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -81,6 +92,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* Subsystems */ private final Drive m_robotDrive = new Drive(); + private final Pneumatics m_robotPneumatics = new Pneumatics(); private final LED m_robotLED = new LED(); private final Intake m_robotIntake = new Intake(); private final Shooter m_robotShooter = new Shooter(); @@ -103,6 +115,10 @@ public class RobotContainer { * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + /* Passing Drive and Pneumatics Subsystems */ + m_robotPneumatics.passRequiredSubsystem(m_robotDrive); + m_robotDrive.passRequiredSubsystem(m_robotPneumatics); + configureButtonBindings(); /* Default Commands */ @@ -134,15 +150,15 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new InstantCommand()); + .whileHeld(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) @@ -151,11 +167,11 @@ public class RobotContainer { /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(false), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); // sets solenoids into low gear new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotDrive.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); /* Operator Buttons */ @@ -225,11 +241,32 @@ public class RobotContainer { RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + + //Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those + //This assumes that we are positioned against the right wall with our shooter facing the target. + return new SequentialCommandGroup(new Wait(0, m_robotDrive), + //add aim command + //add shooter command + //new DriveStraightToPositionMM(m_robotDrive, 48.0), + /*new ParallelCommandGroup( + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DriveStraightToPositionMM(m_robotDrive, 36.0)), + new ParallelCommandGroup( + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DriveStraightToPositionMM(m_robotDrive, 36.0)), + new StorageIntakeGroup(m_robotIntake, m_robotStorage),*/ + //add aim command + //add shooter command +//Below this would be the picking up additional balls outside of those in the trench directly behind us - // return new InstantCommand(); - return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); + //new GotoCoordinates(m_robotDrive, 36, 36), + new GotoCoordinates(m_robotDrive, 36, 36, -90));//, + //new StorageIntakeGroup(m_robotIntake, m_robotStorage), + //new TurnDegrees(m_robotDrive, 75), + //new DriveStraightToPositionMM(m_robotDrive, 18.0), + //new TurnDegrees(m_robotDrive, -45), + //new DriveStraightToPositionMM(m_robotDrive, 12.0)); } - TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND, @@ -281,7 +318,7 @@ public class RobotContainer { * @param state the gearing of the gearbox (true is high, false is low) */ public void setDriveGearState(boolean state) { - m_robotDrive.setShiftState(state); + m_robotPneumatics.setShiftState(state); } /** diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 68d9538..68d8390 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -23,17 +23,18 @@ public class DrivePositionMPAux extends CommandBase { double m_rampAcc; long m_startTime; long m_rampRate; + int m_counter; /** * Creates a new DrivePositionMPAux. * * @param subsystem The drive subsystem - * @param cruiseVel The target velocity for the motors in units + * @param cruiseVel The target velocity for the motors in in/s * @param rampDist The distance before cruise velocity is reached in inches * @param rampRate The time to reach the cruise velocity in seconds * @param targetPos The target position */ - public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos, double targetGyro) { + public DrivePositionMPAux(Drive subsystem, double cruiseVel, double rampDist, float rampRate, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; m_cruiseVel = cruiseVel * DriveConstants.TICKS_PER_INCH_LOW / 10; @@ -54,6 +55,7 @@ public class DrivePositionMPAux extends CommandBase { m_targetVel = m_currentVel; m_startTime = System.currentTimeMillis(); m_rampAcc = (m_cruiseVel - m_currentVel) / m_rampRate; + m_counter = 0; } // Called every m_isRamptime the scheduler runs while the command is scheduled. @@ -64,14 +66,15 @@ public class DrivePositionMPAux extends CommandBase { if (System.currentTimeMillis() - m_startTime < m_rampRate) { // Ramping m_targetVel += m_rampAcc * m_drive.m_deltaTimeMs; - m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); + m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro); } else if (m_targetPos - m_currentPos > m_rampDist) { // Cruising - m_drive.runDriveVelocityPID(-m_cruiseVel, m_targetGyro); + m_drive.runDriveVelocityPID(m_cruiseVel, m_targetGyro); } else { // Deramp PID - m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro); + m_drive.runDrivePositionPID(m_targetPos, m_targetGyro); } + m_counter ++; } // Called once the command ends or is interrupted. @@ -82,8 +85,8 @@ public class DrivePositionMPAux extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_currentPos - m_targetPos <= 0.5f * DriveConstants.TICKS_PER_INCH_LOW) { - return true; + if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) { + //return true; } return false; } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java index 14cc97e..c79ccbc 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightAtVelocityPID.java @@ -37,7 +37,7 @@ public class DriveStraightAtVelocityPID extends CommandBase { @Override public void execute() { //System.err.println(m_drive.m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); - m_drive.runDriveVelocityPID(-m_targetVel, m_targetGyro); + m_drive.runDriveVelocityPID(m_targetVel, m_targetGyro); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java index c56a36b..925f07a 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionMM.java @@ -28,7 +28,7 @@ public class DriveStraightToPositionMM extends CommandBase { public DriveStraightToPositionMM(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } diff --git a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 949a4bf..c31944e 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -27,7 +27,7 @@ public class DriveStraightToPositionPID extends CommandBase { public DriveStraightToPositionPID(Drive subsystem, double targetPos) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW; + m_targetPosIn = targetPos * DriveConstants.TICKS_PER_INCH_LOW * 2; addRequirements(m_drive); //SmartDashboard.putNumber("Distance Target Inches", targetPos); } @@ -58,7 +58,7 @@ public class DriveStraightToPositionPID extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 5){ + if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && i > 10){ return true; } else { i++; diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index f51621a..54bc129 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; import frc4388.utility.controller.IHandController; @@ -48,7 +49,7 @@ public class DriveWithJoystick extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - double cosMultiplier = .55; + double cosMultiplier = 1.0; double deadzone = .1; if (steerInput > 0){ steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; @@ -57,7 +58,24 @@ public class DriveWithJoystick extends CommandBase { } else { steerOutput = 0; } + double tempOutputLimit = 0.8; + boolean isOutputLimited = false; + + if (isOutputLimited) { + if (moveOutput > tempOutputLimit) { + moveOutput = tempOutputLimit; + } else if(moveOutput < -tempOutputLimit) { + moveOutput = -tempOutputLimit; + } + + if (steerOutput > tempOutputLimit) { + steerOutput = tempOutputLimit; + } else if(steerOutput < -tempOutputLimit) { + steerOutput = -tempOutputLimit; + } + } + m_drive.driveWithInput(moveOutput, steerOutput); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java index 12d605c..5a68a9f 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystickUsingDeadAssistPID.java @@ -12,10 +12,12 @@ import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.MathUtil; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.subsystems.Drive; +import frc4388.robot.subsystems.Pneumatics; import frc4388.utility.controller.IHandController; public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { Drive m_drive; + Pneumatics m_pneumatics; double m_targetGyro, m_currentGyro; double m_stopPos; long m_currTime, m_deltaTime; @@ -44,14 +46,15 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { * Creates a new DriveWithJoystickUsingDeadAssistPID to control the drivetrain with an Xbox controller. * Applies a curved ramp to the input from the controllers to make the robot less "touchy". * Also uses PIDs to keep the robot on course when given a "dead" or 0 input. - * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} + * @param subsystemDrive pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} */ - public DriveWithJoystickUsingDeadAssistPID(Drive subsystem, IHandController controller) { + public DriveWithJoystickUsingDeadAssistPID(Drive subsystemDrive, Pneumatics subsystemPneumatics, IHandController controller) { // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; + m_drive = subsystemDrive; + m_pneumatics = subsystemPneumatics; m_controller = controller; addRequirements(m_drive); } @@ -96,7 +99,7 @@ public class DriveWithJoystickUsingDeadAssistPID extends CommandBase { moveOutput = Math.cos(1.571*moveInput)-1; } - if (m_drive.m_isSpeedShiftHigh) { + if (m_pneumatics.m_isSpeedShiftHigh) { runDriveWithInput(moveOutput, steerInput); resetGyroTarget(); } diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java new file mode 100644 index 0000000..5580cfc --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -0,0 +1,67 @@ +/*----------------------------------------------------------------------------*/ +/* 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.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc4388.robot.subsystems.Drive; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html +public class GotoCoordinates extends SequentialCommandGroup { + Drive m_drive; + + double m_xTarget; + double m_yTarget; + double m_currentAngle; + double m_hypotDist; + double m_endAngle; + + /** + * Creates a new GotoPosition. + */ + public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) { + // Add your commands in the super() call, e.g. + // super(new FooCommand(), new BarCommand()); + m_drive = subsystem; + + m_xTarget = xTarget; + m_yTarget = yTarget; + + m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); + + m_currentAngle = calcAngle(); + + SmartDashboard.putNumber("Current Angle Coordinates", m_currentAngle); + + m_endAngle = endAngle; + + + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new DriveStraightToPositionMM(m_drive, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); + } + + public boolean isQuadrantThree() { + if ((m_xTarget < 0) && (m_yTarget < 0)) { + return true; + } else { + return false; + } + } + + public double calcAngle() { + if (isQuadrantThree()) { + return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; + } else { + return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; + } + } + +} diff --git a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java index bb7cef2..5f9dc4a 100644 --- a/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java +++ b/src/main/java/frc4388/robot/commands/RunIntakeWithTriggers.java @@ -41,15 +41,15 @@ public class RunIntakeWithTriggers extends CommandBase { double rightTrigger = m_controller.getRightTriggerAxis(); double leftTrigger = m_controller.getLeftTriggerAxis(); double output = 0; - if (rightTrigger < .5) { - if(rightTrigger > leftTrigger) { - output = rightTrigger; + if (leftTrigger < .5) { + if(leftTrigger > rightTrigger) { + output = leftTrigger; } - if (leftTrigger > rightTrigger) { + if (rightTrigger > leftTrigger) { output = -leftTrigger; } } else { - output = rightTrigger; + output = leftTrigger; } m_intake.runIntake(output); } diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java index 2b47050..83630dd 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java @@ -24,7 +24,7 @@ public class TurnDegrees extends CommandBase { /** * Creates a new TurnDeg. */ - public TurnDegrees(double targetAngle, Drive subsystem) { + public TurnDegrees(Drive subsystem, double targetAngle) { // Use addRequirements() here to declare subsystem dependencies. m_targetAngle = targetAngle; @@ -64,7 +64,7 @@ public class TurnDegrees extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if ((Math.abs(m_drive.getTurnRate()) < 1) && (i > 5)) { + if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) { return true; } return false; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3a87531..096feff 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -36,23 +36,25 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.DriveConstants; +import frc4388.robot.Constants.PneumaticsConstants; import frc4388.robot.Gains; public class Drive extends SubsystemBase { - /* Create Motors, Gyros, Solenoids, etc */ + /* Create Motors, Gyros, etc */ public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID); public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID); public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID); public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID); public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID); - public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1); - public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2); /* Drive objects to manage Drive Train */ public DifferentialDrive m_driveTrain; public final DifferentialDriveOdometry m_odometry; public Orchestra m_orchestra; + /* Pneumatics Subsystem */ + Pneumatics m_pneumaticsSubsystem; + /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW; @@ -86,11 +88,11 @@ public class Drive extends SubsystemBase { public double m_lastAngleYaw = 0; public double m_currentAngleYaw = 0; + public double m_lastAngleGotoCoordinates; /* Smart Dashboard Objects */ SendableChooser m_songChooser = new SendableChooser(); /* Misc */ - public boolean m_isSpeedShiftHigh; String m_currentSong = ""; /** @@ -105,12 +107,6 @@ public class Drive extends SubsystemBase { m_pigeon.configFactoryDefault(); resetGyroYaw(); - /* flip input so forward becomes back, etc */ - m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted); - m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); - m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); - m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); - // m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); /* Config Open Loop Ramp so we don't make sudden output changes */ m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); @@ -119,10 +115,10 @@ public class Drive extends SubsystemBase { m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); /* Config Supply Current Limit (Use only for debugging) */ - m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); - m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); + //m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG); /* Config deadbands so that */ m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS); @@ -251,6 +247,13 @@ public class Drive extends SubsystemBase { /* Set up Orchestra */ m_orchestra = new Orchestra(); + + /* flip input so forward becomes back, etc */ + m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted); + m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted); + m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted); + m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted); + //m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted); /* Set up music for drive train */ m_orchestra.addInstrument(m_leftBackMotor); @@ -273,14 +276,33 @@ public class Drive extends SubsystemBase { @Override public void periodic() { + updateTime(); + updateAngles(); + updatePosition(); + updateSmartDashboard(); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Pneumatics subsystem) { + m_pneumaticsSubsystem = subsystem; + } + + public void updateTime() { m_lastTimeMs = m_currentTimeMs; m_currentTimeMs = System.currentTimeMillis(); m_currentTimeSec = m_currentTimeMs / 1000; m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs; + } + public void updateAngles() { m_lastAngleYaw = m_currentAngleYaw; m_currentAngleYaw = getGyroYaw(); + } + public void updatePosition() { m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition(); m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity(); @@ -295,9 +317,6 @@ public class Drive extends SubsystemBase { m_odometry.update(Rotation2d.fromDegrees( getHeading()), getDistanceInches(m_leftFrontMotor), -getDistanceInches(m_rightFrontMotor)); - - runFalconCooling(); - updateSmartDashboard(); } /** @@ -431,47 +450,6 @@ public class Drive extends SubsystemBase { m_driveTrain.feedWatchdog(); } - /** - * Set to high or low gear based on boolean state, true = high, false = low - * @param state Chooses between high or low gear - */ - public void setShiftState(boolean state) { - if (state == true) { - m_speedShift.set(DoubleSolenoid.Value.kReverse); - } - if (state == false) { - m_speedShift.set(DoubleSolenoid.Value.kForward); - } - setRightMotorGains(state); - m_isSpeedShiftHigh = state; - } - - /** - * Set to open or close solenoid that cools the falcon, true = open, false = close - * @param state Chooses between open and close - */ - public void coolFalcon(boolean state) { - if (state == true) { - m_coolFalcon.set(DoubleSolenoid.Value.kForward); - } - if (state == false) { - m_coolFalcon.set(DoubleSolenoid.Value.kReverse); - } - } - - /** - * - */ - public void runFalconCooling() { - if (m_currentTimeSec % 30 == 0) { - coolFalcon(true); - SmartDashboard.putBoolean("Solenoid", true); - } else if ((m_currentTimeSec - 1) % 30 == 0) { - coolFalcon(false); - SmartDashboard.putBoolean("Solenoid", false); - } - } - /** * Selects a song to play! * @param song The name of the song to be played @@ -624,7 +602,7 @@ public class Drive extends SubsystemBase { * @return The converted value in inches */ public double ticksToInches(double ticks) { - if (m_isSpeedShiftHigh) { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { return ticks * DriveConstants.INCHES_PER_TICK_HIGH; } else { return ticks * DriveConstants.INCHES_PER_TICK_LOW; @@ -637,7 +615,7 @@ public class Drive extends SubsystemBase { * @return The converted value in ticks */ public double inchesToTicks(double inches) { - if (m_isSpeedShiftHigh) { + if (m_pneumaticsSubsystem.m_isSpeedShiftHigh) { return inches * DriveConstants.TICKS_PER_INCH_HIGH; } else { return inches * DriveConstants.TICKS_PER_INCH_LOW; @@ -747,8 +725,14 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Pigeon Roll", getGyroRoll()); SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get()); SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get()); - SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); - SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); + //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); + //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); + + double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity()); + + SmartDashboard.putNumber("Left Motor RPM", leftRPM); + SmartDashboard.putNumber("Right Motor RPM", rightRPM); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -780,9 +764,9 @@ public class Drive extends SubsystemBase { SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent()); //SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY)); - //SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); + SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN)); //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY)); //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN)); @@ -790,7 +774,7 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Odometry Heading", getHeading()); //SmartDashboard.putNumber("Time Seconds", m_currentTimeSec); - //SmartDashboard.putNumber("Delta Time", m_deltaTime); + SmartDashboard.putNumber("Delta Time", m_deltaTimeMs); if (m_currentSong != m_songChooser.getSelected()){ m_currentSong = m_songChooser.getSelected(); diff --git a/src/main/java/frc4388/robot/subsystems/Pneumatics.java b/src/main/java/frc4388/robot/subsystems/Pneumatics.java new file mode 100644 index 0000000..1c373ed --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Pneumatics.java @@ -0,0 +1,89 @@ +/*----------------------------------------------------------------------------*/ +/* 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 edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.RobotContainer; +import frc4388.robot.Constants.PneumaticsConstants; + +public class Pneumatics extends SubsystemBase { + /* Create Solenoids */ + public DoubleSolenoid m_speedShift = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID, + PneumaticsConstants.SPEED_SHIFT_FORWARD_ID, + PneumaticsConstants.SPEED_SHIFT_REVERSE_ID ); + + public DoubleSolenoid m_coolFalcon = new DoubleSolenoid( PneumaticsConstants.PCM_MODULE_ID, + PneumaticsConstants.COOL_FALCON_FORWARD_ID, + PneumaticsConstants.COOL_FALCON_REVERSE_ID ); + + /* Get Drive Subsystem */ + Drive m_driveSubsystem; + + public boolean m_isSpeedShiftHigh; + + /** + * Creates a new Pneumatics. + */ + public Pneumatics() { + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + runFalconCooling(); + } + + /** + * Passes subsystem needed. + * @param subsystem Subsystem needed. + */ + public void passRequiredSubsystem(Drive subsystem) { + m_driveSubsystem = subsystem; + } + + /** + * Set to high or low gear based on boolean state, true = high, false = low + * @param state Chooses between high or low gear + */ + public void setShiftState(boolean state) { + if (state == true) { + m_speedShift.set(DoubleSolenoid.Value.kReverse); + } + if (state == false) { + m_speedShift.set(DoubleSolenoid.Value.kForward); + } + m_driveSubsystem.setRightMotorGains(state); + m_isSpeedShiftHigh = state; + } + + /** + * Set to open or close solenoid that cools the falcon, true = open, false = close + * @param state Chooses between open and close + */ + public void coolFalcon(boolean state) { + if (state == true) { + m_coolFalcon.set(DoubleSolenoid.Value.kForward); + } + if (state == false) { + m_coolFalcon.set(DoubleSolenoid.Value.kReverse); + } + } + + /** + * Runs coolFalcon every 30 seconds for 1 second. + */ + public void runFalconCooling() { + if (m_driveSubsystem.m_currentTimeSec % 30 == 0) { + coolFalcon(true); + } else if ((m_driveSubsystem.m_currentTimeSec - 1) % 30 == 0) { + coolFalcon(false); + } + } +}