diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 89d2b77..dc283e4 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,7 +40,7 @@ public final class Constants { 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); + new SupplyCurrentLimitConfiguration(false, 60, 40, 2); public static final int CLOSED_LOOP_TIME_MS = 1; // Higher numbers mean slower control loops /* Drive Train Characteristics */ @@ -55,8 +55,8 @@ public final class Constants { 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.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); @@ -104,8 +104,8 @@ public final class Constants { } public static final class IntakeConstants { - public static final int INTAKE_SPARK_ID = -9; - public static final int EXTENDER_SPARK_ID = -10; + public static final int INTAKE_SPARK_ID = 12; + public static final int EXTENDER_SPARK_ID = 13; } public static final class ShooterConstants { diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 18dee5d..2fda40a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,7 +30,10 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; 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.GotoCoordinates; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; @@ -114,28 +117,28 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new TurnDegrees(90, m_robotDrive)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); + .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) - .whenPressed(new InstantCommand()); + .whenPressed(new DriveStraightToPositionPID(m_robotDrive, 36)); /* Driver Buttons */ // sets solenoids into high gear new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new InstantCommand(() -> m_robotPneumatics.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_robotPneumatics.setShiftState(true), m_robotDrive)); + .whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); /* Operator Buttons */ //TODO: Shooter Buttons @@ -189,28 +192,28 @@ public class RobotContainer { //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(2, m_robotDrive), + return new SequentialCommandGroup(new Wait(0, m_robotDrive), //add aim command //add shooter command - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0), - new ParallelCommandGroup( + //new DriveStraightToPositionMM(m_robotDrive, 48.0), + /*new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), + new DriveStraightToPositionMM(m_robotDrive, 36.0)), new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), - 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 - new TurnDegrees(-150, m_robotDrive), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0), - new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new TurnDegrees(75, m_robotDrive), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0), - new TurnDegrees(-45, m_robotDrive), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.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( diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index ea76f96..68d8390 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -66,13 +66,13 @@ 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 ++; } 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 01375a4..54bc129 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -39,27 +39,43 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = m_controller.getRightXAxis(); - double steerInput = -m_controller.getLeftYAxis(); + double moveInput = -m_controller.getLeftYAxis(); + double steerInput = m_controller.getRightXAxis(); double moveOutput = 0; double steerOutput = 0; - if (steerInput >= 0){ - steerOutput = -Math.cos(1.571*steerInput)+1; + if (moveInput >= 0){ + moveOutput = -Math.cos(1.571*moveInput)+1; } else { - steerOutput = Math.cos(1.571*steerInput)-1; + moveOutput = Math.cos(1.571*moveInput)-1; } double cosMultiplier = 1.0; double deadzone = .1; - if (moveInput > 0){ - moveOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*moveInput) + cosMultiplier; - } else if (moveInput < 0) { - moveOutput = (cosMultiplier - deadzone) * Math.cos(1.571*moveInput) - cosMultiplier; + if (steerInput > 0){ + steerOutput = -(cosMultiplier - deadzone) * Math.cos(1.571*steerInput) + cosMultiplier; + } else if (steerInput < 0) { + steerOutput = (cosMultiplier - deadzone) * Math.cos(1.571*steerInput) - cosMultiplier; } else { - moveOutput = 0; + 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; + } } - SmartDashboard.putNumber("Steer Output Test", moveOutput); m_drive.driveWithInput(moveOutput, steerOutput); } 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 52eb5dc..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; diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 422d2c7..8c2e2bd 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -88,6 +88,7 @@ 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(); @@ -322,7 +323,7 @@ public class Drive extends SubsystemBase { * using the Differential Drive class to manage the two inputs */ public void driveWithInput(double move, double steer) { - m_driveTrain.arcadeDrive(move, steer); + m_driveTrain.arcadeDrive(steer, move); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } @@ -726,6 +727,12 @@ public class Drive extends SubsystemBase { //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()); //SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());