From 1bad4f73f4f3723678c6f09088c3eb149563a6b8 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Tue, 25 Feb 2020 19:32:43 -0700 Subject: [PATCH 01/19] 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 02/19] 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 553aa6ad942cbca1036d5e347a09db144b1eeadc Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 26 Feb 2020 08:25:39 -0700 Subject: [PATCH 03/19] Rearange Drive Constants --- src/main/java/frc4388/robot/Constants.java | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 3678f33..272cf88 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -36,14 +36,21 @@ public final class Constants { 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; + 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); @@ -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; From 2756df8e1bbc4828b223a41b23718255662756cc Mon Sep 17 00:00:00 2001 From: "Keenan D. Buckley" Date: Wed, 26 Feb 2020 08:41:10 -0700 Subject: [PATCH 04/19] Simplify Periodic to make it easier to read --- src/main/java/frc4388/robot/subsystems/Drive.java | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index fca1e7b..d9f1fb2 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -273,14 +273,26 @@ public class Drive extends SubsystemBase { @Override public void periodic() { + updateTime(); + updateAngles(); + updatePosition(); + runFalconCooling(); + updateSmartDashboard(); + } + + 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 +307,6 @@ public class Drive extends SubsystemBase { m_odometry.update(Rotation2d.fromDegrees( getHeading()), getDistanceInches(m_leftFrontMotor), -getDistanceInches(m_rightFrontMotor)); - - runFalconCooling(); - updateSmartDashboard(); } /** From 20b73e0e31e18bef9f1c771115a03c8f27b926d5 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 26 Feb 2020 16:46:22 -0700 Subject: [PATCH 05/19] Fixed Motion Profile Command, still needs testing --- src/main/java/frc4388/robot/commands/DrivePositionMPAux.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 68d9538..0afb152 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -23,6 +23,7 @@ public class DrivePositionMPAux extends CommandBase { double m_rampAcc; long m_startTime; long m_rampRate; + int m_counter; /** * Creates a new DrivePositionMPAux. @@ -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. @@ -72,6 +74,7 @@ public class DrivePositionMPAux extends CommandBase { // Deramp PID m_drive.runDrivePositionPID(-m_targetPos, m_targetGyro); } + m_counter ++; } // Called once the command ends or is interrupted. @@ -82,7 +85,7 @@ 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) { + if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) { return true; } return false; From 5bcf6c03905b67521ba521fefd7f8b923cb5cb40 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 26 Feb 2020 16:48:35 -0700 Subject: [PATCH 06/19] Deleted problematic import statement --- src/main/java/frc4388/robot/subsystems/Storage.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 3d460fe..b6eb85e 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -19,7 +19,6 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.command.WaitUntilCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Gains; import frc4388.robot.Constants.StorageConstants; From 2e3fea423e502f39d889e356ed46e5d969d1257a Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 26 Feb 2020 17:26:49 -0700 Subject: [PATCH 07/19] Created and Added Solenoid Functionality for Pneumatics Subsystem --- src/main/java/frc4388/robot/Constants.java | 10 +++ .../java/frc4388/robot/RobotContainer.java | 12 ++- .../java/frc4388/robot/subsystems/Drive.java | 63 ++++--------- .../frc4388/robot/subsystems/Pneumatics.java | 89 +++++++++++++++++++ 4 files changed, 123 insertions(+), 51 deletions(-) create mode 100644 src/main/java/frc4388/robot/subsystems/Pneumatics.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 272cf88..4af2056 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -156,6 +156,16 @@ public final class Constants { public static final double STORAGE_MIN_OUTPUT = -1.0; public static final Gains STORAGE_GAINS = new Gains(0.2, 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/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 2156bd0..d2761a7 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -42,6 +42,7 @@ import frc4388.robot.commands.TrackTarget; 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; @@ -56,6 +57,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(); @@ -75,6 +77,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 */ @@ -120,11 +126,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(false), 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(true), m_robotDrive)); /* Operator Buttons */ //TODO: Shooter Buttons @@ -231,7 +237,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/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index d9f1fb2..bbf917b 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; @@ -90,7 +92,6 @@ public class Drive extends SubsystemBase { SendableChooser m_songChooser = new SendableChooser(); /* Misc */ - public boolean m_isSpeedShiftHigh; String m_currentSong = ""; /** @@ -276,10 +277,17 @@ public class Drive extends SubsystemBase { updateTime(); updateAngles(); updatePosition(); - runFalconCooling(); 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(); @@ -440,47 +448,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 @@ -633,7 +600,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; @@ -646,7 +613,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; 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); + } + } +} From afaae3ca1d145c1e03e716dc7b490481fc252791 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 26 Feb 2020 17:36:50 -0700 Subject: [PATCH 08/19] yuh2 --- .../commands/DriveWithJoystickUsingDeadAssistPID.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) 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(); } From 3a3d10a3823425bc2b42a5c5da09a4915e11b80c Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Wed, 26 Feb 2020 20:29:01 -0700 Subject: [PATCH 09/19] Work on Broken PIDs --- src/main/java/frc4388/robot/Constants.java | 4 +-- src/main/java/frc4388/robot/Robot.java | 2 +- .../java/frc4388/robot/RobotContainer.java | 8 +++-- .../robot/commands/DrivePositionMPAux.java | 6 ++-- .../robot/commands/DriveWithJoystick.java | 4 +-- .../frc4388/robot/commands/TurnDegrees.java | 2 +- .../java/frc4388/robot/subsystems/Drive.java | 34 +++++++++---------- 7 files changed, 31 insertions(+), 29 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 4af2056..89d2b77 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -30,7 +30,7 @@ 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; @@ -53,7 +53,7 @@ public final class Constants { /* PID Constants Drive*/ 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; 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 d2761a7..a51873d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -27,6 +27,7 @@ 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.DriveWithJoystick; import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; @@ -39,6 +40,7 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.commands.TrackTarget; +import frc4388.robot.commands.TurnDegrees; import frc4388.robot.commands.storageOutake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; @@ -109,11 +111,11 @@ public class RobotContainer { /* Test Buttons */ // A driver test button new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new TurnDegrees(90, m_robotDrive)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON) - .whenPressed(new InstantCommand()); + .whenPressed(new DriveStraightAtVelocityPID(m_robotDrive, 6000)); // Y driver test button new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON) @@ -183,7 +185,7 @@ public class RobotContainer { //return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return new InstantCommand(); - return new DrivePositionMPAux(m_robotDrive, 500.0, 12.0, 2, 60.0, 0.0); + return new DrivePositionMPAux(m_robotDrive, 12.0, 12.0, 2, 100.0); } TrajectoryConfig getTrajectoryConfig() { diff --git a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java index 0afb152..ea76f96 100644 --- a/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java +++ b/src/main/java/frc4388/robot/commands/DrivePositionMPAux.java @@ -29,12 +29,12 @@ public class DrivePositionMPAux extends CommandBase { * 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; @@ -86,7 +86,7 @@ public class DrivePositionMPAux extends CommandBase { @Override public boolean isFinished() { if (Math.abs((int)m_drive.m_rightFrontMotor.getSelectedSensorVelocity(DriveConstants.PID_PRIMARY)) < 5 && (m_counter > 5)) { - return true; + //return true; } return false; } diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index f51621a..2811fe1 100644 --- a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java +++ b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java @@ -38,8 +38,8 @@ public class DriveWithJoystick extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - double moveInput = -m_controller.getLeftYAxis(); - double steerInput = m_controller.getRightXAxis(); + double moveInput = m_controller.getRightXAxis(); + double steerInput = -m_controller.getLeftYAxis(); double moveOutput = 0; double steerOutput = 0; if (moveInput >= 0){ diff --git a/src/main/java/frc4388/robot/commands/TurnDegrees.java b/src/main/java/frc4388/robot/commands/TurnDegrees.java index 2b47050..52eb5dc 100644 --- a/src/main/java/frc4388/robot/commands/TurnDegrees.java +++ b/src/main/java/frc4388/robot/commands/TurnDegrees.java @@ -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 bbf917b..422d2c7 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -106,13 +106,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); m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS); @@ -120,10 +113,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); @@ -252,6 +245,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); @@ -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(steer, move); + m_driveTrain.arcadeDrive(move, steer); m_leftBackMotor.follow(m_leftFrontMotor); m_rightBackMotor.follow(m_rightFrontMotor); } @@ -723,8 +723,8 @@ 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()); //SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity()); //SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity()); @@ -756,9 +756,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)); @@ -766,7 +766,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(); From 5a7939f5fac3fe846eff3b434534d684114c90eb Mon Sep 17 00:00:00 2001 From: Kyra Rivera <101209@psdschools.org> Date: Thu, 27 Feb 2020 16:27:02 -0700 Subject: [PATCH 10/19] Added Autonomous Command --- .../java/frc4388/robot/RobotContainer.java | 31 +++++++++++++++++-- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d2761a7..9857aa8 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -21,8 +21,10 @@ import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig; import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator; 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.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.*; @@ -39,6 +41,8 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.commands.TrackTarget; +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; @@ -181,11 +185,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(2, m_robotDrive), + //add aim command + //add shooter command + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0, 0.0), + new ParallelCommandGroup( + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0.0)), + new ParallelCommandGroup( + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 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 TurnDegrees(-150, m_robotDrive), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 96.0, 0.0), + new StorageIntakeGroup(m_robotIntake, m_robotStorage), + new TurnDegrees(75, m_robotDrive), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 18.0, 0.0), + new TurnDegrees(-45, m_robotDrive), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0, 0.0)); } - TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( DriveConstants.MAX_SPEED_METERS_PER_SECOND, From 313ae0684ccbc7d5a511045c3ef41215c4875eac Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Thu, 27 Feb 2020 16:37:44 -0700 Subject: [PATCH 11/19] Fixed Errors in RobotContainer --- src/main/java/frc4388/robot/RobotContainer.java | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5b59a6a..18dee5d 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -43,6 +43,7 @@ import frc4388.robot.subsystems.Climber; import frc4388.robot.commands.RunLevelerWithJoystick; import frc4388.robot.commands.TrackTarget; 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; @@ -191,25 +192,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, 0.0), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 120.0), new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0.0)), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0)), new ParallelCommandGroup( new StorageIntakeGroup(m_robotIntake, m_robotStorage), - new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 36.0, 0)), + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 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, 0.0), + 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, 0.0), + 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, 0.0)); + new DrivePositionMPAux(m_robotDrive, 12.0, 10.0, 3, 6.0)); } TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig( From 82286cb180137fcf00fb1df8386027cbad69e47b Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Thu, 27 Feb 2020 17:14:32 -0700 Subject: [PATCH 12/19] Fixed Drive With Joysticks --- .../robot/commands/DriveWithJoystick.java | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/DriveWithJoystick.java b/src/main/java/frc4388/robot/commands/DriveWithJoystick.java index 2811fe1..01375a4 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; @@ -42,22 +43,23 @@ public class DriveWithJoystick extends CommandBase { double steerInput = -m_controller.getLeftYAxis(); double moveOutput = 0; double steerOutput = 0; - if (moveInput >= 0){ - moveOutput = -Math.cos(1.571*moveInput)+1; + if (steerInput >= 0){ + steerOutput = -Math.cos(1.571*steerInput)+1; } else { - moveOutput = Math.cos(1.571*moveInput)-1; + steerOutput = Math.cos(1.571*steerInput)-1; } - double cosMultiplier = .55; + double cosMultiplier = 1.0; double deadzone = .1; - 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; + 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; } else { - steerOutput = 0; + moveOutput = 0; } - + + SmartDashboard.putNumber("Steer Output Test", moveOutput); m_drive.driveWithInput(moveOutput, steerOutput); } From c572182678625990b39ca0dad422de76ba79ab43 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Fri, 28 Feb 2020 17:38:15 -0700 Subject: [PATCH 13/19] 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 14/19] 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 15/19] 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 16/19] 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 17/19] 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 18/19] 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 19/19] 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)