From 1bad4f73f4f3723678c6f09088c3eb149563a6b8 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 25 Feb 2020 19:32:43 -0700 Subject: [PATCH 1/9] Testing and fixing the intake --- src/main/java/frc4388/robot/Constants.java | 4 ++-- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0a5502a..6d194b5 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -75,8 +75,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 f14c5d9..006f0e0 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -154,8 +154,8 @@ public class RobotContainer { .whileHeld(new TrackTarget(m_robotShooter)); //Prepares storage for intaking - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage)); + new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + .whileHeld(new InstantCommand(() -> m_robotIntake.runIntake(0.3), m_robotIntake)); //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) From 4cd54197469b822ed8e06a6eae1d31b89e010f36 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 25 Feb 2020 19:40:05 -0700 Subject: [PATCH 2/9] reversed triggers to fix intake motor direction --- .../frc4388/robot/commands/RunIntakeWithTriggers.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) 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); } From c572182678625990b39ca0dad422de76ba79ab43 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 17:38:15 -0700 Subject: [PATCH 3/9] All low gear PIDs WORKY, test auto command WORKY --- src/main/java/frc4388/robot/Constants.java | 4 +-- .../java/frc4388/robot/RobotContainer.java | 34 ++++++++++--------- .../robot/commands/DrivePositionMPAux.java | 6 ++-- .../commands/DriveStraightAtVelocityPID.java | 2 +- .../commands/DriveStraightToPositionPID.java | 2 +- .../frc4388/robot/commands/TurnDegrees.java | 2 +- 6 files changed, 26 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5fb5bc2..5054477 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -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); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 1b5b04c..4576358 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,6 +30,8 @@ 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.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -114,19 +116,19 @@ 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 DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.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 @@ -192,25 +194,25 @@ public class RobotContainer { return new SequentialCommandGroup(new Wait(2, 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 TurnDegrees(m_robotDrive, -90), + new DriveStraightToPositionMM(m_robotDrive, 96.0));//, + //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/DriveStraightToPositionPID.java b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java index 949a4bf..9d84087 100644 --- a/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java +++ b/src/main/java/frc4388/robot/commands/DriveStraightToPositionPID.java @@ -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/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; From d83d21e062c51f84f943d0c73f3df9da7fdab72e Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 18:11:11 -0700 Subject: [PATCH 4/9] Fixed Drive Train the right way, switched gear buttons for Josh --- .../java/frc4388/robot/RobotContainer.java | 4 +-- .../robot/commands/DriveWithJoystick.java | 28 +++++++++++-------- .../java/frc4388/robot/subsystems/Drive.java | 2 +- 3 files changed, 20 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4576358..20518cc 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -133,11 +133,11 @@ public class RobotContainer { /* 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 diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 01375a4..4cd0d0e 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -39,26 +39,32 @@ 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; } - + + if (moveOutput > 0.5) { + moveOutput = 0.5; + } else if(moveOutput < -0.5) { + moveOutput = -0.5; + } + SmartDashboard.putNumber("Steer Output Test", moveOutput); m_drive.driveWithInput(moveOutput, steerOutput); } diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 422d2c7..f4541dc 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -322,7 +322,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); } From ad50840f7c987c87ee55b67cb0dbe0723c1ad62d Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 19:13:38 -0700 Subject: [PATCH 5/9] Applied 80% Output Limit, Fixed PID 1/2 Distance Problem --- .../robot/commands/DriveStraightToPositionMM.java | 2 +- .../commands/DriveStraightToPositionPID.java | 2 +- .../frc4388/robot/commands/DriveWithJoystick.java | 15 +++++++++++---- 3 files changed, 13 insertions(+), 6 deletions(-) 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 9d84087..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); } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 4cd0d0e..1802f22 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -58,11 +58,18 @@ public class DriveWithJoystick extends CommandBase { } else { steerOutput = 0; } + double tempOutputLimit = 0.8; - if (moveOutput > 0.5) { - moveOutput = 0.5; - } else if(moveOutput < -0.5) { - moveOutput = -0.5; + 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); From a70983a4c2215dc1879218b8b1fd9944f97bf633 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 20:48:22 -0700 Subject: [PATCH 6/9] Removed Output Limits, Added GotoCoordinates Command --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 5 +- .../robot/commands/DriveWithJoystick.java | 25 ++++---- .../robot/commands/GotoCoordinates.java | 61 +++++++++++++++++++ .../java/frc4388/robot/subsystems/Drive.java | 7 +++ 5 files changed, 86 insertions(+), 14 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/GotoCoordinates.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5054477..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 */ diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 20518cc..65f8912 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -33,6 +33,7 @@ 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; @@ -116,7 +117,7 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new DriveStraightToPositionMM(m_robotDrive, -192)); + .whenPressed(new DriveStraightToPositionMM(m_robotDrive, 192)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) @@ -124,7 +125,7 @@ public class RobotContainer { // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new DrivePositionMPAux(m_robotDrive, 12.0, 3.0, 1, 36.0)); + .whenPressed(new GotoCoordinates(m_robotDrive, 12, 40)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 1802f22..54bc129 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -60,19 +60,22 @@ public class DriveWithJoystick extends CommandBase { } double tempOutputLimit = 0.8; - if (moveOutput > tempOutputLimit) { - moveOutput = tempOutputLimit; - } else if(moveOutput < -tempOutputLimit) { - moveOutput = -tempOutputLimit; - } + boolean isOutputLimited = false; - if (steerOutput > tempOutputLimit) { - steerOutput = tempOutputLimit; - } else if(steerOutput < -tempOutputLimit) { - steerOutput = -tempOutputLimit; - } + if (isOutputLimited) { + if (moveOutput > tempOutputLimit) { + moveOutput = tempOutputLimit; + } else if(moveOutput < -tempOutputLimit) { + moveOutput = -tempOutputLimit; + } - SmartDashboard.putNumber("Steer Output Test", moveOutput); + 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/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java new file mode 100644 index 0000000..e836820 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -0,0 +1,61 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.robot.commands; + +import edu.wpi.first.wpilibj2.command.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_lastAngle; + + /** + * Creates a new GotoPosition. + */ + public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) { + // 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(); + + + addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist)); + } + + 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/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index f4541dc..3380fd0 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(); @@ -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 = m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(); + double rightRPM = 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()); From 237eaeb7efcaced6e48cd3478f3c0a29fdca9a17 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 21:25:09 -0700 Subject: [PATCH 7/9] GotoCoordinate Works, Trying to Implement TurnAngle --- src/main/java/frc4388/robot/RobotContainer.java | 10 +++++----- .../java/frc4388/robot/commands/GotoCoordinates.java | 10 ++++++---- src/main/java/frc4388/robot/subsystems/Drive.java | 4 ++-- 3 files changed, 13 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 65f8912..9b3b15d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -125,7 +125,7 @@ public class RobotContainer { // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) - .whenPressed(new GotoCoordinates(m_robotDrive, 12, 40)); + .whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0)); // X driver test button new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON) @@ -192,10 +192,10 @@ 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 DriveStraightToPositionMM(m_robotDrive, 48.0), + //new DriveStraightToPositionMM(m_robotDrive, 48.0), /*new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), new DriveStraightToPositionMM(m_robotDrive, 36.0)), @@ -207,8 +207,8 @@ public class RobotContainer { //add shooter command //Below this would be the picking up additional balls outside of those in the trench directly behind us - new TurnDegrees(m_robotDrive, -90), - new DriveStraightToPositionMM(m_robotDrive, 96.0));//, + //new GotoCoordinates(m_robotDrive, 36, 36), + new GotoCoordinates(m_robotDrive, 36, 36, 0));//, //new StorageIntakeGroup(m_robotIntake, m_robotStorage), //new TurnDegrees(m_robotDrive, 75), //new DriveStraightToPositionMM(m_robotDrive, 18.0), diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index e836820..5ca4c75 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -20,13 +20,12 @@ public class GotoCoordinates extends SequentialCommandGroup { double m_yTarget; double m_currentAngle; double m_hypotDist; - - double m_lastAngle; + double m_endAngle; /** * Creates a new GotoPosition. */ - public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) { + 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; @@ -37,9 +36,12 @@ public class GotoCoordinates extends SequentialCommandGroup { m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); m_currentAngle = calcAngle(); + m_endAngle = endAngle; - addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist)); + addCommands( new TurnDegrees(m_drive, m_currentAngle), + new DriveStraightToPositionMM(m_drive, m_hypotDist), + new TurnDegrees(m_drive, m_endAngle - m_currentAngle - 90)); } public boolean isQuadrantThree() { diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3380fd0..8c2e2bd 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -727,8 +727,8 @@ public class Drive extends SubsystemBase { //SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get()); //SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get()); - double leftRPM = m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity(); - double rightRPM = m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity(); + 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); From 8704cf515f2271bb11fe87a90253d8a67dcff17d Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 21:54:46 -0700 Subject: [PATCH 8/9] Added endAngle to GotoCoordinates --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- src/main/java/frc4388/robot/commands/GotoCoordinates.java | 6 +++++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b3b15d..81f4033 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -208,7 +208,7 @@ public class RobotContainer { //Below this would be the picking up additional balls outside of those in the trench directly behind us //new GotoCoordinates(m_robotDrive, 36, 36), - new GotoCoordinates(m_robotDrive, 36, 36, 0));//, + new GotoCoordinates(m_robotDrive, 36, 36, -90));//, //new StorageIntakeGroup(m_robotIntake, m_robotStorage), //new TurnDegrees(m_robotDrive, 75), //new DriveStraightToPositionMM(m_robotDrive, 18.0), diff --git a/src/main/java/frc4388/robot/commands/GotoCoordinates.java b/src/main/java/frc4388/robot/commands/GotoCoordinates.java index 5ca4c75..5580cfc 100644 --- a/src/main/java/frc4388/robot/commands/GotoCoordinates.java +++ b/src/main/java/frc4388/robot/commands/GotoCoordinates.java @@ -7,6 +7,7 @@ package frc4388.robot.commands; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import frc4388.robot.subsystems.Drive; @@ -36,12 +37,15 @@ public class GotoCoordinates extends SequentialCommandGroup { 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 - 90)); + new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); } public boolean isQuadrantThree() { From 8eddfff30c5e45155fa6c6003e6fc2163c994ae0 Mon Sep 17 00:00:00 2001 From: ryan123rudder <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 29 Feb 2020 09:58:53 -0700 Subject: [PATCH 9/9] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 81f4033..2fda40a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -169,8 +169,8 @@ public class RobotContainer { .whileHeld(new TrackTarget(m_robotShooter)); //Prepares storage for intaking - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whileHeld(new InstantCommand(() -> m_robotIntake.runIntake(0.3), m_robotIntake)); + new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) + .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage)); //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS)