From a4787e0e309c282e4691430d5bf29addc1e608d0 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 20 Jan 2022 18:08:05 -0700 Subject: [PATCH 01/73] stuff happened --- src/main/java/frc4388/robot/Constants.java | 11 ++ .../java/frc4388/robot/RobotContainer.java | 9 +- src/main/java/frc4388/robot/RobotMap.java | 3 + .../frc4388/robot/subsystems/BoomBoom.java | 69 ++++++++ .../java/frc4388/robot/subsystems/Hood.java | 17 ++ .../java/frc4388/robot/subsystems/Turret.java | 17 ++ .../java/frc4388/robot/subsystems/Vision.java | 17 ++ .../java/frc4388/utility/ShooterTables.java | 164 ++++++++++++++++++ 8 files changed, 306 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc4388/robot/subsystems/BoomBoom.java create mode 100644 src/main/java/frc4388/robot/subsystems/Hood.java create mode 100644 src/main/java/frc4388/robot/subsystems/Turret.java create mode 100644 src/main/java/frc4388/robot/subsystems/Vision.java create mode 100644 src/main/java/frc4388/utility/ShooterTables.java diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5581adf..6917401 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -4,6 +4,8 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; + import frc4388.utility.Gains; import frc4388.utility.LEDPatterns; @@ -80,4 +82,13 @@ public final class Constants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; } + public static final class ShooterConstants { + /* PID Constants Shooter */ + public static final int SHOOTER_TIMEOUT_MS = 30; + public static final int SHOOTER_SLOT_IDX = 0; + public static final int SHOOTER_PID_LOOP_IDX = 1; + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + + + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 38c3459..70fd283 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -10,8 +10,12 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Vision; import frc4388.utility.LEDPatterns; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -40,7 +44,10 @@ public class RobotContainer { ); private final LED m_robotLED = new LED(m_robotMap.LEDController); - + private final BoomBoom m_robotBoomBoom = new BoomBoom(); + private final Hood m_robotHood = new Hood(); + private final Turret m_robotTurret = new Turret(); + private final Vision m_robotVison = new Vision(); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 843323c..db9bfd6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -90,6 +90,9 @@ public class RobotMap { //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + /*Boom Boom Subsystem*/ + } + } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java new file mode 100644 index 0000000..e4d8825 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -0,0 +1,69 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.ShooterTables; + +public class BoomBoom extends SubsystemBase { +public WPI_TalonFX m_shooterFalconLeft; +public WPI_TalonFX m_shooterFalconRight; +public ShooterTables m_shooterTable; + + /** Creates a new BoomBoom. */ + public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { + m_shooterFalconLeft = shooterFalconLeft; + m_shooterFalconRight = shooterFalconRight; + + int closedLoopTimeMs = 1; + //LEFT FALCON + m_shooterFalconLeft.configFactoryDefault(); + m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + m_shooterFalconLeft.setInverted(true); + m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + //RIGHT FALCON + m_shooterFalconRight.configFactoryDefault(); + m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); + m_shooterFalconRight.setInverted(false); + m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + m_shooterTable = new ShooterTables(); + + + } + + + @Override + public void periodic() { + // This method will be called once per scheduler run + // Abhi was here + } +public void runDrumShooter(double speed) { + m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); + m_shooterFalconRight.follow(m_shooterFalconLeft); + } + + + public void runDrumShooterVelocityPID(double targetVel) { + m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init + m_shooterFalconRight.follow(m_shooterFalconLeft); + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java new file mode 100644 index 0000000..4cc9392 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Hood extends SubsystemBase { + /** Creates a new Hood. */ + public Hood() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java new file mode 100644 index 0000000..6dcd581 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Turret extends SubsystemBase { + /** Creates a new Turret. */ + public Turret() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java new file mode 100644 index 0000000..afbb9c5 --- /dev/null +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -0,0 +1,17 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.subsystems; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Vision extends SubsystemBase { + /** Creates a new Vision. */ + public Vision() {} + + @Override + public void periodic() { + // This method will be called once per scheduler run + } +} diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java new file mode 100644 index 0000000..2a5ce92 --- /dev/null +++ b/src/main/java/frc4388/utility/ShooterTables.java @@ -0,0 +1,164 @@ +package frc4388.utility; + +import java.io.BufferedReader; +import java.io.File; +import java.io.FileNotFoundException; +import java.io.FileReader; +import java.io.IOException; + +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +/** + * Add your docs here. + */ +public class ShooterTables { + double[][] m_distance = new double[50][4]; + double[][] m_angle = new double[50][2]; + + final int m_columnDistance = 0; + final int m_columnHood = 1; + final int m_columnVel = 2; + final int m_columnCenterDisplacement = 3; + final int m_columnAngle = 0; + final int m_columnDisplacement = 1; + + int m_distanceLength; + int m_angleLength; + + public ShooterTables() { + int lineNum = 0; + + File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv"); + File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv"); + + try { + + + BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV)); + BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV)); + String line = ""; + + while ((line = distanceReader.readLine()) != null) { + + if (lineNum == 0) { + lineNum ++; + } else { + String[] values = line.split(","); + + m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); + m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); + m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); + m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]); + + lineNum ++; + } + + } + + m_distanceLength = lineNum-1; + lineNum = 0; + + while ((line = angleReader.readLine()) != null) { + + if (lineNum == 0) { + lineNum ++; + } else { + String[] values = line.split(","); + + m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]); + m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]); + + lineNum ++; + } + } + + m_angleLength = lineNum-1; + + distanceReader.close(); + angleReader.close(); + + } catch (FileNotFoundException e) { + e.printStackTrace(); + } catch (IOException e) { + e.printStackTrace(); + } + //SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); + //SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); + //SmartDashboard.putNumber("m_distanceLength", m_distanceLength); + //SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); + //SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); + //SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); + } + + public double getHood(double distance) { //Rotations of motor + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnHood]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnHood, m_distance); + } + } + + public double getVelocity(double distance) { //Units per 100ms + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnVel]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnVel, m_distance); + } + } + + public double getCenterDisplacement(double distance) { //Degrees of Lime FOV + int i = 0; + while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { + i ++; + } + if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { + return m_distance[i][m_columnCenterDisplacement]; + } else { + if (i >= m_distanceLength) { + i = m_distanceLength - 1; + } + return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance); + } + } + + + + public double getAngleDisplacement(double angle) { + int i = 0; + while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) { + i ++; + } + if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) { + return m_angle[i][m_columnDisplacement]; + } else { + if (i >= m_angleLength) { + i = m_angleLength - 1; + } + return linearInterpolation(i, angle, m_columnDisplacement, m_angle); + } + } + + public double linearInterpolation(int i, double value, int column, double[][] table) { + if (i != 0) { + double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]); + value = slope * (value - table[i-1][0]) + table[i-1][column]; + return value; + } + return 0.0; + } +} \ No newline at end of file From 39c9fbb593b68fe6760d95ae4bfdd3c78d533a74 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 20 Jan 2022 18:13:07 -0700 Subject: [PATCH 02/73] blob --- src/main/java/frc4388/robot/Constants.java | 2 +- src/main/java/frc4388/robot/RobotMap.java | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6917401..5eda132 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -88,7 +88,7 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); - + } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index db9bfd6..e917689 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,6 +9,7 @@ import com.ctre.phoenix.sensors.CANCoder; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; /** @@ -90,7 +91,14 @@ public class RobotMap { //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - /*Boom Boom Subsystem*/ + } + + //Shooter Config + /*Boom Boom Subsystem*/ + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + void ConfigureShooterMotorControllers() + { } From ed87f4deac9a035b344525e4428e8ea5d2816a27 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 20 Jan 2022 20:00:31 -0700 Subject: [PATCH 03/73] Things Happened --- src/main/java/frc4388/robot/Constants.java | 4 +- src/main/java/frc4388/robot/RobotMap.java | 5 +- .../frc4388/robot/subsystems/BoomBoom.java | 11 +++- .../java/frc4388/robot/subsystems/Hood.java | 61 ++++++++++++++++++- 4 files changed, 75 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5eda132..80a21ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -88,7 +88,9 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); - + public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); + public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); + public static final double SHOOTER_TURRET_MIN = -1.0; } } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index e917689..225a8b4 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -97,9 +97,8 @@ public class RobotMap { /*Boom Boom Subsystem*/ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - void ConfigureShooterMotorControllers() - { - + void ConfigureShooterMotorControllers() { + } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index e4d8825..ceb7569 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -8,6 +8,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; @@ -65,5 +66,13 @@ public void runDrumShooter(double speed) { public void runDrumShooterVelocityPID(double targetVel) { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init m_shooterFalconRight.follow(m_shooterFalconLeft); - } + * // New BoomBoom controller stuff + BangBangController controller = new BangBangController(); + //Controls a motor with the output of the BangBang controller + //Controls a motor with the output of the BangBang conroller and a feedforward + //Shrinks the feedforward slightly to avoid over speeding the shooter + motor.set(controller.calculate(encoder.getRate(), setpoint) + 0.9 * feedforward.calculate(setpoint)); */ + + + } } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 4cc9392..04b2348 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -3,15 +3,74 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; +import com.revrobotics.CANDigitalInput; +import com.revrobotics.CANEncoder; +import com.revrobotics.CANPIDController; +import com.revrobotics.CANSparkMax; +import com.revrobotics.ControlType; +import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; public class Hood extends SubsystemBase { + public Shooter m_shooterSubsystem; + + public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + + public static Gains m_angleGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + + public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + + public boolean m_isHoodReady = false; + /** Creates a new Hood. */ - public Hood() {} + public Hood() { + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + + m_hoodUpLimit = m_angleAdjusterMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodDownLimit = m_angleAdjusterMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_hoodUpLimit.enableLimitSwitch(true); + m_hoodDownLimit.enableLimitSwitch(true); + } + @Override public void periodic() { // This method will be called once per scheduler run } + public void runAngleAdjustPID(double targetAngle) + { + //Set PID Coefficients + m_angleAdjustPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjustPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjustGains.m_IZone); + m_angleAdjusterPIDController.setFF(m_angleAdjustGains.m_kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + + m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + } + + public void runHood(double input) { + m_angleAdjusterMotor.set(input); + } + + public void resetGyroAngleAdj(){ + m_angleEncoder.setPosition(0); + } + + public double getAnglePosition(){ + return m_angleEncoder.getPosition(); + } + + public double getAnglePositionDegrees(){ + return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; + } } From 445cce2ca6595ce4d8dfbe0aa5d369c51a43a148 Mon Sep 17 00:00:00 2001 From: Raghav66296 <90011037+Raghav66296@users.noreply.github.com> Date: Fri, 21 Jan 2022 17:59:12 -0700 Subject: [PATCH 04/73] commets --- src/main/java/frc4388/robot/Constants.java | 3 +++ .../java/frc4388/robot/RobotContainer.java | 4 +++- src/main/java/frc4388/robot/RobotMap.java | 1 + .../java/frc4388/robot/subsystems/Turret.java | 20 ++++++++++++++++++- 4 files changed, 26 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 80a21ee..9c743e6 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -91,6 +91,9 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; + + /* Turret Constants */ + //ID } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 70fd283..de750b6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -66,8 +66,10 @@ public class RobotContainer { // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); - } + + //Turret default command + /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 225a8b4..ac08cff 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -97,6 +97,7 @@ public class RobotMap { /*Boom Boom Subsystem*/ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + //Create motor CANSparkMAx void ConfigureShooterMotorControllers() { } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 6dcd581..f570a34 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -8,10 +8,28 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Turret extends SubsystemBase { /** Creates a new Turret. */ - public Turret() {} + //Motor object + //Variables + public Turret(/*Motor argument*/ */) { + //Motor object = motor argument + //Config motor + } @Override public void periodic() { // This method will be called once per scheduler run } + + //function turnWithJoystick(double input) + // motor.set(input) } + + +/** TODO +* Constants ID +* RobotMap configs +* RobotContainer defaultCommand and Instantiation +* turnWithJoystick function +* setPosition function +* Limit switches +**/ \ No newline at end of file From 2b49f8ed8d9474acbe1717c9971f07df3898d6a6 Mon Sep 17 00:00:00 2001 From: neil92150 <90288985+neil92150@users.noreply.github.com> Date: Fri, 21 Jan 2022 20:56:25 -0700 Subject: [PATCH 05/73] Turret WIP --- src/main/java/frc4388/robot/Constants.java | 21 +++++++++++++++++-- .../java/frc4388/robot/RobotContainer.java | 4 ++++ src/main/java/frc4388/robot/RobotMap.java | 6 +++++- .../java/frc4388/robot/subsystems/Turret.java | 18 +++++++++------- 4 files changed, 38 insertions(+), 11 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 9c743e6..552cb8b 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -88,12 +88,29 @@ public final class Constants { public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); + public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; + public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; + + /* Turret Constants */ + //ID + public static final int TURRET_MOTOR_CAN_ID = 0; public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; + public static final double ENCODER_TRICKS_PER_REV = 0; + public static final double NEO_UNITS_PER_REV = 0; + public static final double DEGREES_PER_ROT = 0; + + + public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(ture,0,0,0); + + public static final int TURRET_RIGHT_SOFT_LIMIT = 0; + public static final int TURRET_LEFT_SOFT_LIMIT = 0; + public static final double TURRET_SPEED_MULTIPLIER = 0.3; + public static final double TURRET_CALIBRATE_SPEED = 0.075; + public static final double TURRET_MOTOR_ROTS_PER_ROT = 1; + public static final double TURRET_MOTOR_POS_AT_ZERO_ROT = 0; - /* Turret Constants */ - //ID } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index de750b6..c117e60 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -68,8 +68,12 @@ public class RobotContainer { m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); + //Turret default command + + m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.aimToCenter())); /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index ac08cff..d713cbe 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -97,9 +97,13 @@ public class RobotMap { /*Boom Boom Subsystem*/ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - //Create motor CANSparkMAx + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.SHOOTER_TURRET_CAN_ID); + //Create motor CANSparkMax void ConfigureShooterMotorControllers() { + /*Turret Subsytem*/ + + } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index f570a34..428fe00 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -8,11 +8,10 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Turret extends SubsystemBase { /** Creates a new Turret. */ - //Motor object + public CANSparkMax m_turretMotor; //Variables - public Turret(/*Motor argument*/ */) { - //Motor object = motor argument - //Config motor + public Turret() { + m_turretMotor = turretMotor; } @Override @@ -20,16 +19,19 @@ public class Turret extends SubsystemBase { // This method will be called once per scheduler run } + public void turnWithJoystick (double input) + { + m_turretMotor.set(input); + + } + //function turnWithJoystick(double input) // motor.set(input) } + /** TODO -* Constants ID -* RobotMap configs -* RobotContainer defaultCommand and Instantiation -* turnWithJoystick function * setPosition function * Limit switches **/ \ No newline at end of file From a1c9f0cef59ce5cb3b1dd868d68a8b74e3734405 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 21 Jan 2022 20:58:13 -0700 Subject: [PATCH 06/73] Finished up shooter (still needs review), started working on Vision --- src/main/java/frc4388/robot/Constants.java | 10 +++- .../frc4388/robot/subsystems/BoomBoom.java | 60 ++++++++++++++++++- .../java/frc4388/robot/subsystems/Vision.java | 36 ++++++++++- 3 files changed, 100 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 80a21ee..95209ee 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -82,6 +82,10 @@ public final class Constants { public static final int XBOX_DRIVER_ID = 0; public static final int XBOX_OPERATOR_ID = 1; } + + + + public static final class ShooterConstants { /* PID Constants Shooter */ public static final int SHOOTER_TIMEOUT_MS = 30; @@ -91,6 +95,10 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; - + public static final int SHOOTER_FALCON_BALLER_ID =; //unknown value, fix later// + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"// + + public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix laterS + } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index ceb7569..6c84711 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -8,16 +8,42 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.SmartDashboard.SmartDashboard; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; +import frc4388.utility.Gains; +import frc4388.utility.Trims; +import frc4388.utility.controller.IHandController; public class BoomBoom extends SubsystemBase { -public WPI_TalonFX m_shooterFalconLeft; -public WPI_TalonFX m_shooterFalconRight; +public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); +public WPI_TalonFX m_shooterFalconRight= new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID); public ShooterTables m_shooterTable; +public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; +public static BoomBoom m_boomBoom; +public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 +double velP; +double input; + +public boolean m_isDrumReady = false; +public double m_fireVel; + +public Trims shooterTrims; + +public Hood m_hoodSubsystem; +public Turret m_turretSubsystem; + +/* +* Creates new BoomBoom subsystem, has drum shooter and angle adjuster +*/ +public BoomBoom(){ +//Testing purposes resetting gyros +//resetGryoAngleADj(); +shooterTrims = new Trims(0,0); +} /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; @@ -56,12 +82,42 @@ public ShooterTables m_shooterTable; public void periodic() { // This method will be called once per scheduler run // Abhi was here +try { + // SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); + + // SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); + + // SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); //all these values should be "defined" elsewhere, fix this + + // SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); + + // SmartDashboard.putNumber("Drum Ready", m_isDrumReady); +} catch (Exception e) { + //TODO: handle exception +} + + } + + public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) { + m_hoodSubsystem = subsystem0; + m_turretSubsystem = subsystem1; + } + + public double addFireVel(){ + return m_fireVel; } public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); m_shooterFalconRight.follow(m_shooterFalconLeft); } +public void setShooterGains() { + m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); + m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); + m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, m_drumShooterGains.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); +} public void runDrumShooterVelocityPID(double targetVel) { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index afbb9c5..f6cc008 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -6,10 +6,40 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; -public class Vision extends SubsystemBase { - /** Creates a new Vision. */ - public Vision() {} + + +public class Vision extends SubsystemBase { +//setup + Turret m_turret; +BoomBoom m_boomBoom; +Hood m_hood; + +NetworkTableEntry xEntry; +IHandController m_driverController; +//Aiming +double turnAmount = 0; +double xAngle = 0; +double yAngle = 0; +double target = 0; +public double distance; +public double realDistance; +public static double fireVel; +public static double fireAngle; + +public double m_hoodTrim; +public double m_turretTrim; + +LimeLight m_limeLight; + +public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { + m_turret = turretSubsystem; + m_boomBoom = m_turret.m_turretSubsystem; + m_hood = m_boomBoom.m_hoodSubsystem; + m_limeLight = limeLight; + addRequirements(m_turret); + +} @Override public void periodic() { // This method will be called once per scheduler run From 632fc12ad0e5f9499d7b8678094f1cc8b014aed3 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Sat, 22 Jan 2022 11:00:53 -0700 Subject: [PATCH 07/73] finished vision code, still needs a few improvements --- src/main/java/frc4388/robot/Constants.java | 19 ++- .../java/frc4388/robot/subsystems/Vision.java | 108 ++++++++++++++++-- 2 files changed, 117 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 95209ee..b085102 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -98,7 +98,24 @@ public final class Constants { public static final int SHOOTER_FALCON_BALLER_ID =; //unknown value, fix later// public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"// - public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix laterS + public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix later + + + public static final class VisionConstants { + public static final double TURN_P_VALUE = "unknown" // needs to be figured out after testing + public static final double X_ANGLE_ERROR = "alsoUnknown" //""// + public static final double TURN_P_VALUE = "alsoAlsoUnknown" //""// + public static final double GRAV = "alsoAlsoAlsoUnknown" //""// + public static final double TARGET_HEIGHT = "alsoAlsoAlsoAlsoUnknown" //""// + + + + + + + + + } } } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index f6cc008..76ba03d 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -5,15 +5,20 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; - - - +import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.networktables.NetworkTableEntry; +import edu.wpi.first.wpilibj2.command.commandBase; +import frc4388.robot.constants.VisionConstants; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Turret; +import frc4388.robot.subsystems.Hood; +import frc4388.utility.controller.IHandController; public class Vision extends SubsystemBase { //setup Turret m_turret; -BoomBoom m_boomBoom; -Hood m_hood; + BoomBoom m_boomBoom; + Hood m_hood; NetworkTableEntry xEntry; IHandController m_driverController; @@ -38,10 +43,95 @@ public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { m_hood = m_boomBoom.m_hoodSubsystem; m_limeLight = limeLight; addRequirements(m_turret); - + limeOff(); + changePipeline(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3); } - @Override - public void periodic() { - // This method will be called once per scheduler run + +public void track(){ + target = getV() + xAngle = getX() + yAngle = getY() + + //find distance + distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); + realDistance = (1.09 * distance) - 12.8; + + if (target == 1.0) { //checks if target is in view + //aims left and right + turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); + if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + turnAmount = 0; + } + else if (turnAmount > 0 && turnAmount < 0.1){ + turnAmount = 0.1; + } + else if (turnAmount < 0 && turnAmount > -0.1){ + turnAmount = -0.1 + } + } + m_turret.runShooterWithInput(-turnAmount); + + SmartDashboard.putNumber("Disance to Target", realDistance); + + + //start CSV + + fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance); + fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance); + //fire angle unknown so far + //end of CSV + + m_boomBoom.m_fireVel = fireVel; + m_hood.m_fireAngle = fireAngle; + m_turret.m_targetDistance = distance; + + checkFinished(); +} + +public void checkFinished(){ + if (xAngle < 0.5 && xAngle > -0.5 && target == 1){ + m_turret.m_isAimReady = true; + } + else{ + m_turret.m_isAimReady = false; } } + +public void limeOff(){ + NetworkTableInstance.getDefault.getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); +} + + public void limeOn(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); +} + + public void changePipeline(int pipelineId) + { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); + } + + public double getV() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); + } + + public double getX() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); + } + + public double getY() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + } + @Override + public void periodic(){ + //called once per scheduler run + } +} + + From 2d66d991b0cc44c852ad67a6b61b956019801199 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Mon, 24 Jan 2022 17:19:59 -0700 Subject: [PATCH 08/73] Update Constants.java --- src/main/java/frc4388/robot/Constants.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 52b1ddd..d499143 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,6 +94,8 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; + public static final int SHOOTER_ROTATE_ID = "unknown value" //figure out later + public static final int SHOOTER_TURRET_GAINS = "'" //""// /* Turret Constants */ //ID From 304c78382920bf99dde0f2763fe7feb4d7eb8923 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Mon, 24 Jan 2022 18:48:03 -0700 Subject: [PATCH 09/73] some debuging subsystems --- src/main/java/frc4388/robot/Constants.java | 21 ++++++++------- .../frc4388/robot/subsystems/BoomBoom.java | 3 +-- .../java/frc4388/robot/subsystems/Vision.java | 26 +++++++++---------- 3 files changed, 25 insertions(+), 25 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 52b1ddd..3f0dfef 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -106,14 +106,19 @@ public final class Constants { public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"// public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix later + /* Hood Constants */ + public static final int SHOOTER_ANGLE_ADJUST_ID = 10; + public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find + public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find - - public static final class VisionConstants { - public static final double TURN_P_VALUE = "unknown" // needs to be figured out after testing - public static final double X_ANGLE_ERROR = "alsoUnknown" //""// - public static final double TURN_P_VALUE = "alsoAlsoUnknown" //""// - public static final double GRAV = "alsoAlsoAlsoUnknown" //""// - public static final double TARGET_HEIGHT = "alsoAlsoAlsoAlsoUnknown" //""// + } + public static final class VisionConstants { + public static final double TURN_P_VALUE = 0.8; + public static final double X_ANGLE_ERROR = 0.5; + public static final double GRAV = 385.83; + public static final double TARGET_HEIGHT = 67.5; + public static final double FOV = 29.8; //Field of view limelight + public static final double LIME_ANGLE = 24.7; @@ -123,6 +128,4 @@ public final class Constants { } - - } } diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6c84711..8a7a485 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -14,7 +14,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; import frc4388.utility.Gains; -import frc4388.utility.Trims; import frc4388.utility.controller.IHandController; public class BoomBoom extends SubsystemBase { @@ -127,7 +126,7 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - motor.set(controller.calculate(encoder.getRate(), setpoint) + 0.9 * feedforward.calculate(setpoint)); */ + m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); */ } diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 76ba03d..3785439 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -6,9 +6,10 @@ package frc4388.robot.subsystems; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.networktables.NetworkTableInstance; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj2.command.commandBase; -import frc4388.robot.constants.VisionConstants; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Turret; import frc4388.robot.subsystems.Hood; @@ -35,14 +36,11 @@ public static double fireAngle; public double m_hoodTrim; public double m_turretTrim; -LimeLight m_limeLight; - -public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { - m_turret = turretSubsystem; - m_boomBoom = m_turret.m_turretSubsystem; +public Vision(Turret aimSubsystem, BoomBoom boomBoom) { + m_turret = aimSubsystem; + m_boomBoom = boomBoom; m_hood = m_boomBoom.m_hoodSubsystem; - m_limeLight = limeLight; - addRequirements(m_turret); + //addRequirements(m_turret); limeOff(); changePipeline(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); @@ -50,9 +48,9 @@ public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { } public void track(){ - target = getV() - xAngle = getX() - yAngle = getY() + target = getV(); + xAngle = getX(); + yAngle = getY(); //find distance distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); @@ -68,10 +66,10 @@ public void track(){ turnAmount = 0.1; } else if (turnAmount < 0 && turnAmount > -0.1){ - turnAmount = -0.1 + turnAmount = -0.1; } } - m_turret.runShooterWithInput(-turnAmount); + m_turret.turnWithJoystick(-turnAmount); SmartDashboard.putNumber("Disance to Target", realDistance); From 1d804903d8ec18d1573bf5309af9d1867f1b9f67 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Mon, 24 Jan 2022 19:55:08 -0700 Subject: [PATCH 10/73] worked on turret --- src/main/java/frc4388/robot/Constants.java | 16 ++-- .../java/frc4388/robot/subsystems/Turret.java | 81 ++++++++++++++++++- .../java/frc4388/robot/subsystems/Vision.java | 2 +- 3 files changed, 91 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 118740a..2bb8bb7 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,8 +94,14 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; - public static final int SHOOTER_ROTATE_ID = "unknown value" //figure out later - public static final int SHOOTER_TURRET_GAINS = "'" //""// + public static final int SHOOTER_ROTATE_ID = 0; //"unknown value" //figure out later + public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// + public static final int TURRET_SPEED_MULTIPLIER = 0; //""// + public static final int DEGREES_PER_ROT = 0; //""// + public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// + public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// + + } /* Turret Constants */ //ID @@ -104,10 +110,10 @@ public final class Constants { public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final int SHOOTER_FALCON_BALLER_ID =; //unknown value, fix later// - public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID =; //"// + public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// + public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// - public static final Gains DRUM_SHOOTER_GAINS = new Gains(x,y,z,a,b);//x,y,z,a,b are not actual values, fix later + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0);//x,y,z,a,b are not actual values, fix later /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 10; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 428fe00..48a6556 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -4,21 +4,98 @@ package frc4388.robot.subsystems; +import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.Constants.ShooterConstants; +import frc4388.utility.Gains; public class Turret extends SubsystemBase { /** Creates a new Turret. */ - public CANSparkMax m_turretMotor; + public BoomBoom m_boomBoomSubsystem; + public SwerveDrive m_sDriveSubsystem; + + public CAnsparkMax m_boomBoomRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); + public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; + CANDigitalInput m_boomBoomRightLimit, m_boomBoomLeftLimit; + public GyroBase m_turretGyro; + + public double m_targetDistance = 0; + + public boolean m_isAimReady = false; + + CANPIDController m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + public CANEncoder m_boomBoomRotateEncoder = m_shooterRotateMotor.getEncoder(); + + public + public CANSparkMax m_turretMotor; + public boolean m_isAimReady; + public double m_targetDistance; //Variables - public Turret() { + public Turret() { + + m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); + + m_turretGyro = getGyroInterface(); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); + + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); + + m_boomBoomRotateMotor.setInverted(false); m_turretMotor = turretMotor; } @Override public void periodic() { + SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); + + SmartDashboard.putData("Turret Angle", m_turretGyro); + + SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } + public void passRequiredSubsystem( BoomBoom subsystem0, SwerveDrive subsystem1){ + m_boomBoomSubsystem = subsystem0; + m_sDriveSubsystem = subsystem1; + } + + public void runShooterWithInput(double input) { + m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); + } + + public void runshooterRotatePID(double targetAngle) { + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + + m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); + } + + public void resetGyroShooterRotate() + { + m_boomBoomRotateEncoder.setPosition(0); + } + + public double getboomBoomRotatePosition() + { + return m_boomBoomRotateEncoder.getPosition(); + } + + public double getAnglePositionDegrees() { + return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; + } + public void turnWithJoystick (double input) { m_turretMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 3785439..85ee3e6 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -98,7 +98,7 @@ public void checkFinished(){ } public void limeOff(){ - NetworkTableInstance.getDefault.getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); } From 93a72b6b87481ac469cac71f2ee9182439a94aee Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 25 Jan 2022 18:57:28 -0700 Subject: [PATCH 11/73] debugged things --- .../frc4388/robot/subsystems/BoomBoom.java | 4 +- .../java/frc4388/robot/subsystems/Hood.java | 3 +- vendordeps/REVLib.json | 73 +++++++++++++++++++ 3 files changed, 76 insertions(+), 4 deletions(-) create mode 100644 vendordeps/REVLib.json diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 8a7a485..1e51ef1 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -8,7 +8,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import edu.wpi.first.wpilibj.SmartDashboard.SmartDashboard; +import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; @@ -121,7 +121,7 @@ public void setShooterGains() { public void runDrumShooterVelocityPID(double targetVel) { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init m_shooterFalconRight.follow(m_shooterFalconLeft); - * // New BoomBoom controller stuff + // New BoomBoom controller stuff BangBangController controller = new BangBangController(); //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 04b2348..19ab8f3 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -3,7 +3,6 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; -import com.revrobotics.CANDigitalInput; import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; @@ -19,7 +18,7 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; public class Hood extends SubsystemBase { - public Shooter m_shooterSubsystem; + public BoomBoom m_shooterSubsystem; public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..fb19ccf --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,73 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2022.1.1", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2022.1.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2022.1.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2022.1.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxaarch64bionic", + "linuxx86-64", + "linuxathena", + "linuxraspbian", + "osxx86-64" + ] + } + ] +} From f9142d40d84810991c6b29fd2b7eb2e29c6f6f63 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Tue, 25 Jan 2022 19:37:37 -0700 Subject: [PATCH 12/73] fixed a few bugs --- src/main/java/frc4388/robot/Constants.java | 4 +- .../frc4388/robot/subsystems/BoomBoom.java | 2 +- .../java/frc4388/robot/subsystems/Turret.java | 50 ++++++++++++------- 3 files changed, 34 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 2bb8bb7..a0cef9f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -101,7 +101,7 @@ public final class Constants { public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// - } + /* Turret Constants */ //ID @@ -113,7 +113,7 @@ public final class Constants { public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0);//x,y,z,a,b are not actual values, fix later + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0);//x,y,z,a,b are not actual values, fix later /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 10; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 1e51ef1..1637844 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -41,7 +41,7 @@ public Turret m_turretSubsystem; public BoomBoom(){ //Testing purposes resetting gyros //resetGryoAngleADj(); -shooterTrims = new Trims(0,0); + } /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 48a6556..de51bf0 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -4,57 +4,69 @@ package frc4388.robot.subsystems; +import java.lang.ModuleLayer.Controller; + +import javax.naming.ldap.Control; + +import com.ctre.phoenix.sensors.CANCoder; + +import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; +import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; -public class Turret extends SubsystemBase { +public class Turret extends SubsystemBase { + private static final String turretMotor = null; /** Creates a new Turret. */ public BoomBoom m_boomBoomSubsystem; public SwerveDrive m_sDriveSubsystem; - public CAnsparkMax m_boomBoomRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); + public CANSparkMax m_boomBoomRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; CANDigitalInput m_boomBoomRightLimit, m_boomBoomLeftLimit; - public GyroBase m_turretGyro; + public Gyro m_turretGyro; public double m_targetDistance = 0; public boolean m_isAimReady = false; - CANPIDController m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); - public CANEncoder m_boomBoomRotateEncoder = m_shooterRotateMotor.getEncoder(); - - public - public CANSparkMax m_turretMotor; - public boolean m_isAimReady; - public double m_targetDistance; + Controller m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + public CANCoder m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + + //Variables - public Turret() { + public Turret() { + Object IdleMode; m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - + boolean enableLimitSwitch = true; m_turretGyro = getGyroInterface(); m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_boomBoomRightLimit.enableLimitSwitch(true); - m_boomBoomLeftLimit.enableLimitSwitch(true); + m_boomBoomRightLimit.enableLimitSwitch; + m_boomBoomLeftLimit.enableLimitSwitch; m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); + boolean setInverted = false; + m_boomBoomRotateMotor.setInverted; + Object turretMotor; + Object m_turretMotor = turretMotor; + } - m_boomBoomRotateMotor.setInverted(false); - m_turretMotor = turretMotor; + private Gyro getGyroInterface() { + return null; } @Override public void periodic() { SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); - SmartDashboard.putData("Turret Angle", m_turretGyro); + SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run @@ -73,7 +85,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setF(m_shooterTGains.m_kF); m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); @@ -98,7 +110,7 @@ public class Turret extends SubsystemBase { public void turnWithJoystick (double input) { - m_turretMotor.set(input); + turretMotor.set(input); } From 9ab312f84a7da4caaefb812e6a1b43f9f38ff1fa Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Tue, 25 Jan 2022 19:56:18 -0700 Subject: [PATCH 13/73] fixed a few bugs --- src/main/java/frc4388/robot/subsystems/BoomBoom.java | 6 ++++-- src/main/java/frc4388/robot/subsystems/Turret.java | 7 ++++--- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 1637844..6879b06 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import java.util.Base64.Encoder; + import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -30,7 +32,7 @@ double input; public boolean m_isDrumReady = false; public double m_fireVel; -public Trims shooterTrims; + public Hood m_hoodSubsystem; public Turret m_turretSubsystem; @@ -126,7 +128,7 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); */ + m_shooterFalconLeft.set(controller.calculate(Encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index de51bf0..eba7f23 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -9,6 +9,7 @@ import java.lang.ModuleLayer.Controller; import javax.naming.ldap.Control; import com.ctre.phoenix.sensors.CANCoder; +import com.revrobotics.CANSparkMax.IdleMode; import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; @@ -18,7 +19,7 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; -public class Turret extends SubsystemBase { +public class Turret extends SubsystemBase { private static final String turretMotor = null; /** Creates a new Turret. */ public BoomBoom m_boomBoomSubsystem; @@ -38,9 +39,9 @@ public class Turret extends SubsystemBase { //Variables - public Turret() { + public Turret() { - Object IdleMode; + m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); boolean enableLimitSwitch = true; m_turretGyro = getGyroInterface(); From b430549b3f806261ed3c279397274e7fe9bbaeea Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 25 Jan 2022 19:56:32 -0700 Subject: [PATCH 14/73] Trying to debug pushkar's errors --- src/main/java/frc4388/robot/subsystems/BoomBoom.java | 8 +++----- src/main/java/frc4388/robot/subsystems/Hood.java | 9 +++++++-- src/main/java/frc4388/robot/subsystems/Vision.java | 3 ++- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 1637844..fc6d775 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -29,9 +29,7 @@ double input; public boolean m_isDrumReady = false; public double m_fireVel; - -public Trims shooterTrims; - +//public Trims shooterTrims; public Hood m_hoodSubsystem; public Turret m_turretSubsystem; @@ -41,7 +39,7 @@ public Turret m_turretSubsystem; public BoomBoom(){ //Testing purposes resetting gyros //resetGryoAngleADj(); - +//shooterTrims = new Trims(0,0); } /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { @@ -126,7 +124,7 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); */ + m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel));; } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 19ab8f3..3969d4f 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -3,10 +3,12 @@ // the WPILib BSD license file in the root directory of this project. package frc4388.robot.subsystems; + import com.revrobotics.CANEncoder; import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; import com.revrobotics.ControlType; +import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; @@ -23,11 +25,13 @@ public class Hood extends SubsystemBase { public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public static Gains m_angleGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - - public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); + public SparkMaxPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); + public boolean m_isHoodReady = false; + +public double m_fireAngle; /** Creates a new Hood. */ public Hood() { @@ -56,6 +60,7 @@ public class Hood extends SubsystemBase { m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); } + public void runHood(double input) { m_angleAdjusterMotor.set(input); diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 85ee3e6..f813959 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -9,6 +9,7 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.networktables.NetworkTableEntry; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Turret; @@ -35,7 +36,7 @@ public static double fireAngle; public double m_hoodTrim; public double m_turretTrim; - +public double m_fireAngle; public Vision(Turret aimSubsystem, BoomBoom boomBoom) { m_turret = aimSubsystem; m_boomBoom = boomBoom; From 0d9847133970aec4c5d1d4fe26f381bc4c085687 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Wed, 26 Jan 2022 16:52:20 -0700 Subject: [PATCH 15/73] Finished Debugging Hood --- .../java/frc4388/robot/subsystems/Hood.java | 44 ++++++++++--------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 3969d4f..de78848 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -4,12 +4,11 @@ package frc4388.robot.subsystems; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; +import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; @@ -22,25 +21,28 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public SparkMaxLimitSwitch m_hoodUpLimitSwitch; + public SparkMaxLimitSwitch m_hoodDownLimitSwitch; + public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); - public static Gains m_angleGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); - - public SparkMaxPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - - public boolean m_isHoodReady = false; + + //public boolean m_isHoodReady = false; public double m_fireAngle; + /** Creates a new Hood. */ public Hood() { - m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - m_hoodUpLimit = m_angleAdjusterMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodDownLimit = m_angleAdjusterMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodUpLimit.enableLimitSwitch(true); - m_hoodDownLimit.enableLimitSwitch(true); + m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); } @@ -51,14 +53,14 @@ public double m_fireAngle; public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients - m_angleAdjustPIDController.setP(m_angleAdjusterGains.m_kP); - m_angleAdjustPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); - m_angleAdjusterPIDController.setIZone(m_angleAdjustGains.m_IZone); - m_angleAdjusterPIDController.setFF(m_angleAdjustGains.m_kF); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } From 64e706cc17a7665a1c1dccb20aabef720d94c604 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Wed, 26 Jan 2022 16:53:05 -0700 Subject: [PATCH 16/73] turret is debugged --- .../java/frc4388/robot/subsystems/Turret.java | 48 +++++++++---------- .../java/frc4388/robot/subsystems/Vision.java | 3 +- 2 files changed, 24 insertions(+), 27 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index eba7f23..0202c6d 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -4,38 +4,42 @@ package frc4388.robot.subsystems; -import java.lang.ModuleLayer.Controller; -import javax.naming.ldap.Control; - -import com.ctre.phoenix.sensors.CANCoder; +import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; - +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; +import com.revrobotics.SparkMaxLimitSwitch; +import com.revrobotics.SparkMaxPIDController; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.wpilibj.drive.RobotDriveBase.MotorType; import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; +import com.revrobotics.CANSparkMax.SoftLimitDirection; -public class Turret extends SubsystemBase { - private static final String turretMotor = null; + + + +public class Turret extends SubsystemBase { + /** Creates a new Turret. */ public BoomBoom m_boomBoomSubsystem; public SwerveDrive m_sDriveSubsystem; public CANSparkMax m_boomBoomRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; - CANDigitalInput m_boomBoomRightLimit, m_boomBoomLeftLimit; + SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; public Gyro m_turretGyro; public double m_targetDistance = 0; public boolean m_isAimReady = false; - Controller m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); - public CANCoder m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + SparkMaxPIDController m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + public RelativeEncoder m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); //Variables @@ -43,20 +47,18 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - boolean enableLimitSwitch = true; + m_turretGyro = getGyroInterface(); - m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_boomBoomRightLimit.enableLimitSwitch; - m_boomBoomLeftLimit.enableLimitSwitch; + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_boomBoomRightLimit.enableLimitSwitch(true); + m_boomBoomLeftLimit.enableLimitSwitch(true); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); - boolean setInverted = false; - m_boomBoomRotateMotor.setInverted; - Object turretMotor; - Object m_turretMotor = turretMotor; + m_boomBoomRotateMotor.setInverted(false); + } private Gyro getGyroInterface() { @@ -86,7 +88,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); @@ -109,11 +111,7 @@ public class Turret extends SubsystemBase { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; } - public void turnWithJoystick (double input) - { - turretMotor.set(input); - } //function turnWithJoystick(double input) // motor.set(input) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 85ee3e6..604bdae 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -69,9 +69,8 @@ public void track(){ turnAmount = -0.1; } } - m_turret.turnWithJoystick(-turnAmount); - SmartDashboard.putNumber("Disance to Target", realDistance); + SmartDashboard.putNumber("Distance to Target", realDistance); //start CSV From 7f802f855e99192b95cc31de2d597d239236cf4f Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Thu, 27 Jan 2022 18:43:54 -0700 Subject: [PATCH 17/73] deleted a bunch --- .../java/frc4388/robot/subsystems/Vision.java | 36 ------------------- 1 file changed, 36 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 4a1e119..ffe41fa 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -97,39 +97,3 @@ public void checkFinished(){ } } -public void limeOff(){ - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); -} - - public void limeOn(){ - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); -} - - public void changePipeline(int pipelineId) - { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); - } - - public double getV() - { - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); - } - - public double getX() - { - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); - } - - public double getY() - { - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - } - @Override - public void periodic(){ - //called once per scheduler run - } -} - - From bc17a10a8e3c1e611fca93427f3d7dac74badb5a Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 27 Jan 2022 18:45:05 -0700 Subject: [PATCH 18/73] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c117e60..67a7414 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -73,7 +73,7 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.aimToCenter())); + new RunCommand(() -> m_robotTurret.aimToCenter()));{} /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses From 1e44637ae76a5349b170b6218dcbe8863edbedb2 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Thu, 27 Jan 2022 18:46:36 -0700 Subject: [PATCH 19/73] Revert "deleted a bunch" This reverts commit 7f802f855e99192b95cc31de2d597d239236cf4f. --- .../java/frc4388/robot/subsystems/Vision.java | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index ffe41fa..4a1e119 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -97,3 +97,39 @@ public void checkFinished(){ } } +public void limeOff(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); +} + + public void limeOn(){ + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); +} + + public void changePipeline(int pipelineId) + { + NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); + } + + public double getV() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); + } + + public double getX() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); + } + + public double getY() + { + return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); + } + @Override + public void periodic(){ + //called once per scheduler run + } +} + + From b786e10610f913c6ffa4bdae1d6ff38bfce42a3a Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Thu, 27 Jan 2022 18:48:27 -0700 Subject: [PATCH 20/73] unnecessary stuff commented out --- .../java/frc4388/robot/subsystems/Vision.java | 44 +++++++++---------- 1 file changed, 22 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 4a1e119..310d70f 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -57,35 +57,35 @@ public void track(){ distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); realDistance = (1.09 * distance) - 12.8; - if (target == 1.0) { //checks if target is in view - //aims left and right - turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); - if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { - turnAmount = 0; - } - else if (turnAmount > 0 && turnAmount < 0.1){ - turnAmount = 0.1; - } - else if (turnAmount < 0 && turnAmount > -0.1){ - turnAmount = -0.1; - } - } + // if (target == 1.0) { //checks if target is in view + // //aims left and right + // turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); + // if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { + // turnAmount = 0; + // } + // else if (turnAmount > 0 && turnAmount < 0.1){ + // turnAmount = 0.1; + // } + // else if (turnAmount < 0 && turnAmount > -0.1){ + // turnAmount = -0.1; + // } + // } SmartDashboard.putNumber("Distance to Target", realDistance); - //start CSV + // //start CSV - fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance); - fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance); - //fire angle unknown so far - //end of CSV + // fireVel = m_boomBoom.m_shooterTable.getVelocity(realDistance); + // fireAngle = m_boomBoom.m_shooterTable.getHood(realDistance); + // //fire angle unknown so far + // //end of CSV - m_boomBoom.m_fireVel = fireVel; - m_hood.m_fireAngle = fireAngle; - m_turret.m_targetDistance = distance; + // m_boomBoom.m_fireVel = fireVel; + // m_hood.m_fireAngle = fireAngle; + // m_turret.m_targetDistance = distance; - checkFinished(); + // checkFinished(); } public void checkFinished(){ From 9cfaa4011bd3ee699471407282a49dbbaa2fa926 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Thu, 27 Jan 2022 19:04:37 -0700 Subject: [PATCH 21/73] trying out for testboard --- src/main/java/frc4388/robot/RobotContainer.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 67a7414..d72c42a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -47,7 +47,7 @@ public class RobotContainer { private final BoomBoom m_robotBoomBoom = new BoomBoom(); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(); - private final Vision m_robotVison = new Vision(); + private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); @@ -70,10 +70,14 @@ public class RobotContainer { //Turret default command + + m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret) + ); m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.aimToCenter()));{} + new RunCommand(() -> m_robotTurret.aimToCenter())); /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses From 26e54c1db73eaa11d0f97fd60f5c63439739bc96 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 27 Jan 2022 19:13:00 -0700 Subject: [PATCH 22/73] updated gradlerio version --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index c1f07ee..7e60634 100644 --- a/build.gradle +++ b/build.gradle @@ -2,7 +2,7 @@ import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2022.1.1" + id "edu.wpi.first.GradleRIO" version "2022.2.1" } sourceCompatibility = JavaVersion.VERSION_11 From 6369870590e39874395d2a4f3aec856b74895821 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 17:05:28 -0700 Subject: [PATCH 23/73] fixed fedforward bug, or at least it looks like it --- src/main/java/frc4388/robot/subsystems/BoomBoom.java | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 531ffb3..6b745da 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -12,11 +12,13 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; import frc4388.utility.Gains; import frc4388.utility.controller.IHandController; +import com.revrobotics.RelativeEncoder; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); @@ -35,6 +37,12 @@ public double m_fireVel; public Hood m_hoodSubsystem; public Turret m_turretSubsystem; + + +SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later + + + /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ @@ -42,6 +50,9 @@ public BoomBoom(){ //Testing purposes resetting gyros //resetGryoAngleADj(); //shooterTrims = new Trims(0,0); +feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); + + } /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { @@ -60,6 +71,7 @@ public BoomBoom(){ m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + //RIGHT FALCON m_shooterFalconRight.configFactoryDefault(); m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); From d2694e2b61dc1b692107f1f0d9e49049cc9a1879 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 17:08:13 -0700 Subject: [PATCH 24/73] haw --- src/main/java/frc4388/robot/subsystems/BoomBoom.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 6b745da..f2809c2 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -19,6 +19,7 @@ import frc4388.utility.ShooterTables; import frc4388.utility.Gains; import frc4388.utility.controller.IHandController; import com.revrobotics.RelativeEncoder; +import com.revrobotics.*; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); From 4e497acab6fa83da65980a8159a81d49e7e9362a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 28 Jan 2022 17:12:28 -0700 Subject: [PATCH 25/73] fixed bugs (still not working -_-) --- simgui-ds.json | 101 ++++++++++++++++++ simgui.json | 34 ++++++ src/main/java/frc4388/robot/Constants.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 8 +- src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/BoomBoom.java | 11 +- .../java/frc4388/robot/subsystems/Turret.java | 6 +- 7 files changed, 154 insertions(+), 16 deletions(-) create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..b16ea5c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,101 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "useGamepad": true + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..72aab8f --- /dev/null +++ b/simgui.json @@ -0,0 +1,34 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK MAX [10]": { + "header": { + "open": true + } + }, + "SPARK MAX [30]": { + "header": { + "open": true + } + }, + "SPARK MAX [31]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Hood": "Subsystem", + "/LiveWindow/LED": "Subsystem", + "/LiveWindow/SwerveDrive": "Subsystem", + "/LiveWindow/SwerveModule": "Subsystem", + "/LiveWindow/Turret": "Subsystem", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/LiveWindow/Vision": "Subsystem" + } + } +} diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a0cef9f..f51492d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,9 +94,9 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; - public static final int SHOOTER_ROTATE_ID = 0; //"unknown value" //figure out later + public static final int SHOOTER_ROTATE_ID = 31; //"unknown value" //figure out later public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// - public static final int TURRET_SPEED_MULTIPLIER = 0; //""// + public static final double TURRET_SPEED_MULTIPLIER = 0.1d; //""// public static final int DEGREES_PER_ROT = 0; //""// public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// @@ -105,7 +105,7 @@ public final class Constants { /* Turret Constants */ //ID - public static final int TURRET_MOTOR_CAN_ID = 0; + public static final int TURRET_MOTOR_CAN_ID = 30; public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d72c42a..8dee4e3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -73,11 +73,11 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret) - ); - + ); + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.aimToCenter())); + } - m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.aimToCenter())); /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d713cbe..a1ee217 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -6,6 +6,8 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; @@ -97,7 +99,7 @@ public class RobotMap { /*Boom Boom Subsystem*/ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.SHOOTER_TURRET_CAN_ID); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); //Create motor CANSparkMax void ConfigureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 531ffb3..df6ca93 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -4,6 +4,7 @@ package frc4388.robot.subsystems; +import java.io.Console; import java.util.Base64.Encoder; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -25,6 +26,7 @@ public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 +BangBangController m_controller = new BangBangController(); double velP; double input; @@ -122,12 +124,11 @@ public void setShooterGains() { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init m_shooterFalconRight.follow(m_shooterFalconLeft); // New BoomBoom controller stuff - BangBangController controller = new BangBangController(); //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - - + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); + m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + System.err.println(targetVel); } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 0202c6d..0528c73 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -67,11 +67,11 @@ public class Turret extends SubsystemBase { @Override public void periodic() { - SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); + // SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); - SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); + // SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); - SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); + // SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } From debdb1c087bf69cef0d48d11b8295d7cf5cc247a Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 28 Jan 2022 17:17:24 -0700 Subject: [PATCH 26/73] added a constant --- src/main/java/frc4388/robot/Constants.java | 2 +- .../java/frc4388/robot/subsystems/BoomBoom.java | 14 +------------- 2 files changed, 2 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a0cef9f..1b9b7a8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -100,7 +100,7 @@ public final class Constants { public static final int DEGREES_PER_ROT = 0; //""// public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// - + public static final double ENCODER_TICKS_PER_REV = 2048; //""// /* Turret Constants */ diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index f2809c2..005abdc 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -5,21 +5,17 @@ package frc4388.robot.subsystems; import java.util.Base64.Encoder; - import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; import frc4388.utility.Gains; import frc4388.utility.controller.IHandController; -import com.revrobotics.RelativeEncoder; -import com.revrobotics.*; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); @@ -40,8 +36,6 @@ public Turret m_turretSubsystem; -SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later - /* @@ -51,15 +45,11 @@ public BoomBoom(){ //Testing purposes resetting gyros //resetGryoAngleADj(); //shooterTrims = new Trims(0,0); -feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); - - } /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - int closedLoopTimeMs = 1; //LEFT FALCON m_shooterFalconLeft.configFactoryDefault(); @@ -72,7 +62,6 @@ feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - //RIGHT FALCON m_shooterFalconRight.configFactoryDefault(); m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); @@ -139,8 +128,7 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - + m_shooterFalconLeft.set(controller.calculate(Encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); } } From 71a6afd240538c6a650d65aa08f41d5e0c6386b8 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 17:20:58 -0700 Subject: [PATCH 27/73] the encoder was fixed --- src/main/java/frc4388/robot/subsystems/BoomBoom.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index f2809c2..2fa28d1 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -19,7 +19,6 @@ import frc4388.utility.ShooterTables; import frc4388.utility.Gains; import frc4388.utility.controller.IHandController; import com.revrobotics.RelativeEncoder; -import com.revrobotics.*; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); @@ -71,6 +70,7 @@ feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + //RIGHT FALCON @@ -139,8 +139,8 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - + m_shooterFalconLeft.set(controller.calculate(m_shooterFalconLeft.getSelectedSensorVelocity(), targetVel) + 0.9 * feedforward.calculate(targetVel)); + } } From 6d53d0431a96bd0885370895de4544dd538a5e60 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 19:05:58 -0700 Subject: [PATCH 28/73] Revert "Merge branch 'Shooter' of https://github.com/Team4388/2022NoWayHome into Shooter" This reverts commit 82297e5a164ed98b057bbdb7f690278d14f6cc2c, reversing changes made to 71a6afd240538c6a650d65aa08f41d5e0c6386b8. --- simgui-ds.json | 101 ------------------ simgui.json | 34 ------ src/main/java/frc4388/robot/Constants.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 8 +- src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/BoomBoom.java | 5 +- .../java/frc4388/robot/subsystems/Turret.java | 6 +- 7 files changed, 13 insertions(+), 151 deletions(-) delete mode 100644 simgui-ds.json delete mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json deleted file mode 100644 index b16ea5c..0000000 --- a/simgui-ds.json +++ /dev/null @@ -1,101 +0,0 @@ -{ - "keyboardJoysticks": [ - { - "axisConfig": [ - { - "decKey": 65, - "incKey": 68 - }, - { - "decKey": 87, - "incKey": 83 - }, - { - "decKey": 69, - "decayRate": 0.0, - "incKey": 82, - "keyRate": 0.009999999776482582 - } - ], - "axisCount": 3, - "buttonCount": 4, - "buttonKeys": [ - 90, - 88, - 67, - 86 - ], - "povConfig": [ - { - "key0": 328, - "key135": 323, - "key180": 322, - "key225": 321, - "key270": 324, - "key315": 327, - "key45": 329, - "key90": 326 - } - ], - "povCount": 1 - }, - { - "axisConfig": [ - { - "decKey": 74, - "incKey": 76 - }, - { - "decKey": 73, - "incKey": 75 - } - ], - "axisCount": 2, - "buttonCount": 4, - "buttonKeys": [ - 77, - 44, - 46, - 47 - ], - "povCount": 0 - }, - { - "axisConfig": [ - { - "decKey": 263, - "incKey": 262 - }, - { - "decKey": 265, - "incKey": 264 - } - ], - "axisCount": 2, - "buttonCount": 6, - "buttonKeys": [ - 260, - 268, - 266, - 261, - 269, - 267 - ], - "povCount": 0 - }, - { - "axisCount": 0, - "buttonCount": 0, - "povCount": 0 - } - ], - "robotJoysticks": [ - { - "useGamepad": true - }, - { - "guid": "78696e70757401000000000000000000", - "useGamepad": true - } - ] -} diff --git a/simgui.json b/simgui.json deleted file mode 100644 index 72aab8f..0000000 --- a/simgui.json +++ /dev/null @@ -1,34 +0,0 @@ -{ - "HALProvider": { - "Other Devices": { - "SPARK MAX [10]": { - "header": { - "open": true - } - }, - "SPARK MAX [30]": { - "header": { - "open": true - } - }, - "SPARK MAX [31]": { - "header": { - "open": true - } - } - } - }, - "NTProvider": { - "types": { - "/FMSInfo": "FMSInfo", - "/LiveWindow/BoomBoom": "Subsystem", - "/LiveWindow/Hood": "Subsystem", - "/LiveWindow/LED": "Subsystem", - "/LiveWindow/SwerveDrive": "Subsystem", - "/LiveWindow/SwerveModule": "Subsystem", - "/LiveWindow/Turret": "Subsystem", - "/LiveWindow/Ungrouped/Scheduler": "Scheduler", - "/LiveWindow/Vision": "Subsystem" - } - } -} diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index f51492d..a0cef9f 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,9 +94,9 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; - public static final int SHOOTER_ROTATE_ID = 31; //"unknown value" //figure out later + public static final int SHOOTER_ROTATE_ID = 0; //"unknown value" //figure out later public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// - public static final double TURRET_SPEED_MULTIPLIER = 0.1d; //""// + public static final int TURRET_SPEED_MULTIPLIER = 0; //""// public static final int DEGREES_PER_ROT = 0; //""// public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// @@ -105,7 +105,7 @@ public final class Constants { /* Turret Constants */ //ID - public static final int TURRET_MOTOR_CAN_ID = 30; + public static final int TURRET_MOTOR_CAN_ID = 0; public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8dee4e3..d72c42a 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -73,11 +73,11 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret) - ); - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.aimToCenter())); - } + ); + + m_robotTurret.setDefaultCommand( + new RunCommand(() -> m_robotTurret.aimToCenter())); /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a1ee217..d713cbe 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -6,8 +6,6 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; @@ -99,7 +97,7 @@ public class RobotMap { /*Boom Boom Subsystem*/ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.SHOOTER_TURRET_CAN_ID); //Create motor CANSparkMax void ConfigureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 10713d5..2fa28d1 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -4,7 +4,6 @@ package frc4388.robot.subsystems; -import java.io.Console; import java.util.Base64.Encoder; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -28,7 +27,6 @@ public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 -BangBangController m_controller = new BangBangController(); double velP; double input; @@ -137,6 +135,7 @@ public void setShooterGains() { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init m_shooterFalconRight.follow(m_shooterFalconLeft); // New BoomBoom controller stuff + BangBangController controller = new BangBangController(); //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter @@ -144,4 +143,4 @@ public void setShooterGains() { } -} \ No newline at end of file +} diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 0528c73..0202c6d 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -67,11 +67,11 @@ public class Turret extends SubsystemBase { @Override public void periodic() { - // SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); + SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); - // SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); + SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); - // SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); + SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } From 2d9cd7abe6b44b26cbba7340edb0f6ad5da95620 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 19:06:28 -0700 Subject: [PATCH 29/73] Revert "Revert "Merge branch 'Shooter' of https://github.com/Team4388/2022NoWayHome into Shooter"" This reverts commit 6d53d0431a96bd0885370895de4544dd538a5e60. --- simgui-ds.json | 101 ++++++++++++++++++ simgui.json | 34 ++++++ src/main/java/frc4388/robot/Constants.java | 6 +- .../java/frc4388/robot/RobotContainer.java | 8 +- src/main/java/frc4388/robot/RobotMap.java | 4 +- .../frc4388/robot/subsystems/BoomBoom.java | 5 +- .../java/frc4388/robot/subsystems/Turret.java | 6 +- 7 files changed, 151 insertions(+), 13 deletions(-) create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 0000000..b16ea5c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,101 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ], + "robotJoysticks": [ + { + "useGamepad": true + }, + { + "guid": "78696e70757401000000000000000000", + "useGamepad": true + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 0000000..72aab8f --- /dev/null +++ b/simgui.json @@ -0,0 +1,34 @@ +{ + "HALProvider": { + "Other Devices": { + "SPARK MAX [10]": { + "header": { + "open": true + } + }, + "SPARK MAX [30]": { + "header": { + "open": true + } + }, + "SPARK MAX [31]": { + "header": { + "open": true + } + } + } + }, + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/LiveWindow/BoomBoom": "Subsystem", + "/LiveWindow/Hood": "Subsystem", + "/LiveWindow/LED": "Subsystem", + "/LiveWindow/SwerveDrive": "Subsystem", + "/LiveWindow/SwerveModule": "Subsystem", + "/LiveWindow/Turret": "Subsystem", + "/LiveWindow/Ungrouped/Scheduler": "Scheduler", + "/LiveWindow/Vision": "Subsystem" + } + } +} diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index a0cef9f..f51492d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,9 +94,9 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; - public static final int SHOOTER_ROTATE_ID = 0; //"unknown value" //figure out later + public static final int SHOOTER_ROTATE_ID = 31; //"unknown value" //figure out later public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// - public static final int TURRET_SPEED_MULTIPLIER = 0; //""// + public static final double TURRET_SPEED_MULTIPLIER = 0.1d; //""// public static final int DEGREES_PER_ROT = 0; //""// public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// @@ -105,7 +105,7 @@ public final class Constants { /* Turret Constants */ //ID - public static final int TURRET_MOTOR_CAN_ID = 0; + public static final int TURRET_MOTOR_CAN_ID = 30; public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d72c42a..8dee4e3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -73,11 +73,11 @@ public class RobotContainer { m_robotTurret.setDefaultCommand( new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret) - ); - + ); + // m_robotTurret.setDefaultCommand( + // new RunCommand(() -> m_robotTurret.aimToCenter())); + } - m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.aimToCenter())); /** * Use this method to define your button->command mappings. Buttons can be * created by instantiating a {@link GenericHID} or one of its subclasses diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d713cbe..a1ee217 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -6,6 +6,8 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; @@ -97,7 +99,7 @@ public class RobotMap { /*Boom Boom Subsystem*/ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.SHOOTER_TURRET_CAN_ID); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); //Create motor CANSparkMax void ConfigureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 2fa28d1..10713d5 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -4,6 +4,7 @@ package frc4388.robot.subsystems; +import java.io.Console; import java.util.Base64.Encoder; import com.ctre.phoenix.motorcontrol.NeutralMode; @@ -27,6 +28,7 @@ public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 +BangBangController m_controller = new BangBangController(); double velP; double input; @@ -135,7 +137,6 @@ public void setShooterGains() { m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); //Init m_shooterFalconRight.follow(m_shooterFalconLeft); // New BoomBoom controller stuff - BangBangController controller = new BangBangController(); //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter @@ -143,4 +144,4 @@ public void setShooterGains() { } -} +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 0202c6d..0528c73 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -67,11 +67,11 @@ public class Turret extends SubsystemBase { @Override public void periodic() { - SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); + // SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); - SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); + // SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); - SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); + // SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } From 4a30390305e282100f12796d9b2a43bb113cd019 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 19:21:04 -0700 Subject: [PATCH 30/73] fix --- src/main/java/frc4388/robot/subsystems/Turret.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 0528c73..5a4fa7a 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -12,9 +12,7 @@ import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import edu.wpi.first.util.sendable.Sendable; import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; From efab5da238ff86ea8e5f32b804a4531f89707deb Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Fri, 28 Jan 2022 19:21:34 -0700 Subject: [PATCH 31/73] Update BoomBoom.java --- src/main/java/frc4388/robot/subsystems/BoomBoom.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 90c7d15..5d4ba74 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -40,7 +40,6 @@ public Turret m_turretSubsystem; - /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ @@ -134,7 +133,7 @@ public void setShooterGains() { m_shooterFalconLeft.set(controller.calculate(Encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + m_shooterFalconLeft.set(controller.calculate(m_shooterFalconLeft.get(), targetVel)); System.err.println(targetVel); m_shooterFalconLeft.set(controller.calculate(m_shooterFalconLeft.getSelectedSensorVelocity(), targetVel) + 0.9 * feedforward.calculate(targetVel)); From 82c6918ba303ed68c03a34637f1e25c25b8da656 Mon Sep 17 00:00:00 2001 From: Pushkar9236 <104144@psdschools.org> Date: Fri, 28 Jan 2022 19:35:24 -0700 Subject: [PATCH 32/73] fixed bugges --- .../frc4388/robot/subsystems/BoomBoom.java | 48 +++++++++++++++++-- .../java/frc4388/robot/subsystems/Turret.java | 1 + 2 files changed, 45 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 90c7d15..88835cd 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -6,12 +6,14 @@ package frc4388.robot.subsystems; import java.io.Console; import java.util.Base64.Encoder; + import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.ShooterTables; @@ -41,6 +43,42 @@ public Turret m_turretSubsystem; + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later + + + /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ @@ -48,11 +86,15 @@ public BoomBoom(){ //Testing purposes resetting gyros //resetGryoAngleADj(); //shooterTrims = new Trims(0,0); +feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); + + } /** Creates a new BoomBoom. */ public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; + int closedLoopTimeMs = 1; //LEFT FALCON m_shooterFalconLeft.configFactoryDefault(); @@ -66,6 +108,7 @@ public BoomBoom(){ m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + //RIGHT FALCON m_shooterFalconRight.configFactoryDefault(); m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); @@ -131,12 +174,9 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang controller //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - m_shooterFalconLeft.set(controller.calculate(Encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); - System.err.println(targetVel); - m_shooterFalconLeft.set(controller.calculate(m_shooterFalconLeft.getSelectedSensorVelocity(), targetVel) + 0.9 * feedforward.calculate(targetVel)); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 5a4fa7a..6067a8c 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -13,6 +13,7 @@ import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; From d666e9eb9d7a86340e1f58a49b42052cb057b547 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org> Date: Sat, 29 Jan 2022 11:33:57 -0700 Subject: [PATCH 33/73] added tentative button mapping for boomboom --- .../java/frc4388/robot/RobotContainer.java | 6 ++- .../frc4388/robot/subsystems/BoomBoom.java | 41 ------------------- 2 files changed, 5 insertions(+), 42 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 8dee4e3..5360b3c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -92,8 +92,12 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); + // activates "BoomBoom" + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) + .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); + /* Driver Buttons */ } - /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 88835cd..2e4b8f3 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -39,42 +39,6 @@ public double m_fireVel; public Hood m_hoodSubsystem; public Turret m_turretSubsystem; - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later @@ -151,9 +115,6 @@ try { m_turretSubsystem = subsystem1; } - public double addFireVel(){ - return m_fireVel; - } public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); m_shooterFalconRight.follow(m_shooterFalconLeft); @@ -177,7 +138,5 @@ public void setShooterGains() { // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); - - } } \ No newline at end of file From 3e43b50f04ed236f373b1ab114a0e85188ced892 Mon Sep 17 00:00:00 2001 From: Ryan Manley Date: Sat, 5 Feb 2022 14:27:11 -0700 Subject: [PATCH 34/73] commented out bunch of stuff in order to make turret work --- src/main/java/frc4388/robot/Constants.java | 26 ++--- .../java/frc4388/robot/RobotContainer.java | 29 +++-- src/main/java/frc4388/robot/RobotMap.java | 110 +++++++++--------- .../java/frc4388/robot/subsystems/Hood.java | 38 +++--- .../java/frc4388/robot/subsystems/Turret.java | 10 +- 5 files changed, 107 insertions(+), 106 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7edef00..b31b7d8 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,18 +31,18 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND = 5; public static final double MAX_SPEED_FEET_PER_SEC = 16; public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; - public static final int LEFT_FRONT_STEER_CAN_ID = 2; - public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; - public static final int RIGHT_FRONT_STEER_CAN_ID = 4; - public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; - public static final int LEFT_BACK_STEER_CAN_ID = 6; - public static final int LEFT_BACK_WHEEL_CAN_ID = 7; - public static final int RIGHT_BACK_STEER_CAN_ID = 8; - public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; - public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; - public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; + // public static final int LEFT_FRONT_STEER_CAN_ID = 2; + // public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; + // public static final int RIGHT_FRONT_STEER_CAN_ID = 4; + // public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; + // public static final int LEFT_BACK_STEER_CAN_ID = 6; + // public static final int LEFT_BACK_WHEEL_CAN_ID = 7; + // public static final int RIGHT_BACK_STEER_CAN_ID = 8; + // public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; + // public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; + // public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; + // public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; + // public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // ofsets are in degrees //ofsets are in degrees // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; @@ -115,7 +115,7 @@ public final class Constants { public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0);//x,y,z,a,b are not actual values, fix later /* Hood Constants */ - public static final int SHOOTER_ANGLE_ADJUST_ID = 10; + public static final int SHOOTER_ANGLE_ADJUST_ID = 32; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5360b3c..6ba81e1 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -32,16 +32,16 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, - m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, - m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, - m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, - m_robotMap.leftFrontEncoder, - m_robotMap.rightFrontEncoder, - m_robotMap.leftBackEncoder, - m_robotMap.rightBackEncoder - ); + // private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( + // m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, + // m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, + // m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, + // m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, + // m_robotMap.leftFrontEncoder, + // m_robotMap.rightFrontEncoder, + // m_robotMap.leftBackEncoder, + // m_robotMap.rightBackEncoder + // ); private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(); @@ -60,9 +60,9 @@ public class RobotContainer { /* Default Commands */ // drives the swerve drive with a two-axis input from the driver controller - m_robotSwerveDrive.setDefaultCommand( - new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), - getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); + // m_robotSwerveDrive.setDefaultCommand( + // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), + // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); @@ -72,8 +72,7 @@ public class RobotContainer { //Turret default command m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret) - ); + new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index a1ee217..3572923 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -22,7 +22,7 @@ public class RobotMap { public RobotMap() { configureLEDMotorControllers(); - configureSwerveMotorControllers(); + //configureSwerveMotorControllers(); } /* LED Subsystem */ @@ -33,73 +33,73 @@ public class RobotMap { } /* Swerve Subsystem */ - public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); - public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + // public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); + // public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); + // public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); + // public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + // public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + // public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + // public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + // public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); + // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); + // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); + // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - void configureSwerveMotorControllers() { - leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); - leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // void configureSwerveMotorControllers() { + // leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + // rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + // leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + // rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - leftFrontSteerMotor.configFactoryDefault(); - leftFrontWheelMotor.configFactoryDefault(); - rightFrontSteerMotor.configFactoryDefault(); - rightFrontWheelMotor.configFactoryDefault(); - leftBackSteerMotor.configFactoryDefault(); - leftBackWheelMotor.configFactoryDefault(); - rightBackSteerMotor.configFactoryDefault(); - rightBackWheelMotor.configFactoryDefault(); + // leftFrontSteerMotor.configFactoryDefault(); + // leftFrontWheelMotor.configFactoryDefault(); + // rightFrontSteerMotor.configFactoryDefault(); + // rightFrontWheelMotor.configFactoryDefault(); + // leftBackSteerMotor.configFactoryDefault(); + // leftBackWheelMotor.configFactoryDefault(); + // rightBackSteerMotor.configFactoryDefault(); + // rightBackWheelMotor.configFactoryDefault(); - leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // config cancoder as remote encoder for swerve steer motors + // // config cancoder as remote encoder for swerve steer motors //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - } + //} //Shooter Config /*Boom Boom Subsystem*/ - public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + // public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + // public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); //Create motor CANSparkMax void ConfigureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index de78848..1ef6af3 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -21,13 +21,13 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + // public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); - public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); //public boolean m_isHoodReady = false; @@ -37,12 +37,12 @@ public double m_fireAngle; /** Creates a new Hood. */ public Hood() { - m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + // m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodUpLimitSwitch.enableLimitSwitch(true); - m_hoodDownLimitSwitch.enableLimitSwitch(true); + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); + // m_hoodDownLimitSwitch.enableLimitSwitch(true); } @@ -53,30 +53,30 @@ public double m_fireAngle; public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients - m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); - m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); - m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); - m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); - m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); - m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); + // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); + // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); + // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); + // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); - m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } public void runHood(double input) { - m_angleAdjusterMotor.set(input); + // m_angleAdjusterMotor.set(input); } public void resetGyroAngleAdj(){ - m_angleEncoder.setPosition(0); + // m_angleEncoder.setPosition(0); } public double getAnglePosition(){ - return m_angleEncoder.getPosition(); + return 0.0;//m_angleEncoder.getPosition(); } public double getAnglePositionDegrees(){ - return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; + return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; } } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 6067a8c..2f70146 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -28,7 +28,7 @@ public class Turret extends SubsystemBase { public BoomBoom m_boomBoomSubsystem; public SwerveDrive m_sDriveSubsystem; - public CANSparkMax m_boomBoomRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; public Gyro m_turretGyro; @@ -37,14 +37,16 @@ public class Turret extends SubsystemBase { public boolean m_isAimReady = false; - SparkMaxPIDController m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); - public RelativeEncoder m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); + SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); + public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); //Variables public Turret() { - + m_boomBoomRotateMotor = new CANSparkMax(30, MotorType.kBrushless); + m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); + m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); m_turretGyro = getGyroInterface(); From 0fedb240d7cfb8130afd31f5eeb16b4a353525f7 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Wed, 9 Feb 2022 17:56:22 -0700 Subject: [PATCH 35/73] BoomBoom is working, but the motors are spinning in opposite directions --- src/main/java/frc4388/robot/Constants.java | 8 +++-- .../java/frc4388/robot/RobotContainer.java | 4 ++- src/main/java/frc4388/robot/RobotMap.java | 31 +++++++++++++++++-- .../frc4388/robot/subsystems/BoomBoom.java | 30 +++--------------- 4 files changed, 40 insertions(+), 33 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b31b7d8..6b7b4aa 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -88,12 +88,13 @@ public final class Constants { public static final class ShooterConstants { /* PID Constants Shooter */ - public static final int SHOOTER_TIMEOUT_MS = 30; + public static final int CLOSED_LOOP_TIME_MS = 1; + public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); - public static final int SHOOTER_FALCON_LEFT_CAN_ID = 0; - public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 0; + public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; + public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; public static final int SHOOTER_ROTATE_ID = 31; //"unknown value" //figure out later public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// public static final double TURRET_SPEED_MULTIPLIER = 0.1d; //""// @@ -102,6 +103,7 @@ public final class Constants { public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// public static final double ENCODER_TICKS_PER_REV = 2048; //""// + /* Turret Constants */ //ID diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6ba81e1..964e0d9 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -44,7 +44,7 @@ public class RobotContainer { // ); private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final BoomBoom m_robotBoomBoom = new BoomBoom(); + private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(); private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); @@ -95,6 +95,8 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); + + /* Driver Buttons */ } /** diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3572923..19cb616 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,6 +4,7 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; @@ -97,12 +98,36 @@ public class RobotMap { //Shooter Config /*Boom Boom Subsystem*/ - // public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); - // public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); + public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); + // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); - //Create motor CANSparkMax + // Create motor CANSparkMax void ConfigureShooterMotorControllers() { + //LEFT FALCON + shooterFalconLeft.configFactoryDefault(); + shooterFalconLeft.setNeutralMode(NeutralMode.Coast); + shooterFalconLeft.setInverted(true); + shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + + + + //RIGHT FALCON + shooterFalconRight.configFactoryDefault(); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.setInverted(false); + shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); + //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0,ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); /*Turret Subsytem*/ diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 2e4b8f3..0a235e3 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -22,8 +22,8 @@ import frc4388.utility.controller.IHandController; import com.revrobotics.RelativeEncoder; public class BoomBoom extends SubsystemBase { -public WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); -public WPI_TalonFX m_shooterFalconRight= new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID); +public WPI_TalonFX m_shooterFalconLeft; +public WPI_TalonFX m_shooterFalconRight; public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; @@ -59,31 +59,9 @@ feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - int closedLoopTimeMs = 1; - //LEFT FALCON - m_shooterFalconLeft.configFactoryDefault(); - m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast); - m_shooterFalconLeft.setInverted(true); - m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - + - //RIGHT FALCON - m_shooterFalconRight.configFactoryDefault(); - m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); - m_shooterFalconRight.setInverted(false); - m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) - m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterTable = new ShooterTables(); @@ -117,7 +95,7 @@ try { public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); - m_shooterFalconRight.follow(m_shooterFalconLeft); + } public void setShooterGains() { From 10e70b6ae24c89727558bfdade95284c5b7ae5a1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 11 Feb 2022 20:22:40 -0700 Subject: [PATCH 36/73] default max vel and acc for autos --- src/main/java/frc4388/robot/Constants.java | 3 +++ .../java/frc4388/robot/RobotContainer.java | 25 +++++++++++++++---- 2 files changed, 23 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 496b7e6..597ab9c 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -69,6 +69,9 @@ public final class Constants { public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0); public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController( 15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI)); + + public static final double MAX_VEL = 5.0; + public static final double MAX_ACC = 5.0; // swerve configuration public static final double NEUTRAL_DEADBAND = 0.04; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 58fc9e6..5a52550 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -11,6 +11,7 @@ import java.util.ArrayList; import java.util.Arrays; import java.util.HashMap; import java.util.List; +import java.util.Objects; import javax.swing.plaf.nimbus.State; @@ -127,7 +128,17 @@ public class RobotContainer { .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); } - public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) { + /** + * Generate autonomous + * @param maxVel max velocity for the path (null to override default value of 5.0 m/s) + * @param maxAccel max acceleration for the path (null to override default value of 5.0 m/s/s) + * @param inputs strings (paths) or commands you want to run (in order) + * @return array of commands + */ + public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) { + + maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL); + maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC); ArrayList commands = new ArrayList(); commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); @@ -174,10 +185,14 @@ public class RobotContainer { */ public Command getAutonomousCommand() { // https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki - - Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down"); - SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands); - return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))))); + return new ParallelCommandGroup( + buildAuto( + null, + null, + new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")), + new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)) + ) + ); } /** From 0605305a898a5a4e1427ecd01b19fe59b47ee26c Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 11 Feb 2022 20:24:23 -0700 Subject: [PATCH 37/73] slight change for fun --- .../java/frc4388/robot/RobotContainer.java | 21 ++----------------- 1 file changed, 2 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5a52550..203bcff 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,17 +4,9 @@ package frc4388.robot; - -import java.lang.reflect.Array; -import java.nio.file.FileSystem; import java.util.ArrayList; -import java.util.Arrays; -import java.util.HashMap; -import java.util.List; import java.util.Objects; -import javax.swing.plaf.nimbus.State; - import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; import com.pathplanner.lib.PathPlanner; @@ -22,25 +14,16 @@ import com.pathplanner.lib.PathPlannerTrajectory; import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState; import com.pathplanner.lib.commands.PPSwerveControllerCommand; -import edu.wpi.first.math.controller.HolonomicDriveController; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.ProfiledPIDController; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.trajectory.Trajectory; -import edu.wpi.first.math.trajectory.TrajectoryConfig; -import edu.wpi.first.math.trajectory.TrajectoryGenerator; -import edu.wpi.first.math.trajectory.TrapezoidProfile; -import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint; -import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.SwerveControllerCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; @@ -130,8 +113,8 @@ public class RobotContainer { /** * Generate autonomous - * @param maxVel max velocity for the path (null to override default value of 5.0 m/s) - * @param maxAccel max acceleration for the path (null to override default value of 5.0 m/s/s) + * @param maxVel max velocity for the path (null to override default value of 5.0) + * @param maxAccel max acceleration for the path (null to override default value of 5.0) * @param inputs strings (paths) or commands you want to run (in order) * @return array of commands */ From 749085a0b6679edf2039e276443939cdbf0b17a1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Feb 2022 17:56:18 -0700 Subject: [PATCH 38/73] added paths --- src/main/deploy/pathplanner/Every Ball.path | 1 + src/main/deploy/pathplanner/Two Ball High to Low.path | 1 + src/main/deploy/pathplanner/Two Ball Low.path | 1 + .../deploy/pathplanner/Two Ball Straight to Player Station.path | 1 + 4 files changed, 4 insertions(+) create mode 100644 src/main/deploy/pathplanner/Every Ball.path create mode 100644 src/main/deploy/pathplanner/Two Ball High to Low.path create mode 100644 src/main/deploy/pathplanner/Two Ball Low.path create mode 100644 src/main/deploy/pathplanner/Two Ball Straight to Player Station.path diff --git a/src/main/deploy/pathplanner/Every Ball.path b/src/main/deploy/pathplanner/Every Ball.path new file mode 100644 index 0000000..d1e4271 --- /dev/null +++ b/src/main/deploy/pathplanner/Every Ball.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.888836249581911,"y":5.215172677538834},"prevControl":null,"nextControl":{"x":6.537459367052471,"y":6.518965846916928},"holonomicAngle":106.3895403340348,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.8254588419278255,"y":7.8320057763675734},"prevControl":{"x":5.95491348285958,"y":6.759381608647329},"nextControl":{"x":5.95491348285958,"y":6.759381608647329},"holonomicAngle":-136.27303002005684,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.085718036603518,"y":6.18608248452099},"prevControl":{"x":5.222728278411091,"y":6.456690985532331},"nextControl":{"x":4.768933589337623,"y":5.5604025113498725},"holonomicAngle":-109.50244850666218,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.77803093326875,"y":3.4953054389840252},"prevControl":{"x":5.623399779842569,"y":3.7042882657946703},"nextControl":{"x":2.9363292766883204,"y":3.040020127851594},"holonomicAngle":-30.256437163529373,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.7486154042034336,"y":2.667447141251462},"prevControl":{"x":1.8144326730676037,"y":2.686839729398941},"nextControl":{"x":0.7297128839172015,"y":2.3672347915242677},"holonomicAngle":-75.06858282186249,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2129730034385093,"y":1.1845800804778424},"prevControl":{"x":1.2001001569914929,"y":1.2299758967179297},"nextControl":{"x":1.530279149856384,"y":0.06560677694911808},"holonomicAngle":-39.472459848343846,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.160119378376086,"y":1.9305622828301685},"prevControl":{"x":5.131309514738882,"y":1.9153315905888442},"nextControl":{"x":6.588402375563037,"y":2.6856418291137163},"holonomicAngle":36.02737338510347,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.60730489584927,"y":0.37491647060743843},"prevControl":{"x":7.329902093852659,"y":0.5875919521381767},"nextControl":{"x":7.884707697845882,"y":0.16224098907670015},"holonomicAngle":-41.00908690157027,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.617110072204374,"y":0.9389517943373179},"prevControl":{"x":8.535233976824232,"y":0.12019084053588047},"nextControl":{"x":8.698986167584517,"y":1.7577127481387553},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":9.144756020209744,"y":0.31123506308954896},"prevControl":{"x":9.199340083796505,"y":0.7206155399902687},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball High to Low.path b/src/main/deploy/pathplanner/Two Ball High to Low.path new file mode 100644 index 0000000..43221e3 --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball High to Low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.620680207650139,"y":5.35387407853639},"prevControl":null,"nextControl":{"x":6.047381083523801,"y":5.557302800000574},"holonomicAngle":169.99202019855858,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.020990716136325,"y":6.167588964393128},"prevControl":{"x":5.409354638931585,"y":5.5018222396012515},"nextControl":{"x":5.409354638931585,"y":5.5018222396012515},"holonomicAngle":129.4995885297982,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9325728539114686},"prevControl":{"x":5.085718036602201,"y":3.2825998236283294},"nextControl":null,"holonomicAngle":-77.66091272167375,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball Low.path b/src/main/deploy/pathplanner/Two Ball Low.path new file mode 100644 index 0000000..f43098c --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball Low.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":8.109408578365308,"y":2.117508055242545},"prevControl":null,"nextControl":{"x":8.192629418964295,"y":1.470234850583776},"holonomicAngle":-92.202598161766,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.637823814971061,"y":0.36987040266386867},"prevControl":{"x":8.775175303157184,"y":0.48083152346251423},"nextControl":{"x":6.123786635522426,"y":0.22215945832741724},"holonomicAngle":178.6360724683971,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.141198597001524,"y":1.9048325737118068},"prevControl":{"x":5.61278336039577,"y":1.729144132447284},"nextControl":null,"holonomicAngle":143.8418145601917,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":null,"maxAcceleration":5.0,"isReversed":null} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path b/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path new file mode 100644 index 0000000..b4b5c9c --- /dev/null +++ b/src/main/deploy/pathplanner/Two Ball Straight to Player Station.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":6.777875128781554,"y":2.4688849377715907},"prevControl":null,"nextControl":{"x":5.797718561726847,"y":2.043533974710114},"holonomicAngle":-156.37062226934333,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.2298190888492497,"y":1.174338528454054},"prevControl":{"x":1.904832573707682,"y":1.0171436073226392},"nextControl":null,"holonomicAngle":180.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file From dc932775177e854be27168e5a0336858f1d04a09 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org> Date: Thu, 17 Feb 2022 19:38:49 -0700 Subject: [PATCH 39/73] new hood controls --- src/main/java/frc4388/robot/Constants.java | 3 + .../java/frc4388/robot/RobotContainer.java | 57 ++++++++++++------- .../frc4388/robot/subsystems/BoomBoom.java | 2 +- .../java/frc4388/robot/subsystems/Hood.java | 4 +- 4 files changed, 44 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6b7b4aa..76c7b44 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -88,7 +88,10 @@ public final class Constants { public static final class ShooterConstants { /* PID Constants Shooter */ +<<<<<<< Updated upstream public static final int CLOSED_LOOP_TIME_MS = 1; +======= +>>>>>>> Stashed changes public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 964e0d9..9b5f674 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,6 +4,9 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonFX; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -12,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; +//import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; @@ -30,21 +34,22 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); + private final TalonFX m_testMotor = new TalonFX(2); - /* Subsystems */ - // private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - // m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, - // m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, - // m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, - // m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, - // m_robotMap.leftFrontEncoder, - // m_robotMap.rightFrontEncoder, - // m_robotMap.leftBackEncoder, - // m_robotMap.rightBackEncoder - // ); - + /* Subsystems + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( + m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, + m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, + m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, + m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, + m_robotMap.leftFrontEncoder, + m_robotMap.rightFrontEncoder, + m_robotMap.leftBackEncoder, + m_robotMap.rightBackEncoder + ); + */ private final LED m_robotLED = new LED(m_robotMap.LEDController); - private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); + private final BoomBoom m_robotBoomBoom = new BoomBoom(); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(); private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); @@ -60,19 +65,20 @@ public class RobotContainer { /* Default Commands */ // drives the swerve drive with a two-axis input from the driver controller - // m_robotSwerveDrive.setDefaultCommand( - // new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), - // getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); + /*m_robotSwerveDrive.setDefaultCommand( + new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(), + getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); - + */ //Turret default command m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); + new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret) + ); // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } @@ -88,6 +94,10 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" + + new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) + .whileHeld(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.5)); + new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); @@ -95,9 +105,16 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); - - /* Driver Buttons */ + //activates intake + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON); + // .whenPressed() -> m_robot + /*operator button*/ + //activates hood + new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + .whenPressed(() -> m_robotHood.runHood(0.5d)) + .whenReleased(() -> m_robotHood.runHood(0.d)); + new JoystickButton(getOperatorJoystick() } /** * Use this to pass the autonomous command to the main {@link Robot} class. diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 0a235e3..5dfb993 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -114,7 +114,7 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter - // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); + //m_shooterFalconLeft.set(calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 1ef6af3..046d402 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -4,6 +4,8 @@ package frc4388.robot.subsystems; +import javax.sql.rowset.WebRowSet; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; @@ -21,7 +23,7 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - // public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + //public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; From f97b24d88e357ab9c52dcaac060e03f554990bc9 Mon Sep 17 00:00:00 2001 From: Raghav66296 <90011037+Raghav66296@users.noreply.github.com> Date: Thu, 17 Feb 2022 19:53:06 -0700 Subject: [PATCH 40/73] pushkar has covid --- src/main/java/frc4388/robot/RobotMap.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 19cb616..fb93b88 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -5,11 +5,12 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; +import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; - import edu.wpi.first.wpilibj.motorcontrol.Spark; import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.ShooterConstants; @@ -119,9 +120,9 @@ public class RobotMap { //RIGHT FALCON - shooterFalconRight.configFactoryDefault(); - shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.setInverted(false); + shooterFalconRight.setNeutralMode(NeutralMode.Coast); + shooterFalconRight.configFactoryDefault(); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) @@ -130,6 +131,9 @@ public class RobotMap { shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); /*Turret Subsytem*/ + + shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); + shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); } From 32ae624588ff425ce69a7b935fa705354486c37e Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 17 Feb 2022 19:52:05 -0700 Subject: [PATCH 41/73] java docs and yaw print --- src/main/java/frc4388/robot/Main.java | 3 +-- src/main/java/frc4388/robot/Robot.java | 8 +++---- .../java/frc4388/robot/RobotContainer.java | 19 +++++++++++++--- .../frc4388/robot/subsystems/SwerveDrive.java | 22 +++++++------------ .../robot/subsystems/SwerveModule.java | 14 ++++++++---- 5 files changed, 38 insertions(+), 28 deletions(-) diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index 21f4236..fdb3853 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -28,5 +28,4 @@ public final class Main { } } - -// hi ryan \ No newline at end of file +// hi ryan -aarav \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index db7bb1a..debf72c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -66,6 +66,8 @@ public class Robot extends TimedRobot { // and running subsystem periodic() methods. This must be called from the robot's periodic // block in order for anything in the Command-based framework to work. CommandScheduler.getInstance().run(); + + // print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it) SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); @@ -84,9 +86,6 @@ public class Robot extends TimedRobot { @Override public void disabledPeriodic() { - // SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX()); - // SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY()); - // SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees()); } /** @@ -114,6 +113,7 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { LOGGER.fine("teleopInit()"); + m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw()); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove @@ -130,8 +130,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - // m_robotContainer.getDriverController().updateInput(); - // m_robotContainer.getOperatorController().updateInput(); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 203bcff..9440932 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -44,7 +44,7 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); private final TalonFX m_testMotor = new TalonFX(23); @@ -115,17 +115,19 @@ public class RobotContainer { * Generate autonomous * @param maxVel max velocity for the path (null to override default value of 5.0) * @param maxAccel max acceleration for the path (null to override default value of 5.0) - * @param inputs strings (paths) or commands you want to run (in order) - * @return array of commands + * @param inputs strings (path names) or commands you want to run (in order) + * @return array of commands, which can then be processed in a command group */ public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) { + // default vel and acc maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL); maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC); ArrayList commands = new ArrayList(); commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset())); + // pids controlling the path PIDController xController = SwerveDriveConstants.X_CONTROLLER; PIDController yController = SwerveDriveConstants.Y_CONTROLLER; ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER; @@ -134,7 +136,9 @@ public class RobotContainer { // parse input for (int i=0; i Date: Thu, 17 Feb 2022 19:54:24 -0700 Subject: [PATCH 42/73] :)) --- .../java/frc4388/robot/RobotContainer.java | 6 ++- .../robot/commands/AimWithOdometry.java | 37 +++++++++++++++++++ .../frc4388/robot/subsystems/BoomBoom.java | 33 ++++++----------- .../java/frc4388/robot/subsystems/Turret.java | 30 +++++---------- .../java/frc4388/robot/subsystems/Vision.java | 2 + 5 files changed, 64 insertions(+), 44 deletions(-) create mode 100644 src/main/java/frc4388/robot/commands/AimWithOdometry.java diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 964e0d9..907cdb5 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,6 +4,8 @@ package frc4388.robot; +import com.revrobotics.CANSparkMax; + import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -72,7 +74,7 @@ public class RobotContainer { //Turret default command m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runShooterWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); + new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } @@ -96,7 +98,7 @@ public class RobotContainer { .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); - + /* Driver Buttons */ } /** diff --git a/src/main/java/frc4388/robot/commands/AimWithOdometry.java b/src/main/java/frc4388/robot/commands/AimWithOdometry.java new file mode 100644 index 0000000..cb882a4 --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AimWithOdometry.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.CommandBase; + +public class AimWithOdometry extends CommandBase { + /** Creates a new AimWithOdometry. */ + public AimWithOdometry() { + // Use addRequirements() here to declare subsystem dependencies. + } + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + Pose2d odo = new Pose2d(new Translation2d(0,0), new Rotation2d(0)); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 0a235e3..60caaf3 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; +import edu.wpi.first.wpilibj.simulation.JoystickSim; import edu.wpi.first.wpilibj.smartdashboard.*; import edu.wpi.first.math.controller.BangBangController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; @@ -28,7 +29,7 @@ public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 -BangBangController m_controller = new BangBangController(); +// BangBangController m_controller = new BangBangController(); double velP; double input; @@ -39,33 +40,19 @@ public double m_fireVel; public Hood m_hoodSubsystem; public Turret m_turretSubsystem; -SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later +// SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster */ -public BoomBoom(){ -//Testing purposes resetting gyros -//resetGryoAngleADj(); -//shooterTrims = new Trims(0,0); -feedforward.calculate(15, 20); // feedforward.calculate(velocity, acceleration); - - -} /** Creates a new BoomBoom. */ - public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { +public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - - - - - m_shooterTable = new ShooterTables(); - - - } + m_shooterTable = new ShooterTables(); +} @Override @@ -92,7 +79,10 @@ try { m_hoodSubsystem = subsystem0; m_turretSubsystem = subsystem1; } - +/** + * Runs the Drum motor at a given speed + * @param speed percent output form -1.0 to 1.0 + */ public void runDrumShooter(double speed) { m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); @@ -114,7 +104,8 @@ public void setShooterGains() { //Controls a motor with the output of the BangBang conroller and a feedforward //Shrinks the feedforward slightly to avoid over speeding the shooter + // m_shooterFalconLeft.set(controller.calculate(encoder.getRate(), targetVel) + 0.9 * feedforward.calculate(targetVel)); - m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); + // m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel)); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2f70146..2890df7 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -42,14 +42,14 @@ public class Turret extends SubsystemBase { //Variables - public Turret() { + public Turret(CANSparkMax BoomBoomRotateMotor) { //Take in rotate motor as an argument - m_boomBoomRotateMotor = new CANSparkMax(30, MotorType.kBrushless); + m_boomBoomRotateMotor = BoomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - m_turretGyro = getGyroInterface(); + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); @@ -57,14 +57,11 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit m_boomBoomRotateMotor.setInverted(false); } - private Gyro getGyroInterface() { - return null; - } @Override public void periodic() { @@ -81,16 +78,18 @@ public class Turret extends SubsystemBase { m_sDriveSubsystem = subsystem1; } - public void runShooterWithInput(double input) { + public void runTurretWithInput(double input) { //Rename to turret m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } - public void runshooterRotatePID(double targetAngle) { + public void runshooterRotatePID(double targetAngle) { //Split into configure and run m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; @@ -111,16 +110,5 @@ public class Turret extends SubsystemBase { public double getAnglePositionDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; } - - - //function turnWithJoystick(double input) - // motor.set(input) -} - - - -/** TODO -* setPosition function -* Limit switches -**/ \ No newline at end of file +} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 310d70f..0b6ca09 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -37,6 +37,8 @@ public static double fireAngle; public double m_hoodTrim; public double m_turretTrim; public double m_fireAngle; + + public Vision(Turret aimSubsystem, BoomBoom boomBoom) { m_turret = aimSubsystem; m_boomBoom = boomBoom; From 1c79102077deb25f4e3c3ade07d0722cab7d9cca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org> Date: Fri, 18 Feb 2022 17:20:49 -0700 Subject: [PATCH 43/73] Update RobotContainer.java --- src/main/java/frc4388/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9b5f674..bd518e2 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -114,7 +114,6 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) .whenPressed(() -> m_robotHood.runHood(0.5d)) .whenReleased(() -> m_robotHood.runHood(0.d)); - new JoystickButton(getOperatorJoystick() } /** * Use this to pass the autonomous command to the main {@link Robot} class. From b5215b7a09d242aba5b79853be3d1d992d922390 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 19 Feb 2022 11:05:44 -0700 Subject: [PATCH 44/73] aimtocenter --- src/main/java/frc4388/robot/Constants.java | 11 ++++--- src/main/java/frc4388/robot/RobotMap.java | 2 +- ...{AimWithOdometry.java => AimToCenter.java} | 33 +++++++++++++++---- .../java/frc4388/robot/subsystems/Turret.java | 24 +++++++------- 4 files changed, 46 insertions(+), 24 deletions(-) rename src/main/java/frc4388/robot/commands/{AimWithOdometry.java => AimToCenter.java} (50%) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 76c7b44..b23b51e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -83,16 +83,17 @@ public final class Constants { public static final int XBOX_OPERATOR_ID = 1; } + public static final class OdoAimConstants { + public static final double X_VECTOR_DISTANCE = 12.3; + // public static final Gains AIM_GAINS = new Gains(0, 0, 0, 0, 0, 0, 1); - + } public static final class ShooterConstants { /* PID Constants Shooter */ -<<<<<<< Updated upstream public static final int CLOSED_LOOP_TIME_MS = 1; -======= ->>>>>>> Stashed changes - public static final int SHOOTER_TIMEOUT_MS = 32; + + public static final int SHOOTER_TIMEOUT_MS = 32; public static final int SHOOTER_SLOT_IDX = 0; public static final int SHOOTER_PID_LOOP_IDX = 1; public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index fb93b88..1e2b648 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -102,7 +102,7 @@ public class RobotMap { public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - // public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // Create motor CANSparkMax void ConfigureShooterMotorControllers() { diff --git a/src/main/java/frc4388/robot/commands/AimWithOdometry.java b/src/main/java/frc4388/robot/commands/AimToCenter.java similarity index 50% rename from src/main/java/frc4388/robot/commands/AimWithOdometry.java rename to src/main/java/frc4388/robot/commands/AimToCenter.java index cb882a4..734b9c2 100644 --- a/src/main/java/frc4388/robot/commands/AimWithOdometry.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -4,27 +4,46 @@ package frc4388.robot.commands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; -public class AimWithOdometry extends CommandBase { +public class AimToCenter extends CommandBase { /** Creates a new AimWithOdometry. */ - public AimWithOdometry() { + Turret m_turret; + SwerveDrive m_drive; + + // use odometry to find x and y later + double x = 0; + double y = 0; + double z = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); + double m_targetAngle; + + // public static Gains m_aimGains; + + public AimToCenter(Turret turret, SwerveDrive drive) { // Use addRequirements() here to declare subsystem dependencies. + m_turret = turret; + m_drive = drive; + addRequirements(m_turret, m_drive); } // Called when the command is initially scheduled. @Override - public void initialize() {} + public void initialize() { + m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - Pose2d odo = new Pose2d(new Translation2d(0,0), new Rotation2d(0)); + aimTurret(m_targetAngle); } + public void aimTurret(double targetAngle) { //Split into configure and run + m_turret.runshooterRotatePID(targetAngle); + } + // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) {} diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 2890df7..1abcfa1 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -41,10 +42,11 @@ public class Turret extends SubsystemBase { public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); + //Variables - public Turret(CANSparkMax BoomBoomRotateMotor) { //Take in rotate motor as an argument + public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument - m_boomBoomRotateMotor = BoomBoomRotateMotor; + m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); @@ -83,18 +85,18 @@ public class Turret extends SubsystemBase { } public void runshooterRotatePID(double targetAngle) { //Split into configure and run - m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); - m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); - m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); - m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); - m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); - targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; + targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); + m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); } public void resetGyroShooterRotate() @@ -107,7 +109,7 @@ public class Turret extends SubsystemBase { return m_boomBoomRotateEncoder.getPosition(); } - public double getAnglePositionDegrees() { + public double getBoomBoomAngleDegrees() { return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; } From a3fc1b4d14603364c76d018d4a25270d10108b96 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 19 Feb 2022 11:16:24 -0700 Subject: [PATCH 45/73] Swerve fixes --- src/main/deploy/pathplanner/New Path.path | 1 + src/main/java/frc4388/robot/Constants.java | 8 ++++---- src/main/java/frc4388/robot/RobotMap.java | 17 +++++++++-------- .../robot/subsystems/SwerveModule.java | 19 +++++++++++++++---- 4 files changed, 29 insertions(+), 16 deletions(-) create mode 100644 src/main/deploy/pathplanner/New Path.path diff --git a/src/main/deploy/pathplanner/New Path.path b/src/main/deploy/pathplanner/New Path.path new file mode 100644 index 0000000..9c09583 --- /dev/null +++ b/src/main/deploy/pathplanner/New Path.path @@ -0,0 +1 @@ +{"waypoints":[{"anchorPoint":{"x":1.0,"y":3.0},"prevControl":null,"nextControl":{"x":2.3149413387434365,"y":2.75478019310469},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.143744996350752,"y":3.8428026223141054},"prevControl":{"x":1.0069994823533945,"y":5.5442845062905315},"nextControl":{"x":7.478686694138279,"y":2.0338121072780666},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":7.870800551727686,"y":4.710905624342894},"prevControl":{"x":6.870800551727686,"y":4.710905624342894},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 597ab9c..aa9be76 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -53,10 +53,10 @@ public final class Constants { public static final int GYRO_ID = 14; // ofsets are in degrees - public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; - public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; - public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; - public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; + public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0; + public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0; + public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0; + public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index faefdd5..d2e1fde 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -91,14 +91,15 @@ public class RobotMap { rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - leftFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - rightFrontSteerMotor.setNeutralMode(NeutralMode.Brake); - rightFrontWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - leftBackSteerMotor.setNeutralMode(NeutralMode.Brake); - leftBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast - rightBackSteerMotor.setNeutralMode(NeutralMode.Brake); - rightBackWheelMotor.setNeutralMode(NeutralMode.Brake);//Coast + NeutralMode mode = NeutralMode.Brake; + leftFrontSteerMotor.setNeutralMode(mode); + leftFrontWheelMotor.setNeutralMode(mode);//Coast + rightFrontSteerMotor.setNeutralMode(mode); + rightFrontWheelMotor.setNeutralMode(mode);//Coast + leftBackSteerMotor.setNeutralMode(mode); + leftBackWheelMotor.setNeutralMode(mode);//Coast + rightBackSteerMotor.setNeutralMode(mode); + rightBackWheelMotor.setNeutralMode(mode);//Coast leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET); leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index fdd7ed1..d9c7665 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -15,6 +15,7 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; @@ -49,9 +50,9 @@ public class SwerveModule extends SubsystemBase { angleMotor.configAllSettings(angleTalonFXConfiguration); TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - driveTalonFXConfiguration.slot0.kP = 0.15; + driveTalonFXConfiguration.slot0.kP = 0.1; driveTalonFXConfiguration.slot0.kI = 0.0; - driveTalonFXConfiguration.slot0.kD = 0.5; + driveTalonFXConfiguration.slot0.kD = 0.0; driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); @@ -72,8 +73,7 @@ public class SwerveModule extends SubsystemBase { */ public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) { Rotation2d currentRotation = getAngle(); - - //SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + // SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); state = SwerveModuleState.optimize(desiredState, currentRotation); // Find the difference between our current rotational position + our new rotational position @@ -84,17 +84,23 @@ public class SwerveModule extends SubsystemBase { // Convert the CANCoder from it's position reading back to ticks double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient; double desiredTicks = currentTicks + deltaTicks; + if (!ignoreAngle){ angleMotor.set(TalonFXControlMode.Position, desiredTicks); } double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); +<<<<<<< Updated upstream // Add this line to correct for the slight drive movement the angle motor makes when turning in place. // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); +======= + // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC) + angleMotor.get()); +>>>>>>> Stashed changes } /** @@ -114,5 +120,10 @@ public class SwerveModule extends SubsystemBase { driveMotor.set(0); angleMotor.set(0); } + @Override + public void periodic(){ + Rotation2d currentRotation = getAngle(); + SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + } } From 997540b2c3856e2c87f13d3fd089883f619f5133 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 19 Feb 2022 11:18:15 -0700 Subject: [PATCH 46/73] gyro regression --- .../frc4388/robot/subsystems/SwerveDrive.java | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 681fcb9..c456027 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -56,7 +56,7 @@ public class SwerveDrive extends SubsystemBase { public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW; public boolean ignoreAngles; - public Rotation2d rotTarget = new Rotation2d();; + public Rotation2d rotTarget = new Rotation2d(); private final Field2d m_field = new Field2d(); @@ -204,6 +204,15 @@ public class SwerveDrive extends SubsystemBase { return m_poseEstimator.getEstimatedPosition(); } + /** + * Gets the current gyro using regression formula. + * @return Rotation2d object holding current gyro in radians + */ + public Rotation2d getRegGyro() { + double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527; + return new Rotation2d(regCur * Math.PI / 180); + } + /** * Resets the odometry of the robot to the given pose. */ @@ -211,10 +220,10 @@ public class SwerveDrive extends SubsystemBase { m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d()); } - /** Updates the field relative position of the robot. */ - + /** Updates the field relative position of the robot. + */ public void updateOdometry() { - m_poseEstimator.update( m_gyro.getRotation2d(), + m_poseEstimator.update( getRegGyro(), modules[0].getState(), modules[1].getState(), modules[2].getState(), From a75e98d911ec4c364c3a6817a46b2e25ae040ebc Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 19 Feb 2022 11:18:58 -0700 Subject: [PATCH 47/73] Merge conflict --- src/main/java/frc4388/robot/subsystems/SwerveModule.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index d9c7665..c2af4e3 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -91,16 +91,13 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); -<<<<<<< Updated upstream // Add this line to correct for the slight drive movement the angle motor makes when turning in place. // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); - driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); -======= + // driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC) + angleMotor.get()); ->>>>>>> Stashed changes } /** From 64481cb1cc22b497e90b3460fc988d3dad37a5e0 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 19 Feb 2022 13:32:39 -0700 Subject: [PATCH 48/73] edited aimtocenter --- src/main/java/frc4388/robot/commands/AimToCenter.java | 8 ++------ src/main/java/frc4388/robot/subsystems/Turret.java | 2 +- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 734b9c2..a284d8a 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -31,17 +31,13 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { - m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - aimTurret(m_targetAngle); - } - - public void aimTurret(double targetAngle) { //Split into configure and run - m_turret.runshooterRotatePID(targetAngle); + m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + m_turret.runshooterRotatePID(m_targetAngle); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 1abcfa1..5f785c5 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -9,6 +9,7 @@ import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; +import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; @@ -95,7 +96,6 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); } From 92777c105d3bc9c555ddbd48abeed13813b3a734 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Sat, 19 Feb 2022 15:05:49 -0700 Subject: [PATCH 49/73] updated aim to center --- .../frc4388/robot/commands/AimToCenter.java | 14 +++++++++- .../java/frc4388/robot/subsystems/Turret.java | 28 ++++++------------- 2 files changed, 22 insertions(+), 20 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index a284d8a..b63c5c5 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -36,7 +36,19 @@ public class AimToCenter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = m_turret.getBoomBoomAngleDegrees() + m_drive.gyro.getAngle() + Math.atan(y/x); + if (x > 0) { + m_targetAngle = 180 + Math.atan(y/x) - m_drive.gyro.getAngle(); + } + if (x < 0) { + m_targetAngle = 360 + Math.atan(y/x) - m_drive.gyro.getAngle(); + } + if (x == 0 && y > 0) { + m_targetAngle = 270 - m_drive.gyro.getAngle(); + } + if (x == 0 && y < 0) { + m_targetAngle = 90 - m_drive.gyro.getAngle(); + } + m_turret.runshooterRotatePID(m_targetAngle); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 5f785c5..530de78 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -42,8 +42,6 @@ public class Turret extends SubsystemBase { SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); - - //Variables public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument @@ -62,17 +60,18 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit m_boomBoomRotateMotor.setInverted(false); - + + m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); + m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); + m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); + m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); + m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); + m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); } @Override public void periodic() { - // SmartDashboard.putNumber("Turret Angle Raw", getboomBoomRotatePosition()); - - // SmartDashboard.putData("Turret Angle", (Sendable) m_turretGyro); - - // SmartDashboard.putBoolean("Turret Aimed", m_isAimReady); // This method will be called once per scheduler run } @@ -81,20 +80,11 @@ public class Turret extends SubsystemBase { m_sDriveSubsystem = subsystem1; } - public void runTurretWithInput(double input) { //Rename to turret + public void runTurretWithInput(double input) { m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); } - public void runshooterRotatePID(double targetAngle) { //Split into configure and run - m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); - m_boomBoomRotatePIDController.setI(m_shooterTGains.m_kI); - m_boomBoomRotatePIDController.setD(m_shooterTGains.m_kD); - m_boomBoomRotatePIDController.setFF(m_shooterTGains.m_kF); - m_boomBoomRotatePIDController.setIZone(m_shooterTGains.m_kIzone); - - - m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); - + public void runshooterRotatePID(double targetAngle) { targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); } From 97ba1e132ccc7ae345b6ee1fe54d90975f5bee1a Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 19 Feb 2022 15:48:39 -0700 Subject: [PATCH 50/73] AngleMotor + DriveMotor correction --- build.gradle | 4 ++-- src/main/java/frc4388/robot/Constants.java | 7 +++++-- src/main/java/frc4388/robot/RobotMap.java | 2 +- .../frc4388/robot/subsystems/SwerveModule.java | 16 ++++++++++------ 4 files changed, 18 insertions(+), 11 deletions(-) diff --git a/build.gradle b/build.gradle index 283effa..ccc227f 100644 --- a/build.gradle +++ b/build.gradle @@ -1,4 +1,4 @@ -import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO +import edu.wpi.first.gradlerio.deploy.roborio.RoboRIO plugins { id "java" @@ -28,7 +28,7 @@ deploy { frcJava(getArtifactTypeClass('FRCJavaArtifact')) { // Enable visualvm - jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" + //jvmArgs << "-Dcom.sun.management.jmxremote=true -Dcom.sun.management.jmxremote.port=1099 -Dcom.sun.management.jmxremote.local.only=false -Dcom.sun.management.jmxremote.ssl=false -Dcom.sun.management.jmxremote.authenticate=false -Djava.rmi.server.hostname=10.43.88.2" } // Static files artifact diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index aa9be76..e31c27d 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -53,6 +53,8 @@ public final class Constants { public static final int GYRO_ID = 14; // ofsets are in degrees + + //NATHAN if you truncate or round or simplify these i will cry public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0; public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0; public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0; @@ -79,10 +81,11 @@ public final class Constants { public static final int REMOTE_0 = 0; // conversions - // gear ratio: 5.12 rev motor = 1 rev wheel + // gear ratio: 5.14 rev motor = 1 rev wheel // wheel diameter: official = 4 in, measured = 3.8 in /* Ratio Calculation */ - public static final double MOTOR_REV_PER_WHEEL_REV = 5.12; + public static final double MOTOR_REV_PER_STEER_REV = 12.8; + public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857; public static final double WHEEL_DIAMETER_INCHES = 4.0; public static final double TICKS_PER_MOTOR_REV = 2048; public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index d2e1fde..926cfc6 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -91,7 +91,7 @@ public class RobotMap { rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - NeutralMode mode = NeutralMode.Brake; + NeutralMode mode = NeutralMode.Coast; leftFrontSteerMotor.setNeutralMode(mode); leftFrontWheelMotor.setNeutralMode(mode);//Coast rightFrontSteerMotor.setNeutralMode(mode); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index c2af4e3..6ec5a2f 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -91,13 +91,16 @@ public class SwerveModule extends SubsystemBase { double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - - // Add this line to correct for the slight drive movement the angle motor makes when turning in place. - // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69; // driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); - // driveMotor.set(TalonFXControlMode.Velocity, angleMotor.get() + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); - driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC) + angleMotor.get()); + driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + //driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));// - angleMotor.get()); + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1 + System.out.println(angleCorrection); + + // angleMotor.set(1.0); + // driveMotor.set(0.6375); } /** @@ -120,7 +123,8 @@ public class SwerveModule extends SubsystemBase { @Override public void periodic(){ Rotation2d currentRotation = getAngle(); - SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees()); + SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360); } } From 94f757080b116aaf09a1b15cec07d32f75d746fc Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Tue, 22 Feb 2022 16:56:36 -0700 Subject: [PATCH 51/73] 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 9440932..86ec010 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -85,8 +85,8 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - new JoystickButton(getDriverController(), XboxController.Button.kY.value) - // new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.Y_BUTTON) + // "XboxController.Button.kBack" was undefiened yet, 7 works just fine + new JoystickButton(getDriverController(), 7) .whenPressed(() -> m_robotSwerveDrive.resetGyro()); new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) From c2cd96040bfe5e5d0f7ac01ae8e1e5736bd12854 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E2=80=AEAarav=20Shah?= <76842@psdschools.org> Date: Tue, 22 Feb 2022 17:14:48 -0700 Subject: [PATCH 52/73] added check for deadzone --- src/main/java/frc4388/robot/commands/AimToCenter.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index b63c5c5..b24c657 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -50,6 +50,14 @@ public class AimToCenter extends CommandBase { } m_turret.runshooterRotatePID(m_targetAngle); + + } + public boolean noIsDeadzone(){ + if ((-20 < m_targetAngle) && (m_targetAngle < 0)) { + return false; + } else { + return true; + } } // Called once the command ends or is interrupted. From f97c7d8b9dcbd03ce7ebdfeae4cafe2a59b15f05 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 22 Feb 2022 17:10:21 -0700 Subject: [PATCH 53/73] refined aimtocenter command --- src/main/java/frc4388/robot/commands/AimToCenter.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index b24c657..592e008 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -16,7 +16,6 @@ public class AimToCenter extends CommandBase { // use odometry to find x and y later double x = 0; double y = 0; - double z = Math.sqrt(Math.pow(x, 2) + Math.pow(y, 2)); double m_targetAngle; // public static Gains m_aimGains; From 449a3dc8c45c06b69d224b278fa9dc2124bdd668 Mon Sep 17 00:00:00 2001 From: evan Date: Thu, 24 Feb 2022 18:12:57 -0700 Subject: [PATCH 54/73] some aimtocenter stuff, unfinished + has errors --- .../java/frc4388/robot/RobotContainer.java | 50 +++-- src/main/java/frc4388/robot/RobotMap.java | 205 +++++++++++------- .../frc4388/robot/commands/AimToCenter.java | 26 ++- .../java/frc4388/robot/subsystems/Turret.java | 41 ++-- 4 files changed, 187 insertions(+), 135 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4fc150f..e8cdbdf 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -14,6 +14,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; +import frc4388.robot.commands.AimToCenter; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; //import frc4388.robot.subsystems.Intake; @@ -37,23 +38,25 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); private final TalonFX m_testMotor = new TalonFX(2); - /* Subsystems - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, - m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, - m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, - m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, - m_robotMap.leftFrontEncoder, - m_robotMap.rightFrontEncoder, - m_robotMap.leftBackEncoder, - m_robotMap.rightBackEncoder - ); - */ + /* + * Subsystems + * private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( + * m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, + * m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, + * m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, + * m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, + * m_robotMap.leftFrontEncoder, + * m_robotMap.rightFrontEncoder, + * m_robotMap.leftBackEncoder, + * m_robotMap.rightBackEncoder + * ); + */ private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_) /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); @@ -78,7 +81,7 @@ public class RobotContainer { //Turret default command m_robotTurret.setDefaultCommand( - new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftXAxis()), m_robotTurret)); + new AimToCenter(m_robotTurret, m_drive) // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } @@ -101,21 +104,22 @@ public class RobotContainer { new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); - // activates "BoomBoom" - new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) - .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) - .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); + // activates "BoomBoom" + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) + .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); /* Driver Buttons */ - //activates intake + // activates intake new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON); - // .whenPressed() -> m_robot - /*operator button*/ - //activates hood + // .whenPressed() -> m_robot + /* operator button */ + // activates hood new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(() -> m_robotHood.runHood(0.5d)) - .whenReleased(() -> m_robotHood.runHood(0.d)); + .whenPressed(() -> m_robotHood.runHood(0.5d)) + .whenReleased(() -> m_robotHood.runHood(0.d)); // new JoystickButton(getOperatorJoystick()); } + /** * Use this to pass the autonomous command to the main {@link Robot} class. * diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 1e2b648..7aedfce 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -24,118 +24,167 @@ public class RobotMap { public RobotMap() { configureLEDMotorControllers(); - //configureSwerveMotorControllers(); + // configureSwerveMotorControllers(); } /* LED Subsystem */ public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); void configureLEDMotorControllers() { - + } - + /* Swerve Subsystem */ - // public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); - // public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); - // public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); - // public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - // public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - // public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - // public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - // public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - // public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - // public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - // public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - // public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - + public final WPI_TalonFX leftFrontSteerMotor; + // public final WPI_TalonFX leftFrontSteerMotor = new + // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); + // public final WPI_TalonFX leftFrontWheelMotor = new + // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); + // public final WPI_TalonFX rightFrontSteerMotor = new + // WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); + // public final WPI_TalonFX rightFrontWheelMotor = new + // WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + // public final WPI_TalonFX leftBackSteerMotor = new + // WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + // public final WPI_TalonFX leftBackWheelMotor = new + // WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + // public final WPI_TalonFX rightBackSteerMotor = new + // WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + // public final WPI_TalonFX rightBackWheelMotor = new + // WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); + // public final CANCoder leftFrontEncoder = new + // CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + // public final CANCoder rightFrontEncoder = new + // CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); + // public final CANCoder leftBackEncoder = new + // CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); + // public final CANCoder rightBackEncoder = new + // CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + // void configureSwerveMotorControllers() { - // leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - // rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); - // leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - // rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + // leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + // rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + // leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + // rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - // leftFrontSteerMotor.configFactoryDefault(); - // leftFrontWheelMotor.configFactoryDefault(); - // rightFrontSteerMotor.configFactoryDefault(); - // rightFrontWheelMotor.configFactoryDefault(); - // leftBackSteerMotor.configFactoryDefault(); - // leftBackWheelMotor.configFactoryDefault(); - // rightBackSteerMotor.configFactoryDefault(); - // rightBackWheelMotor.configFactoryDefault(); + // leftFrontSteerMotor.configFactoryDefault(); + // leftFrontWheelMotor.configFactoryDefault(); + // rightFrontSteerMotor.configFactoryDefault(); + // rightFrontWheelMotor.configFactoryDefault(); + // leftBackSteerMotor.configFactoryDefault(); + // leftBackWheelMotor.configFactoryDefault(); + // rightBackSteerMotor.configFactoryDefault(); + // rightBackWheelMotor.configFactoryDefault(); - // leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // // config cancoder as remote encoder for swerve steer motors - //leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - //} + // // config cancoder as remote encoder for swerve steer motors + // leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), + // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), + // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), + // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), + // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // } - //Shooter Config - /*Boom Boom Subsystem*/ + // Shooter Config + /* Boom Boom Subsystem */ public final WPI_TalonFX shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_LEFT_CAN_ID); public final WPI_TalonFX shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_RIGHT_CAN_ID); - + public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); + // Create motor CANSparkMax void ConfigureShooterMotorControllers() { - //LEFT FALCON + // LEFT FALCON shooterFalconLeft.configFactoryDefault(); shooterFalconLeft.setNeutralMode(NeutralMode.Coast); shooterFalconLeft.setInverted(true); shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + ShooterConstants.SHOOTER_TIMEOUT_MS); - - - //RIGHT FALCON + // RIGHT FALCON shooterFalconRight.setInverted(false); shooterFalconRight.setNeutralMode(NeutralMode.Coast); shooterFalconRight.configFactoryDefault(); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - //m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) - shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedLoopPeriod(0,ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - /*Turret Subsytem*/ - - - shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); - shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); + // m_shooterFalconRight.configPeakOutputForward(0, + // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, + ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, + ShooterConstants.SHOOTER_TIMEOUT_MS); + /* Turret Subsytem */ + + shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); + shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); } - } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 592e008..d463f07 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -17,7 +17,7 @@ public class AimToCenter extends CommandBase { double x = 0; double y = 0; double m_targetAngle; - + // public static Gains m_aimGains; public AimToCenter(Turret turret, SwerveDrive drive) { @@ -36,32 +36,34 @@ public class AimToCenter extends CommandBase { @Override public void execute() { if (x > 0) { - m_targetAngle = 180 + Math.atan(y/x) - m_drive.gyro.getAngle(); - } + m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle(); + } if (x < 0) { - m_targetAngle = 360 + Math.atan(y/x) - m_drive.gyro.getAngle(); - } + m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle(); + } if (x == 0 && y > 0) { - m_targetAngle = 270 - m_drive.gyro.getAngle(); + m_targetAngle = 270 - m_drive.gyro.getAngle(); } if (x == 0 && y < 0) { m_targetAngle = 90 - m_drive.gyro.getAngle(); } - + m_turret.runshooterRotatePID(m_targetAngle); } - public boolean noIsDeadzone(){ + + public boolean isDeadzone() { if ((-20 < m_targetAngle) && (m_targetAngle < 0)) { - return false; + return true; } else { - return true; + return false; } } - + // Called once the command ends or is interrupted. @Override - public void end(boolean interrupted) {} + public void end(boolean interrupted) { + } // Returns true when the command should end. @Override diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 530de78..4d7ac99 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -4,7 +4,6 @@ package frc4388.robot.subsystems; - import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; import com.revrobotics.CANEncoder; @@ -21,16 +20,14 @@ import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; import com.revrobotics.CANSparkMax.SoftLimitDirection; - - - public class Turret extends SubsystemBase { - + /** Creates a new Turret. */ public BoomBoom m_boomBoomSubsystem; public SwerveDrive m_sDriveSubsystem; - public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); + public CANSparkMax m_boomBoomRotateMotor;// = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, + // MotorType.kBrushless); public static Gains m_shooterTGains = ShooterConstants.SHOOTER_TURRET_GAINS; SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit; public Gyro m_turretGyro; @@ -41,16 +38,15 @@ public class Turret extends SubsystemBase { SparkMaxPIDController m_boomBoomRotatePIDController;// = m_boomBoomRotateMotor.getPIDController(); public RelativeEncoder m_boomBoomRotateEncoder;// = m_boomBoomRotateMotor.getEncoder(); - - //Variables - public Turret(CANSparkMax boomBoomRotateMotor) { //Take in rotate motor as an argument + + // Variables + public Turret(CANSparkMax boomBoomRotateMotor) { // Take in rotate motor as an argument m_boomBoomRotateMotor = boomBoomRotateMotor; m_boomBoomRotatePIDController = m_boomBoomRotateMotor.getPIDController(); m_boomBoomRotateEncoder = m_boomBoomRotateMotor.getEncoder(); m_boomBoomRotateMotor.setIdleMode(IdleMode.kBrake); - - + m_boomBoomLeftLimit = m_boomBoomRotateMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); @@ -58,7 +54,10 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); //Set second soft limit + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set + // second + // soft + // limit m_boomBoomRotateMotor.setInverted(false); m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); @@ -69,38 +68,36 @@ public class Turret extends SubsystemBase { m_boomBoomRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTGains.m_kPeakOutput); } - @Override public void periodic() { // This method will be called once per scheduler run } - public void passRequiredSubsystem( BoomBoom subsystem0, SwerveDrive subsystem1){ + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; } public void runTurretWithInput(double input) { - m_boomBoomRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); + m_boomBoomRotateMotor.set(input * ShooterConstants.TURRET_SPEED_MULTIPLIER); } public void runshooterRotatePID(double targetAngle) { - targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - m_boomBoomRotatePIDController.setReference(targetAngle,ControlType.kPosition); + targetAngle = targetAngle / ShooterConstants.DEGREES_PER_ROT; + m_boomBoomRotatePIDController.setReference(targetAngle, ControlType.kPosition); } - public void resetGyroShooterRotate() - { + public void resetGyroShooterRotate() { m_boomBoomRotateEncoder.setPosition(0); } - public double getboomBoomRotatePosition() - { + public double getboomBoomRotatePosition() { return m_boomBoomRotateEncoder.getPosition(); } public double getBoomBoomAngleDegrees() { - return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants. TURRET_MOTOR_ROTS_PER_ROT; + return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360 + / ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; } } \ No newline at end of file From 234586b6d5865db24855d8993bedf41d9dd821f0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 24 Feb 2022 18:54:58 -0700 Subject: [PATCH 55/73] took out pid stuff (not part of MVP), drives good --- .../frc4388/robot/subsystems/SwerveDrive.java | 9 ++- .../robot/subsystems/SwerveModule.java | 74 ++++++++++++++----- 2 files changed, 65 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index c456027..f725e06 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -153,6 +153,7 @@ public class SwerveDrive extends SubsystemBase { SwerveModuleState state = desiredStates[i]; module.setDesiredState(state, false); } + // modules[0].setDesiredState(desiredStates[0], false); } @Override @@ -160,7 +161,13 @@ public class SwerveDrive extends SubsystemBase { updateOdometry(); // SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus)); - SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + + SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw()); + + // SmartDashboard.putNumber("Front Left", modules[0].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Front Right", modules[1].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Back Left", modules[2].driveMotor.getSelectedSensorPosition()); + // SmartDashboard.putNumber("Back Right", modules[3].driveMotor.getSelectedSensorPosition()); // SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle()); // SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees()); // SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading}); diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 6ec5a2f..44afb70 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -7,6 +7,7 @@ package frc4388.robot.subsystems; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice; import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; @@ -21,14 +22,21 @@ import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; public class SwerveModule extends SubsystemBase { - private WPI_TalonFX driveMotor; - private WPI_TalonFX angleMotor; + public WPI_TalonFX angleMotor; + public WPI_TalonFX driveMotor; private CANCoder canCoder; public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS; private static double kEncoderTicksPerRotation = 4096; private SwerveModuleState state; private double canCoderFeedbackCoefficient; + + public long m_currentTime; + public long m_lastTime; + public double m_deltaTime; + + public double m_currentPos; + public double m_lastPos; /** Creates a new SwerveModule. */ public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) { @@ -49,17 +57,34 @@ public class SwerveModule extends SubsystemBase { angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0; angleMotor.configAllSettings(angleTalonFXConfiguration); - TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); - driveTalonFXConfiguration.slot0.kP = 0.1; - driveTalonFXConfiguration.slot0.kI = 0.0; - driveTalonFXConfiguration.slot0.kD = 0.0; - driveMotor.configAllSettings(driveTalonFXConfiguration); + // TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration(); + // driveTalonFXConfiguration.slot0.kP = 0.05; + // driveTalonFXConfiguration.slot0.kI = 0.0; + // driveTalonFXConfiguration.slot0.kD = 0.0; + // driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor; + driveMotor.configFactoryDefault(); + driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30); + driveMotor.configNominalOutputForward(0, 30); + driveMotor.configNominalOutputReverse(0, 30); + driveMotor.configPeakOutputForward(1, 30); + driveMotor.configPeakOutputReverse(-1, 30); + driveMotor.configAllowableClosedloopError(0, 0, 30); + driveMotor.config_kP(0, 0.5, 30); + driveMotor.config_kI(0, 0, 30); + driveMotor.config_kD(0, 0, 30); + // maybe try a feedforward value? + + // driveMotor.configAllSettings(driveTalonFXConfiguration); CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration(); canCoderConfiguration.magnetOffsetDegrees = offset; canCoder.configAllSettings(canCoderConfiguration); - } + m_currentTime = System.currentTimeMillis(); + m_lastTime = System.currentTimeMillis(); + + m_lastPos = driveMotor.getSelectedSensorPosition(); + } private Rotation2d getAngle() { // Note: This assumes the CANCoders are setup with the default feedback coefficient @@ -89,18 +114,33 @@ public class SwerveModule extends SubsystemBase { angleMotor.set(TalonFXControlMode.Position, desiredTicks); } - - double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond); - double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69; + double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond); + double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC; + // double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69; - // driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC); - driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); - //driveMotor.set((feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));// - angleMotor.get()); + // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + driveMotor.set(normFtPerSec);// - angleMotor.get()); // driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1 - System.out.println(angleCorrection); - // angleMotor.set(1.0); - // driveMotor.set(0.6375); + // m_currentTime = System.currentTimeMillis(); + // m_deltaTime = (double) (m_currentTime - m_lastTime); + // m_deltaTime = m_deltaTime / 10.0; + + // m_currentPos = driveMotor.getSelectedSensorPosition(); + + // double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity(); + // double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048; + // double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity()); + // double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10); + + // System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition()); + // System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos); + // System.out.println("Last Pos: " + m_lastPos); + + // driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/); + + // m_lastTime = m_currentTime; + // m_lastPos = m_currentPos; } /** From 5f1bed26cd59206016849661cccfddc04ca4d39a Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 24 Feb 2022 19:18:05 -0700 Subject: [PATCH 56/73] AimToCenter finished --- src/main/java/frc4388/robot/RobotContainer.java | 16 +++++++++++++--- src/main/java/frc4388/robot/RobotMap.java | 16 +++++++++++++++- 2 files changed, 28 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index e8cdbdf..ae911a6 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,7 +56,18 @@ public class RobotContainer { private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_) + private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, + m_robotMap.leftFrontWheelMotor, + m_robotMap.rightFrontSteerMotor, + m_robotMap.rightFrontWheelMotor, + m_robotMap.leftBackSteerMotor, + m_robotMap.leftBackWheelMotor, + m_robotMap.rightBackSteerMotor, + m_robotMap.rightBackWheelMotor, + m_robotMap.leftFrontEncoder, + m_robotMap.rightFrontEncoder, + m_robotMap.leftBackEncoder, + m_robotMap.rightBackEncoder); /* Controllers */ private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); @@ -80,8 +91,7 @@ public class RobotContainer { */ //Turret default command - m_robotTurret.setDefaultCommand( - new AimToCenter(m_robotTurret, m_drive) + m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive)); // m_robotTurret.setDefaultCommand( // new RunCommand(() -> m_robotTurret.aimToCenter())); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 7aedfce..86707d8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,6 +4,8 @@ package frc4388.robot; +import javax.swing.text.WrappedPlainView; + import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; @@ -35,7 +37,19 @@ public class RobotMap { } /* Swerve Subsystem */ - public final WPI_TalonFX leftFrontSteerMotor; + public final WPI_TalonFX leftFrontSteerMotor = null; + public final WPI_TalonFX leftFrontWheelMotor = null; + public final WPI_TalonFX rightFrontSteerMotor = null; + public final WPI_TalonFX rightFrontWheelMotor = null; + public final WPI_TalonFX leftBackSteerMotor = null; + public final WPI_TalonFX leftBackWheelMotor = null; + public final WPI_TalonFX rightBackSteerMotor = null; + public final WPI_TalonFX rightBackWheelMotor = null; + public final CANCoder leftFrontEncoder = null; + public final CANCoder rightFrontEncoder = null; + public final CANCoder leftBackEncoder = null; + public final CANCoder rightBackEncoder = null; + // public final WPI_TalonFX leftFrontSteerMotor = new // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); // public final WPI_TalonFX leftFrontWheelMotor = new From 038ef47841f62ee45aaad07a94a147dcdcae9cc0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Thu, 24 Feb 2022 19:26:58 -0700 Subject: [PATCH 57/73] Update AimToCenter.java --- src/main/java/frc4388/robot/commands/AimToCenter.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index d463f07..ede60bb 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -38,13 +38,13 @@ public class AimToCenter extends CommandBase { if (x > 0) { m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle(); } - if (x < 0) { + else if (x < 0) { m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle(); } - if (x == 0 && y > 0) { + else if (x == 0 && y > 0) { m_targetAngle = 270 - m_drive.gyro.getAngle(); } - if (x == 0 && y < 0) { + else if (x == 0 && y < 0) { m_targetAngle = 90 - m_drive.gyro.getAngle(); } @@ -53,7 +53,7 @@ public class AimToCenter extends CommandBase { } public boolean isDeadzone() { - if ((-20 < m_targetAngle) && (m_targetAngle < 0)) { + if ((-20 < m_targetAngle) && (m_targetAngle < 0)) { //Make both -20 and 0 Constants return true; } else { return false; From 31346ff646d7ebe3026fbcc01913a550339ab83e Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Thu, 24 Feb 2022 19:32:33 -0700 Subject: [PATCH 58/73] finished AimToCenter --- src/main/java/frc4388/robot/Constants.java | 17 +++-------------- .../frc4388/robot/commands/AimToCenter.java | 3 ++- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b23b51e..6dde32e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -83,12 +83,6 @@ public final class Constants { public static final int XBOX_OPERATOR_ID = 1; } - public static final class OdoAimConstants { - public static final double X_VECTOR_DISTANCE = 12.3; - // public static final Gains AIM_GAINS = new Gains(0, 0, 0, 0, 0, 0, 1); - - - } public static final class ShooterConstants { /* PID Constants Shooter */ public static final int CLOSED_LOOP_TIME_MS = 1; @@ -116,6 +110,9 @@ public final class Constants { public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; + public static final double DEADZONE_LEFT = -20.0; + public static final double DEADZONE_RIGHT = 0.0; + public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// @@ -133,13 +130,5 @@ public final class Constants { public static final double TARGET_HEIGHT = 67.5; public static final double FOV = 29.8; //Field of view limelight public static final double LIME_ANGLE = 24.7; - - - - - - - - } } diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index ede60bb..723bd88 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -5,6 +5,7 @@ package frc4388.robot.commands; import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; @@ -53,7 +54,7 @@ public class AimToCenter extends CommandBase { } public boolean isDeadzone() { - if ((-20 < m_targetAngle) && (m_targetAngle < 0)) { //Make both -20 and 0 Constants + if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) { return true; } else { return false; From 6e55d6fabddfa630dda97bfe29488a05dc22987a Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 25 Feb 2022 20:18:21 -0700 Subject: [PATCH 59/73] shooter tables and shoot command --- src/main/deploy/Robot Data - Distances.csv | 3 + .../frc4388/robot/commands/AimToCenter.java | 10 +- .../java/frc4388/robot/commands/Shoot.java | 58 +++++ .../frc4388/robot/subsystems/BoomBoom.java | 85 +++++-- .../java/frc4388/robot/subsystems/Hood.java | 30 +-- .../java/frc4388/robot/subsystems/Turret.java | 8 +- src/main/java/frc4388/utility/CSV.java | 240 ++++++++++++++++++ .../java/frc4388/utility/ShooterTables.java | 164 ------------ 8 files changed, 385 insertions(+), 213 deletions(-) create mode 100644 src/main/deploy/Robot Data - Distances.csv create mode 100644 src/main/java/frc4388/robot/commands/Shoot.java create mode 100644 src/main/java/frc4388/utility/CSV.java delete mode 100644 src/main/java/frc4388/utility/ShooterTables.java diff --git a/src/main/deploy/Robot Data - Distances.csv b/src/main/deploy/Robot Data - Distances.csv new file mode 100644 index 0000000..99413bc --- /dev/null +++ b/src/main/deploy/Robot Data - Distances.csv @@ -0,0 +1,3 @@ +Distance (in) ,Hood Ext. (u) ,Drum Velocity (u/ds) +0 ,16 ,12000 +999 ,28.8 ,12000 \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 723bd88..9a90cb5 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -53,12 +53,12 @@ public class AimToCenter extends CommandBase { } + /** + * Checks if in deadzone. + * @return True if deadzone. + */ public boolean isDeadzone() { - if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) { - return true; - } else { - return false; - } + return ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java new file mode 100644 index 0000000..2f8c5ef --- /dev/null +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -0,0 +1,58 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import java.nio.file.SecureDirectoryStream; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.BoomBoom; +import frc4388.robot.subsystems.Hood; +import frc4388.robot.subsystems.SwerveDrive; + +public class Shoot extends CommandBase { + + public SwerveDrive m_swerve; + public BoomBoom m_boomBoom; + public Hood m_hood; + + public double m_targetVel; + public double m_targetHood; + public double m_distance; + + /** Creates a new Shoot. */ + public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Hood sHood) { + // Use addRequirements() here to declare subsystem dependencies. + m_swerve = sDrive; + m_boomBoom = sShooter; + m_hood = sHood; + + addRequirements(m_swerve, m_boomBoom, m_hood); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + m_distance = 0; //TODO: get this value using odometry + m_targetVel = m_boomBoom.getVelocity(m_distance); + m_targetHood = m_boomBoom.getHood(m_distance); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_hood.runAngleAdjustPID(m_targetHood); + m_boomBoom.runDrumShooterVelocityPID(m_targetVel); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 01c850c..9dfd332 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -4,28 +4,29 @@ package frc4388.robot.subsystems; -import java.io.Console; -import java.util.Base64.Encoder; +import java.io.File; +import java.io.IOException; +import java.util.Comparator; +import java.util.Map; +import java.util.Optional; +import java.util.function.Function; +import java.util.regex.Pattern; +import java.util.stream.IntStream; -import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; -import edu.wpi.first.wpilibj.simulation.JoystickSim; -import edu.wpi.first.wpilibj.smartdashboard.*; -import edu.wpi.first.math.controller.BangBangController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; +import edu.wpi.first.wpilibj.Filesystem; +import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; -import frc4388.utility.ShooterTables; +import frc4388.utility.CSV; import frc4388.utility.Gains; import frc4388.utility.controller.IHandController; -import com.revrobotics.RelativeEncoder; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft; public WPI_TalonFX m_shooterFalconRight; -public ShooterTables m_shooterTable; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 @@ -42,7 +43,11 @@ public Turret m_turretSubsystem; // SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(69, 42, 0); //get real values later +public static class ShooterTableEntry { + public Double distance, hoodExt, drumVelocity; +} +private ShooterTableEntry[] m_shooterTable; /* * Creates new BoomBoom subsystem, has drum shooter and angle adjuster @@ -51,27 +56,55 @@ public Turret m_turretSubsystem; public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) { m_shooterFalconLeft = shooterFalconLeft; m_shooterFalconRight = shooterFalconRight; - m_shooterTable = new ShooterTables(); + + + try { + CSV csv = new CSV<>(ShooterTableEntry::new) { + private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); + + @Override + protected String headerSanitizer(final String header) { + return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); + } + + }; + m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()); + new Thread(() -> System.out.println(CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start(); + } catch (final IOException e) { + e.printStackTrace(); + // throw new RuntimeException(e); + } +} + +public Double getVelocity(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); +} + +public Double getHood(final Double distance) { + return linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue(); +} + +private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { + final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); + final E closestRecord = closestEntry.getValue(); + final int closestRecordIndex = closestEntry.getKey(); + final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; + return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); +} + +private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { + final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); + return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); +} + +private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { + final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); + return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); } - @Override public void periodic() { // This method will be called once per scheduler run - -try { - // SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); - - // SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - - // SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); //all these values should be "defined" elsewhere, fix this - - // SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); - - // SmartDashboard.putNumber("Drum Ready", m_isDrumReady); -} catch (Exception e) { - //TODO: handle exception -} } diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 046d402..72bb3f8 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -23,13 +23,13 @@ import frc4388.utility.Gains; public class Hood extends SubsystemBase { public BoomBoom m_shooterSubsystem; - //public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); + public CANSparkMax m_angleAdjusterMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); public SparkMaxLimitSwitch m_hoodUpLimitSwitch; public SparkMaxLimitSwitch m_hoodDownLimitSwitch; public static Gains m_angleAdjusterGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - // public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); + public RelativeEncoder m_angleEncoder = m_angleAdjusterMotor.getEncoder(); - // public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); + public SparkMaxPIDController m_angleAdjusterPIDController = m_angleAdjusterMotor.getPIDController(); //public boolean m_isHoodReady = false; @@ -39,12 +39,12 @@ public double m_fireAngle; /** Creates a new Hood. */ public Hood() { - // m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); + m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_hoodUpLimitSwitch.enableLimitSwitch(true); - // m_hoodDownLimitSwitch.enableLimitSwitch(true); + m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + m_hoodUpLimitSwitch.enableLimitSwitch(true); + m_hoodDownLimitSwitch.enableLimitSwitch(true); } @@ -55,14 +55,14 @@ public double m_fireAngle; public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients - // m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); - // m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); - // m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); - // m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); - // m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); - // m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); + m_angleAdjusterPIDController.setP(m_angleAdjusterGains.m_kP); + m_angleAdjusterPIDController.setI(m_angleAdjusterGains.m_kI); + m_angleAdjusterPIDController.setD(m_angleAdjusterGains.m_kD); + m_angleAdjusterPIDController.setIZone(m_angleAdjusterGains.m_kIzone); + m_angleAdjusterPIDController.setFF(m_angleAdjusterGains.m_kF); + m_angleAdjusterPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjusterGains.m_kPeakOutput); - // m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); + m_angleAdjusterPIDController.setReference(targetAngle, ControlType.kPosition); } diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 4d7ac99..28c9583 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -51,10 +51,12 @@ public class Turret extends SubsystemBase { m_boomBoomRightLimit = m_boomBoomRotateMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_boomBoomRightLimit.enableLimitSwitch(true); m_boomBoomLeftLimit.enableLimitSwitch(true); + SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); + SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set + // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); + // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); + // m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set // second // soft // limit diff --git a/src/main/java/frc4388/utility/CSV.java b/src/main/java/frc4388/utility/CSV.java new file mode 100644 index 0000000..522980a --- /dev/null +++ b/src/main/java/frc4388/utility/CSV.java @@ -0,0 +1,240 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018-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.utility; + +import java.awt.Color; +import java.io.BufferedReader; +import java.io.IOException; +import java.lang.invoke.MethodHandleProxies; +import java.lang.invoke.MethodHandles; +import java.lang.invoke.MethodType; +import java.lang.reflect.Array; +import java.lang.reflect.Field; +import java.lang.reflect.Modifier; +import java.nio.file.Files; +import java.nio.file.Path; +import java.text.MessageFormat; +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; +import java.util.Objects; +import java.util.function.BiConsumer; +import java.util.function.Function; +import java.util.function.IntFunction; +import java.util.function.Predicate; +import java.util.function.Supplier; +import java.util.regex.Pattern; +import java.util.stream.Collectors; +import java.util.stream.IntStream; +import java.util.stream.Stream; + +public class CSV { + private static final Pattern SANITIZER = Pattern.compile("[^$\\w,]"); + + private final Supplier generator; + private final IntFunction arrayGenerator; + private final Map> setters; + + /** + * A binary string operator to be applied to the entire header of the CSV. + */ + protected String headerSanitizer(final String header) { + return SANITIZER.matcher(header).replaceAll(""); + } + + /** + * A binary string operator to be applied to each name in the header of the CSV. + */ + protected String nameProcessor(final String name) { + return Character.toLowerCase(name.charAt(0)) + name.substring(1); + } + + /** + * Creates a new {@code CSV} instance and prepares for populating the fields of objects created by + * the given generator. Private fields and fields of primitive types are not supported. + * @param generator a parameterless supplier which produces a new object with any number of fields + * corresponding to header names from a CSV file. The first character of the names + * from the header in the CSV file will be made lowercase and invalid characters + * will be removed to match Java naming conventions. + * @see #read(Path) + */ + @SuppressWarnings("unchecked") + public CSV(final Supplier generator) { + final Class clazz = generator.get().getClass(); + final Map, Function> fieldParsers = new HashMap<>(); + this.arrayGenerator = size -> (R[]) Array.newInstance(clazz, size); + this.generator = generator; + this.setters = new HashMap<>(); + for (final Field field : clazz.getFields()) { + final Function parser = Modifier.isStatic(field.getModifiers()) ? null : fieldParsers.computeIfAbsent(field.getType(), CSV::getTypeParser); + if (parser != null) + this.setters.put(field.getName(), (final R obj, final String rawValue) -> { + try { + field.set(obj, rawValue.isEmpty() ? null : parser.apply(rawValue)); + } catch (final IllegalAccessException e) { + throw new RuntimeException(e); + } + }); + } + } + + /** + * Reads and parses the contents of the given CSV file, and returns an array filled with populated + * objects created with the previously given generator. Cells are parsed using their corresponding + * field's {@code valueOf(String)} function. + * @param path the path to a CSV file + * @return the parsed data from the CSV file + * @throws IOException if an I/O error occurs opening the file + */ + @SuppressWarnings("unchecked") + public R[] read(final Path path) throws IOException { + try (final BufferedReader reader = Files.newBufferedReader(path)) { + final BiConsumer[] fieldSetters = Stream.of(headerSanitizer(reader.readLine()).split(",")).map(this::nameProcessor).map(setters::get).toArray(BiConsumer[]::new); + final Stream lines = reader.lines(); + return lines.filter(Predicate.not(String::isBlank)).map(line -> deserializeRecordString(line, fieldSetters, generator.get())).toArray(this.arrayGenerator); + } + } + + @SuppressWarnings("unchecked") + private static Function getTypeParser(final Class type) { + try { + return type.isAssignableFrom(String.class) ? Function.identity() : MethodHandleProxies.asInterfaceInstance(Function.class, MethodHandles.publicLookup().findStatic(type, "valueOf", MethodType.methodType(type, String.class))); + } catch (final NoSuchMethodException | IllegalAccessException e) { + return null; + } + } + + private static R deserializeRecordString(final String recordString, final BiConsumer[] fieldParseSetters, final R object) { + final int recordStringLength = recordString.length(); + int fieldBeginIndex = 0, tryFieldEndFromIndex = 0, i = 0; + while (tryFieldEndFromIndex < recordStringLength && i < fieldParseSetters.length) { + final int tryFieldEndIndex = recordString.indexOf(',', tryFieldEndFromIndex); + String field = recordString.substring(fieldBeginIndex, tryFieldEndIndex == -1 ? recordStringLength : tryFieldEndIndex).strip(); + if (!field.isEmpty() && (tryFieldEndFromIndex != fieldBeginIndex || field.charAt(0) == '"')) { + final int fieldLength = field.length(); + if (countTrailing(field, '"') % 2 == 0) { + tryFieldEndFromIndex = tryFieldEndIndex + 1; + continue; + } else + field = field.substring(1, fieldLength - 1).replace("\"\"", "\""); + } + final BiConsumer setter = fieldParseSetters[i]; + if (setter != null) + setter.accept(object, field); + tryFieldEndFromIndex = fieldBeginIndex = tryFieldEndIndex + 1; + i++; + } + return object; + } + + private static int countTrailing(final String str, final char c) { + final int l = str.length(); + int count = 0; + while (str.charAt(l - count - 1) == c && count < l) + count++; + return count; + } + + public static final class ReflectionTable { + public static String create(final T[] objects, boolean colored) { + final Field[] fields = Stream.of(objects).flatMap(object -> Stream.of(object.getClass().getFields())).distinct().toArray(Field[]::new); + final List> rows = new ArrayList<>(); + rows.add(Stream.of(fields).map(ReflectionTable::new).collect(Collectors.toList())); + rows.addAll(Stream.of(objects).map(obj -> Stream.of(fields).map(field -> new ReflectionTable(obj, field)).collect(Collectors.toList())).collect(Collectors.toList())); + final int[] columnWidths = rows.stream().map(row -> row.stream().map(cell -> cell.string).mapToInt(String::length).toArray()).reduce(new int[fields.length], (result, row) -> IntStream.range(0, row.length).map(i -> Math.max(result[i], row[i])).toArray()); + if (colored) + IntStream.range(0, fields.length).forEach(i -> { + final var columnSummaryStatistics = rows.stream().skip(1).mapToDouble(row -> row.get(i).getValue().doubleValue()).summaryStatistics(); + rows.stream().skip(1).forEach(row -> row.get(i).colorByValue(columnSummaryStatistics.getMin(), columnSummaryStatistics.getMax())); + }); + MessageFormat formatFormat = new MessageFormat(colored ? "{2} %{0}{1}s {3}" : " %{0}{1}s "); + return rows.stream().map(row -> IntStream.range(0, row.size()).mapToObj(i -> String.format(formatFormat.format(new Object[] { row.get(i).padRight ? "-" : "", columnWidths[i], row.get(i).escape, RESET_STYLE }), row.get(i).string)).collect(Collectors.joining("|"))).collect(Collectors.joining(LF)); + } + + private static final Color GRADIENT_MIN = new Color(0x00, 0x99, 0x00); + private static final Color GRADIENT_MAX = new Color(0x66, 0xFF, 0x66); + private static final String CONTROL = "\033"; + private static final String CSI = "["; + private static final String LF = "\n"; + private static final String RESET = "0"; + private static final String BOLD = "1"; + private static final String ITALIC = "3"; + private static final String UNDERLINE = "4"; + private static final String BACKGROUND_RED = "41"; + private static final String FOREGROUND = "38"; + private static final String BACKGROUND = "48"; + private static final String TRUECOLOR = "2"; + private static final String SEPARATOR = ";"; + private static final String SGR = "m"; + private static final String HEADER_STYLE = CONTROL + CSI + BOLD + SEPARATOR + UNDERLINE + SGR; + private static final String NULL_STYLE = CONTROL + CSI + ITALIC + SGR; + private static final String ERROR_STYLE = CONTROL + CSI + ITALIC + SGR + CONTROL + CSI + BACKGROUND_RED + SGR; + private static final String RESET_STYLE = CONTROL + CSI + RESET + SGR; + private Object value; + private String string; + private boolean padRight; + private String escape; + + private ReflectionTable(final Object obj, final Field field) { + try { + value = field.get(obj); + string = Objects.toString(value); + padRight = !Number.class.isAssignableFrom(field.getType()); + escape = Objects.isNull(value) ? NULL_STYLE : ""; + } catch (final IllegalAccessException | IllegalArgumentException e) { + value = null; + string = e.getClass().getSimpleName(); + padRight = false; + escape = ERROR_STYLE; + } + } + + private ReflectionTable(final Field field) { + value = null; + string = field.getName(); + padRight = true; + escape = HEADER_STYLE; + } + + private Number getValue() { + return padRight ? Objects.hashCode(string) : Objects.requireNonNullElse((Number) value, 0); + } + + private void colorByValue(final Number min, final Number max) { + if (Objects.nonNull(value)) { + final double range = max.doubleValue() - min.doubleValue(); + final double normal = range == 0 ? 0 : (getValue().doubleValue() - min.doubleValue()) / range; + final Color color = new Color(range(normal, GRADIENT_MIN.getRed(), GRADIENT_MAX.getRed()), range(normal, GRADIENT_MIN.getGreen(), GRADIENT_MAX.getGreen()), range(normal, GRADIENT_MIN.getBlue(), GRADIENT_MAX.getBlue())); + escape += (contrastRatio(color, Color.BLACK) > contrastRatio(Color.WHITE, color) ? colorTo24BitSGR(Color.BLACK, false) : colorTo24BitSGR(Color.WHITE, false)) + colorTo24BitSGR(color, true); + } + } + + private static String colorTo24BitSGR(final Color color, final boolean background) { + return CONTROL + CSI + (background ? BACKGROUND : FOREGROUND) + SEPARATOR + TRUECOLOR + SEPARATOR + color.getRed() + SEPARATOR + color.getGreen() + SEPARATOR + color.getBlue() + SGR; + } + + private static int range(final double normal, final int min, final int max) { + return (int) (normal * (max - min) + min); + } + + /* https://www.w3.org/TR/WCAG20/#contrast-ratiodef */ + private static float contrastRatio(final Color lighter, final Color darker) { + return (relativeLuminance(lighter) + 0.05f) / (relativeLuminance(darker) + 0.05f); + } + + /* https://www.w3.org/TR/2008/REC-WCAG20-20081211/#relativeluminancedef */ + private static float relativeLuminance(final Color color) { + final float[] components = color.getRGBComponents(null); + final float r = components[0] <= 0.03928f ? components[0] / 12.92f : (float) Math.pow((components[0] + 0.055f) / 1.055f, 2.4f); + final float g = components[1] <= 0.03928f ? components[1] / 12.92f : (float) Math.pow((components[1] + 0.055f) / 1.055f, 2.4f); + final float b = components[2] <= 0.03928f ? components[2] / 12.92f : (float) Math.pow((components[2] + 0.055f) / 1.055f, 2.4f); + return 0.2126f * r + 0.7152f * g + 0.0722f * b; + } + } +} diff --git a/src/main/java/frc4388/utility/ShooterTables.java b/src/main/java/frc4388/utility/ShooterTables.java deleted file mode 100644 index 2a5ce92..0000000 --- a/src/main/java/frc4388/utility/ShooterTables.java +++ /dev/null @@ -1,164 +0,0 @@ -package frc4388.utility; - -import java.io.BufferedReader; -import java.io.File; -import java.io.FileNotFoundException; -import java.io.FileReader; -import java.io.IOException; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -/** - * Add your docs here. - */ -public class ShooterTables { - double[][] m_distance = new double[50][4]; - double[][] m_angle = new double[50][2]; - - final int m_columnDistance = 0; - final int m_columnHood = 1; - final int m_columnVel = 2; - final int m_columnCenterDisplacement = 3; - final int m_columnAngle = 0; - final int m_columnDisplacement = 1; - - int m_distanceLength; - int m_angleLength; - - public ShooterTables() { - int lineNum = 0; - - File distanceCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Distances.csv"); - File angleCSV = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/Robot Data - Angles.csv"); - - try { - - - BufferedReader distanceReader = new BufferedReader(new FileReader(distanceCSV)); - BufferedReader angleReader = new BufferedReader(new FileReader(angleCSV)); - String line = ""; - - while ((line = distanceReader.readLine()) != null) { - - if (lineNum == 0) { - lineNum ++; - } else { - String[] values = line.split(","); - - m_distance[lineNum - 1][m_columnDistance] = Double.parseDouble(values[0]); - m_distance[lineNum - 1][m_columnHood] = Double.parseDouble(values[1]); - m_distance[lineNum - 1][m_columnVel] = Double.parseDouble(values[2]); - m_distance[lineNum - 1][m_columnCenterDisplacement] = Double.parseDouble(values[3]); - - lineNum ++; - } - - } - - m_distanceLength = lineNum-1; - lineNum = 0; - - while ((line = angleReader.readLine()) != null) { - - if (lineNum == 0) { - lineNum ++; - } else { - String[] values = line.split(","); - - m_angle[lineNum - 1][m_columnAngle] = Double.parseDouble(values[0]); - m_angle[lineNum - 1][m_columnDisplacement] = Double.parseDouble(values[1]); - - lineNum ++; - } - } - - m_angleLength = lineNum-1; - - distanceReader.close(); - angleReader.close(); - - } catch (FileNotFoundException e) { - e.printStackTrace(); - } catch (IOException e) { - e.printStackTrace(); - } - //SmartDashboard.putNumber("Row 2 Column 1", m_angle[1][0]); - //SmartDashboard.putNumber("Row 4 Column 2", m_angle[3][1]); - //SmartDashboard.putNumber("m_distanceLength", m_distanceLength); - //SmartDashboard.putNumber("Distance last row 0", m_distance[m_distanceLength-1][0]); - //SmartDashboard.putNumber("Distance last row 1", m_distance[m_distanceLength-1][1]); - //SmartDashboard.putNumber("Distance last row 2", m_distance[m_distanceLength-1][2]); - } - - public double getHood(double distance) { //Rotations of motor - int i = 0; - while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { - i ++; - } - if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { - return m_distance[i][m_columnHood]; - } else { - if (i >= m_distanceLength) { - i = m_distanceLength - 1; - } - return linearInterpolation(i, distance, m_columnHood, m_distance); - } - } - - public double getVelocity(double distance) { //Units per 100ms - int i = 0; - while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { - i ++; - } - if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { - return m_distance[i][m_columnVel]; - } else { - if (i >= m_distanceLength) { - i = m_distanceLength - 1; - } - return linearInterpolation(i, distance, m_columnVel, m_distance); - } - } - - public double getCenterDisplacement(double distance) { //Degrees of Lime FOV - int i = 0; - while ((i < m_distanceLength) && (m_distance[i][m_columnDistance] < distance)) { - i ++; - } - if ((i < m_distanceLength) && (m_distance[i][m_columnDistance] == distance)) { - return m_distance[i][m_columnCenterDisplacement]; - } else { - if (i >= m_distanceLength) { - i = m_distanceLength - 1; - } - return linearInterpolation(i, distance, m_columnCenterDisplacement, m_distance); - } - } - - - - public double getAngleDisplacement(double angle) { - int i = 0; - while ((i < m_angleLength) && (m_angle[i][m_columnAngle] < angle)) { - i ++; - } - if ((i < m_angleLength) && (m_angle[i][m_columnAngle] == angle)) { - return m_angle[i][m_columnDisplacement]; - } else { - if (i >= m_angleLength) { - i = m_angleLength - 1; - } - return linearInterpolation(i, angle, m_columnDisplacement, m_angle); - } - } - - public double linearInterpolation(int i, double value, int column, double[][] table) { - if (i != 0) { - double slope = (table[i][column] - table[i-1][column]) / (table[i][0] - table[i-1][0]); - value = slope * (value - table[i-1][0]) + table[i-1][column]; - return value; - } - return 0.0; - } -} \ No newline at end of file From 350cf33dd01e51b78ca5df6ea29884fd495ff347 Mon Sep 17 00:00:00 2001 From: Ryan Manley <42309874+ryan123rudder@users.noreply.github.com> Date: Sat, 26 Feb 2022 13:48:50 -0700 Subject: [PATCH 60/73] Cleaned up Math, added unit testing to AimToCenter --- simgui-ds.json | 3 +- simgui.json | 8 + src/main/deploy/Distances.csv | 0 src/main/java/frc4388/robot/Constants.java | 44 ++-- src/main/java/frc4388/robot/Robot.java | 1 + src/main/java/frc4388/robot/RobotMap.java | 213 +++++++++--------- .../frc4388/robot/commands/AimToCenter.java | 33 ++- .../frc4388/commands/AimToCenterTest.java | 113 ++++++++++ .../robot/subsystems/LEDSubsystemTest.java | 12 +- 9 files changed, 262 insertions(+), 165 deletions(-) create mode 100644 src/main/deploy/Distances.csv create mode 100644 src/test/java/frc4388/commands/AimToCenterTest.java diff --git a/simgui-ds.json b/simgui-ds.json index b16ea5c..0c5f2e8 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -91,10 +91,9 @@ ], "robotJoysticks": [ { - "useGamepad": true + "guid": "78696e70757401000000000000000000" }, { - "guid": "78696e70757401000000000000000000", "useGamepad": true } ] diff --git a/simgui.json b/simgui.json index 72aab8f..280daf8 100644 --- a/simgui.json +++ b/simgui.json @@ -30,5 +30,13 @@ "/LiveWindow/Ungrouped/Scheduler": "Scheduler", "/LiveWindow/Vision": "Subsystem" } + }, + "NetworkTables": { + "SmartDashboard": { + "Recording": { + "open": true + }, + "open": true + } } } diff --git a/src/main/deploy/Distances.csv b/src/main/deploy/Distances.csv new file mode 100644 index 0000000..e69de29 diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 6dde32e..5d61976 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -31,28 +31,28 @@ public final class Constants { public static final double JOYSTICK_TO_METERS_PER_SECOND = 5; public static final double MAX_SPEED_FEET_PER_SEC = 16; public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; - // public static final int LEFT_FRONT_STEER_CAN_ID = 2; - // public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; - // public static final int RIGHT_FRONT_STEER_CAN_ID = 4; - // public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; - // public static final int LEFT_BACK_STEER_CAN_ID = 6; - // public static final int LEFT_BACK_WHEEL_CAN_ID = 7; - // public static final int RIGHT_BACK_STEER_CAN_ID = 8; - // public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; - // public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; - // public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; - // public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; - // public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; + public static final int LEFT_FRONT_STEER_CAN_ID = 2; + public static final int LEFT_FRONT_WHEEL_CAN_ID = 3; + public static final int RIGHT_FRONT_STEER_CAN_ID = 4; + public static final int RIGHT_FRONT_WHEEL_CAN_ID = 5; + public static final int LEFT_BACK_STEER_CAN_ID = 6; + public static final int LEFT_BACK_WHEEL_CAN_ID = 7; + public static final int RIGHT_BACK_STEER_CAN_ID = 8; + public static final int RIGHT_BACK_WHEEL_CAN_ID = 9; + public static final int LEFT_FRONT_STEER_CAN_ENCODER_ID = 10; + public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11; + public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12; + public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13; // ofsets are in degrees //ofsets are in degrees - // public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; - // public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; - // public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531; - // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594; - public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; - public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; - public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; - public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; + public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141; + public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281; + public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531; + public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594; + // public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0; + // public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0; + // public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0; + // public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0; // swerve PID constants public static final int SWERVE_SLOT_IDX = 0; @@ -110,8 +110,8 @@ public final class Constants { public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double DEADZONE_LEFT = -20.0; - public static final double DEADZONE_RIGHT = 0.0; + public static final double DEADZONE_LEFT = 0.0; + public static final double DEADZONE_RIGHT = 340.0; public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 119a032..bf9b82c 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -30,6 +30,7 @@ public class Robot extends TimedRobot { public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. + //AimToCenterTest.RunAll(); m_robotContainer = new RobotContainer(); } diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 86707d8..29ebda8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -7,6 +7,7 @@ package frc4388.robot; import javax.swing.text.WrappedPlainView; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; @@ -37,124 +38,112 @@ public class RobotMap { } /* Swerve Subsystem */ - public final WPI_TalonFX leftFrontSteerMotor = null; - public final WPI_TalonFX leftFrontWheelMotor = null; - public final WPI_TalonFX rightFrontSteerMotor = null; - public final WPI_TalonFX rightFrontWheelMotor = null; - public final WPI_TalonFX leftBackSteerMotor = null; - public final WPI_TalonFX leftBackWheelMotor = null; - public final WPI_TalonFX rightBackSteerMotor = null; - public final WPI_TalonFX rightBackWheelMotor = null; - public final CANCoder leftFrontEncoder = null; - public final CANCoder rightFrontEncoder = null; - public final CANCoder leftBackEncoder = null; - public final CANCoder rightBackEncoder = null; - - // public final WPI_TalonFX leftFrontSteerMotor = new - // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); - // public final WPI_TalonFX leftFrontWheelMotor = new - // WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); - // public final WPI_TalonFX rightFrontSteerMotor = new - // WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); - // public final WPI_TalonFX rightFrontWheelMotor = new - // WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); - // public final WPI_TalonFX leftBackSteerMotor = new - // WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); - // public final WPI_TalonFX leftBackWheelMotor = new - // WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); - // public final WPI_TalonFX rightBackSteerMotor = new - // WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); - // public final WPI_TalonFX rightBackWheelMotor = new - // WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); - // public final CANCoder leftFrontEncoder = new - // CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); - // public final CANCoder rightFrontEncoder = new - // CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); - // public final CANCoder leftBackEncoder = new - // CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); - // public final CANCoder rightBackEncoder = new - // CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); + // public final WPI_TalonFX leftFrontSteerMotor = null; + // public final WPI_TalonFX leftFrontWheelMotor = null; + // public final WPI_TalonFX rightFrontSteerMotor = null; + // public final WPI_TalonFX rightFrontWheelMotor = null; + // public final WPI_TalonFX leftBackSteerMotor = null; + // public final WPI_TalonFX leftBackWheelMotor = null; + // public final WPI_TalonFX rightBackSteerMotor = null; + // public final WPI_TalonFX rightBackWheelMotor = null; + // public final CANCoder leftFrontEncoder = null; + // public final CANCoder rightFrontEncoder = null; + // public final CANCoder leftBackEncoder = null; + // public final CANCoder rightBackEncoder = null; - // void configureSwerveMotorControllers() { - // leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - // rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); - // leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - // rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX rightFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ID); + public final WPI_TalonFX rightFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_FRONT_WHEEL_CAN_ID); + public final WPI_TalonFX leftBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ID); + public final WPI_TalonFX leftBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_BACK_WHEEL_CAN_ID); + public final WPI_TalonFX rightBackSteerMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ID); + public final WPI_TalonFX rightBackWheelMotor = new WPI_TalonFX(SwerveDriveConstants.RIGHT_BACK_WHEEL_CAN_ID); + public final CANCoder leftFrontEncoder = new CANCoder(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder rightFrontEncoder = new CANCoder(SwerveDriveConstants.RIGHT_FRONT_STEER_CAN_ENCODER_ID); + public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID); + public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); - // leftFrontSteerMotor.configFactoryDefault(); - // leftFrontWheelMotor.configFactoryDefault(); - // rightFrontSteerMotor.configFactoryDefault(); - // rightFrontWheelMotor.configFactoryDefault(); - // leftBackSteerMotor.configFactoryDefault(); - // leftBackWheelMotor.configFactoryDefault(); - // rightBackSteerMotor.configFactoryDefault(); - // rightBackWheelMotor.configFactoryDefault(); + void configureSwerveMotorControllers() { + leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - // leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configFactoryDefault(); + leftFrontWheelMotor.configFactoryDefault(); + rightFrontSteerMotor.configFactoryDefault(); + rightFrontWheelMotor.configFactoryDefault(); + leftBackSteerMotor.configFactoryDefault(); + leftBackWheelMotor.configFactoryDefault(); + rightBackSteerMotor.configFactoryDefault(); + rightBackWheelMotor.configFactoryDefault(); - // leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // // config cancoder as remote encoder for swerve steer motors - // leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), - // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), - // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), - // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), - // RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - // SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // } + leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + + // config cancoder as remote encoder for swerve steer motors + leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + } // Shooter Config /* Boom Boom Subsystem */ diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 723bd88..7b4300f 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -17,6 +17,7 @@ public class AimToCenter extends CommandBase { // use odometry to find x and y later double x = 0; double y = 0; + double angle = 0; double m_targetAngle; // public static Gains m_aimGains; @@ -31,34 +32,26 @@ public class AimToCenter extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { + x = 0; + y = 0; } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (x > 0) { - m_targetAngle = 180 + Math.atan(y / x) - m_drive.gyro.getAngle(); - } - else if (x < 0) { - m_targetAngle = 360 + Math.atan(y / x) - m_drive.gyro.getAngle(); - } - else if (x == 0 && y > 0) { - m_targetAngle = 270 - m_drive.gyro.getAngle(); - } - else if (x == 0 && y < 0) { - m_targetAngle = 90 - m_drive.gyro.getAngle(); - } - + m_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle()); m_turret.runshooterRotatePID(m_targetAngle); - } - public boolean isDeadzone() { - if ((ShooterConstants.DEADZONE_LEFT < m_targetAngle) && (m_targetAngle < ShooterConstants.DEADZONE_RIGHT)) { - return true; - } else { - return false; - } + public static double angleToCenter(double x, double y, double gyro) { + double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + return(angle); + //m_turret.runshooterRotatePID(m_targetAngle); + } + + public static boolean isDeadzone(double angle) { + if ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT)) return true; + else return false; } // Called once the command ends or is interrupted. diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java new file mode 100644 index 0000000..8023e72 --- /dev/null +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -0,0 +1,113 @@ +package frc4388.commands; + +import org.junit.Test; + +import frc4388.robot.commands.AimToCenter; +import org.junit.Assert; + +public class AimToCenterTest { + + private static final double DELTA = 1e-15; + + @Test + public void givenAngle_whenTestIfDeadzone_thenReturnIfInDeadzone() { + boolean output; + + //20 deg + output = AimToCenter.isDeadzone(20.); + Assert.assertFalse(output); + + //-10 deg + output = AimToCenter.isDeadzone(-10.); + Assert.assertTrue(output); + + //-1 deg + output = AimToCenter.isDeadzone(-1.); + Assert.assertTrue(output); + + //341 deg + output = AimToCenter.isDeadzone(341.); + Assert.assertTrue(output); + + //340 deg + output = AimToCenter.isDeadzone(340.); + Assert.assertFalse(output); + + //0 deg + output = AimToCenter.isDeadzone(0.); + Assert.assertFalse(output); + + //200 deg + output = AimToCenter.isDeadzone(200.); + Assert.assertFalse(output); + + //2000000 deg + output = AimToCenter.isDeadzone(2000000.); + Assert.assertTrue(output); + + //NaN deg + output = AimToCenter.isDeadzone(Double.NaN); + Assert.assertFalse(output); + } + + @Test + public void givenOdometry_whenCalculateAngleToCenter_thenReturnAngleToCenter() { + double actual; + double expected; + + //(5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., 5., 0.); + expected = 180. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, 5., 0.); + expected = 180. + 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(-5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(-5.0, -5., 0.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(5,-5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(5., -5., 0.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(0,0) Gyro = 0 deg + actual = AimToCenter.angleToCenter(0., 0., 0.); + Assert.assertNotNull(actual); + + //(5,5) Gyro = 180 deg + actual = AimToCenter.angleToCenter(5., 5., 180.); + expected = 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(100,100) Gyro = 90 deg + actual = AimToCenter.angleToCenter(100., 100., 90.); + expected = 90. + 45.; + Assert.assertEquals(expected, actual, DELTA); + + //(null,5) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, 5., 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = 0 deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, 0.); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(null,null) Gyro = null deg + actual = AimToCenter.angleToCenter(Double.NaN, Double.NaN, Double.NaN); + expected = Double.NaN; + Assert.assertEquals(expected, actual, DELTA); + + //(5,5) Gyro = -20 deg + actual = AimToCenter.angleToCenter(5., -5., -45.); + expected = 180.; + Assert.assertEquals(expected, actual, DELTA); + + } +} diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 87ab85c..ca0e96e 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -17,23 +17,17 @@ import frc4388.utility.LEDPatterns; * Add your docs here. */ public class LEDSubsystemTest { + // Arrange + Spark ledController = mock(Spark.class); + LED led = new LED(ledController); @Test public void testConstructor() { - // Arrange - Spark ledController = mock(Spark.class); - - // Act - LED led = new LED(ledController); - // Assert assertEquals(LEDConstants.DEFAULT_PATTERN.getValue(), led.getPattern().getValue(), 0.0001); } @Test public void testPatterns() { - // Arrange - Spark ledController = mock(Spark.class); - LED led = new LED(ledController); // Act led.setPattern(LEDPatterns.RAINBOW_RAINBOW); From 343f423bb27716b5c606f97733d20d28179fbab6 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Sat, 26 Feb 2022 15:00:09 -0700 Subject: [PATCH 61/73] custom pid for shoot command, needs testing --- .../java/frc4388/robot/commands/Shoot.java | 110 ++++++++++++++++-- 1 file changed, 103 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index 2f8c5ef..f82bc1e 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -4,44 +4,139 @@ package frc4388.robot.commands; -import java.nio.file.SecureDirectoryStream; +import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; +import frc4388.robot.subsystems.Turret; public class Shoot extends CommandBase { + // subsystems public SwerveDrive m_swerve; public BoomBoom m_boomBoom; + public Turret m_turret; public Hood m_hood; - public double m_targetVel; - public double m_targetHood; + // given + public double m_gyroAngle; + public double m_odoX; + public double m_odoY; public double m_distance; + // targets + public double m_targetVel; + public double m_targetHood; + public double m_targetAngle; + public double m_driveTargetAngle; + + // pid + public double error; + public double prevError; + public double kP, kI, kD; + public double integral, derivative; + public double time; + public double output; + public double tolerance = 5.0; + + // // dummy motor + // public WPI_TalonFX dummy = new WPI_TalonFX(69 - 420); + // public TalonFXConfiguration dummyConfiguration = new TalonFXConfiguration(); + /** Creates a new Shoot. */ - public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Hood sHood) { + public Shoot(SwerveDrive sDrive, BoomBoom sShooter, Turret sTurret, Hood sHood) { // Use addRequirements() here to declare subsystem dependencies. m_swerve = sDrive; m_boomBoom = sShooter; + m_turret = sTurret; m_hood = sHood; - addRequirements(m_swerve, m_boomBoom, m_hood); + addRequirements(m_swerve, m_boomBoom, m_turret, m_hood); + + kP = 0.1; + kI = 0.0; + kD = 0.0; + + integral = 0; + derivative = 0; + time = 0.02; + } + + /** + * Updates error for custom PID. + */ + public void updateError() { + error = (m_targetAngle - m_turret.getBoomBoomAngleDegrees() + 360) % 360; } // Called when the command is initially scheduled. @Override public void initialize() { - m_distance = 0; //TODO: get this value using odometry + m_odoX = 0; //TODO: get this value using odometry + m_odoY = 0; //TODO: get this value using odometry + m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); + + // get targets (shooter tables) m_targetVel = m_boomBoom.getVelocity(m_distance); m_targetHood = m_boomBoom.getHood(m_distance); + m_targetAngle = ((Math.atan2(m_odoY, m_odoX) * (180./Math.PI) - m_gyroAngle) + 180. + 360.) % 360.; + m_driveTargetAngle = m_targetAngle + m_turret.getBoomBoomAngleDegrees(); + + // // normal (i think) PID stuff + // dummyConfiguration.primaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.IntegratedSensor.toFeedbackDevice(); + // dummyConfiguration.remoteFilter0.remoteSensorDeviceID = dummy.getDeviceID(); + // dummyConfiguration.remoteFilter0.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; + + // dummyConfiguration.slot0.kP = 0.1; + // dummyConfiguration.slot0.kI = 0; + // dummyConfiguration.slot0.kD = 0; + // dummyConfiguration.slot0.kF = 0; + + // // weird PID stuff + // dummyConfiguration.auxiliaryPID.selectedFeedbackSensor = TalonFXFeedbackDevice.SoftwareEmulatedSensor.toFeedbackDevice(); + // dummyConfiguration.remoteFilter1.remoteSensorDeviceID = ShooterConstants.TURRET_MOTOR_CAN_ID; + // dummyConfiguration.remoteFilter1.remoteSensorSource = RemoteSensorSource.TalonFX_SelectedSensor; + // // dummyConfiguration.auxiliaryPID.selectedFeedbackCoefficient = 0; + + // dummyConfiguration.slot1.kP = 0.1; + // dummyConfiguration.slot1.kI = 0; + // dummyConfiguration.slot1.kD = 0; + // dummyConfiguration.slot1.kF = 0; + + // dummy.configAllSettings(dummyConfiguration); + + // initial error + updateError(); + prevError = error; } + /** + * Run custom PID. + */ + public void runPID() { + prevError = error; + updateError(); + integral = integral + error * time; + derivative = (error - prevError) / time; + output = kP * error + kI * integral + kD * derivative; + } + // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { + // dummy.selectProfileSlot(0, 0); + // dummy.selectProfileSlot(1, 1); + // dummy.set(TalonFXControlMode.Position, m_driveTargetAngle, DemandType.AuxPID, m_targetAngle); + // m_swerve.driveWithInput(0, 0, m_driveTargetAngle, true); + // m_swerve.driveWithInput(0, 0, Math.cos(m_driveTargetAngle), Math.sin(m_driveTargetAngle), true); // only works for new DWI in swerve branch + + // custom pid + runPID(); + m_swerve.driveWithInput(0, 0, output, true); + m_hood.runAngleAdjustPID(m_targetHood); m_boomBoom.runDrumShooterVelocityPID(m_targetVel); } @@ -53,6 +148,7 @@ public class Shoot extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - return false; + updateError(); + return Math.abs(error) <= tolerance; } } From fa15241483ee3d110308386e8a58b15f7655b1f1 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 16:46:40 -0700 Subject: [PATCH 62/73] digital deadzone implementation --- src/main/java/frc4388/robot/Constants.java | 8 ++++++-- .../frc4388/robot/commands/AimToCenter.java | 18 ++++++++++++++---- .../java/frc4388/commands/AimToCenterTest.java | 18 +++++++++--------- 3 files changed, 29 insertions(+), 15 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 5d61976..7c7ef2e 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -110,8 +110,12 @@ public final class Constants { public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; - public static final double DEADZONE_LEFT = 0.0; - public static final double DEADZONE_RIGHT = 340.0; + // deadzones + public static final double HARD_DEADZONE_LEFT = 0.0; + public static final double HARD_DEADZONE_RIGHT = 340.0; + + public static final double DIG_DEADZONE_LEFT = 40.0; + public static final double DIG_DEADZONE_RIGHT = 60.0; public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 93c1501..54cdbce 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -50,11 +50,21 @@ public class AimToCenter extends CommandBase { } /** - * Checks if in deadzone. - * @return True if deadzone. + * Checks if in hardware deadzone (due to mechanical limitations). + * @param angle Angle to check. + * @return True if in hardware deadzone. */ - public static boolean isDeadzone(double angle) { - return ((ShooterConstants.DEADZONE_LEFT > angle) || (angle > ShooterConstants.DEADZONE_RIGHT)); + public static boolean isHardwareDeadzone(double angle) { + return ((ShooterConstants.HARD_DEADZONE_LEFT > angle) || (angle > ShooterConstants.HARD_DEADZONE_RIGHT)); + } + + /** + * Checks if in digital deadzone (due to climber). + * @param angle Angle to check. + * @return True if in digital deadzone. + */ + public static boolean isDigitalDeadzone(double angle) { + return ((ShooterConstants.DIG_DEADZONE_LEFT < angle) && (angle < ShooterConstants.DIG_DEADZONE_RIGHT)); } // Called once the command ends or is interrupted. diff --git a/src/test/java/frc4388/commands/AimToCenterTest.java b/src/test/java/frc4388/commands/AimToCenterTest.java index 8023e72..72a84f4 100644 --- a/src/test/java/frc4388/commands/AimToCenterTest.java +++ b/src/test/java/frc4388/commands/AimToCenterTest.java @@ -14,39 +14,39 @@ public class AimToCenterTest { boolean output; //20 deg - output = AimToCenter.isDeadzone(20.); + output = AimToCenter.isHardwareDeadzone(20.); Assert.assertFalse(output); //-10 deg - output = AimToCenter.isDeadzone(-10.); + output = AimToCenter.isHardwareDeadzone(-10.); Assert.assertTrue(output); //-1 deg - output = AimToCenter.isDeadzone(-1.); + output = AimToCenter.isHardwareDeadzone(-1.); Assert.assertTrue(output); //341 deg - output = AimToCenter.isDeadzone(341.); + output = AimToCenter.isHardwareDeadzone(341.); Assert.assertTrue(output); //340 deg - output = AimToCenter.isDeadzone(340.); + output = AimToCenter.isHardwareDeadzone(340.); Assert.assertFalse(output); //0 deg - output = AimToCenter.isDeadzone(0.); + output = AimToCenter.isHardwareDeadzone(0.); Assert.assertFalse(output); //200 deg - output = AimToCenter.isDeadzone(200.); + output = AimToCenter.isHardwareDeadzone(200.); Assert.assertFalse(output); //2000000 deg - output = AimToCenter.isDeadzone(2000000.); + output = AimToCenter.isHardwareDeadzone(2000000.); Assert.assertTrue(output); //NaN deg - output = AimToCenter.isDeadzone(Double.NaN); + output = AimToCenter.isHardwareDeadzone(Double.NaN); Assert.assertFalse(output); } From a61ec76d477563e8cdfd209c3769a4358605c0ce Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 16:50:55 -0700 Subject: [PATCH 63/73] random changes bc im bored --- src/main/java/frc4388/robot/Constants.java | 18 +++++++++--------- .../frc4388/robot/commands/AimToCenter.java | 8 +++----- 2 files changed, 12 insertions(+), 14 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 7c7ef2e..8d7984a 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -93,13 +93,13 @@ public final class Constants { public static final SupplyCurrentLimitConfiguration SUPPLY_CURRENT_LIMIT_CONFIG = new SupplyCurrentLimitConfiguration(true, 60, 40, 0.5); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; - public static final int SHOOTER_ROTATE_ID = 31; //"unknown value" //figure out later - public static final int TURRET_RIGHT_SOFT_LIMIT = 0; //""// - public static final double TURRET_SPEED_MULTIPLIER = 0.1d; //""// - public static final int DEGREES_PER_ROT = 0; //""// - public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; //""// - public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; //""// - public static final double ENCODER_TICKS_PER_REV = 2048; //""// + public static final int SHOOTER_ROTATE_ID = 31; // TODO: find + public static final int TURRET_RIGHT_SOFT_LIMIT = 0; + public static final double TURRET_SPEED_MULTIPLIER = 0.1d; + public static final int DEGREES_PER_ROT = 0; + public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; + public static final int TURRET_MOTOR_ROTS_PER_ROT = 0; + public static final double ENCODER_TICKS_PER_REV = 2048; @@ -117,10 +117,10 @@ public final class Constants { public static final double DIG_DEADZONE_LEFT = 40.0; public static final double DIG_DEADZONE_RIGHT = 60.0; - public static final int SHOOTER_FALCON_BALLER_ID = 0; //unknown value, fix later// + public static final int SHOOTER_FALCON_BALLER_ID = 0; // TODO: find public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// - public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0);//x,y,z,a,b are not actual values, fix later + public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 32; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 54cdbce..70e65f3 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -15,9 +15,8 @@ public class AimToCenter extends CommandBase { SwerveDrive m_drive; // use odometry to find x and y later - double x = 0; - double y = 0; - double angle = 0; + double x; + double y; double m_targetAngle; // public static Gains m_aimGains; @@ -45,8 +44,7 @@ public class AimToCenter extends CommandBase { public static double angleToCenter(double x, double y, double gyro) { double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + 360.) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) - return(angle); - //m_turret.runshooterRotatePID(m_targetAngle); + return angle; } /** From 26ffb88ed7f87db6dd2687234062fcb12c79433b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 16:54:46 -0700 Subject: [PATCH 64/73] very bored --- src/main/java/frc4388/robot/RobotContainer.java | 4 +--- src/main/java/frc4388/robot/RobotMap.java | 4 ++-- src/main/java/frc4388/robot/commands/Shoot.java | 4 +--- src/main/java/frc4388/robot/subsystems/Hood.java | 5 +---- src/main/java/frc4388/robot/subsystems/SwerveDrive.java | 3 +-- src/main/java/frc4388/robot/subsystems/Turret.java | 6 ++---- src/main/java/frc4388/robot/subsystems/Vision.java | 6 +----- 7 files changed, 9 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index ae911a6..bc0c544 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -4,20 +4,18 @@ package frc4388.robot; -import com.revrobotics.CANSparkMax; import com.ctre.phoenix.motorcontrol.TalonFXControlMode; import com.ctre.phoenix.motorcontrol.can.TalonFX; import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; + import frc4388.robot.Constants.*; import frc4388.robot.commands.AimToCenter; import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; -//import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.robot.subsystems.Turret; diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 29ebda8..53cfd6e 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -4,8 +4,6 @@ package frc4388.robot; -import javax.swing.text.WrappedPlainView; - import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.RemoteSensorSource; import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration; @@ -14,7 +12,9 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.motorcontrol.Spark; + import frc4388.robot.Constants.LEDConstants; import frc4388.robot.Constants.ShooterConstants; import frc4388.robot.Constants.SwerveDriveConstants; diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index f82bc1e..b962d4b 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -4,10 +4,8 @@ package frc4388.robot.commands; -import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - import edu.wpi.first.wpilibj2.command.CommandBase; + import frc4388.robot.subsystems.BoomBoom; import frc4388.robot.subsystems.Hood; import frc4388.robot.subsystems.SwerveDrive; diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 72bb3f8..995af74 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -4,19 +4,16 @@ package frc4388.robot.subsystems; -import javax.sql.rowset.WebRowSet; - import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 93cffc9..716a2ed 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -13,9 +13,8 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.interfaces.Gyro; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc4388.robot.Constants.SwerveDriveConstants; import frc4388.utility.Gains; import frc4388.utility.RobotGyro; diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 28c9583..21e7a73 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -6,19 +6,17 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANEncoder; import com.revrobotics.CANSparkMax; -import com.revrobotics.MotorFeedbackSensor; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.SparkMaxPIDController; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; + import edu.wpi.first.wpilibj.interfaces.Gyro; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; + import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.Gains; -import com.revrobotics.CANSparkMax.SoftLimitDirection; public class Turret extends SubsystemBase { diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 0b6ca09..686ef2c 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -8,12 +8,8 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; + import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.BoomBoom; -import frc4388.robot.subsystems.Turret; -import frc4388.robot.subsystems.Hood; import frc4388.utility.controller.IHandController; public class Vision extends SubsystemBase { From 9c58e04ec74cc515dcd80d6cd54b9132e1ab1397 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 17:13:13 -0700 Subject: [PATCH 65/73] turret soft limits + setter method --- src/main/java/frc4388/robot/Constants.java | 4 +++- .../java/frc4388/robot/subsystems/Turret.java | 24 ++++++++++++++----- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 8d7984a..0434331 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -94,7 +94,6 @@ public final class Constants { public static final int SHOOTER_FALCON_LEFT_CAN_ID = 23; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 24; public static final int SHOOTER_ROTATE_ID = 31; // TODO: find - public static final int TURRET_RIGHT_SOFT_LIMIT = 0; public static final double TURRET_SPEED_MULTIPLIER = 0.1d; public static final int DEGREES_PER_ROT = 0; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; @@ -126,6 +125,9 @@ public final class Constants { public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find + public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find + } public static final class VisionConstants { public static final double TURN_P_VALUE = 0.8; diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index 21e7a73..a4eca33 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -6,6 +6,10 @@ package frc4388.robot.subsystems; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; + +import java.util.concurrent.TimeoutException; + import com.revrobotics.CANSparkMax; import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkMaxLimitSwitch; @@ -16,6 +20,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; +import frc4388.robot.commands.Shoot; import frc4388.utility.Gains; public class Turret extends SubsystemBase { @@ -52,12 +57,10 @@ public class Turret extends SubsystemBase { SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled()); SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled()); - // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - // m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - // m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); // Set - // second - // soft - // limit + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); + m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); + setSoftLimits(true); + m_boomBoomRotateMotor.setInverted(false); m_boomBoomRotatePIDController.setP(m_shooterTGains.m_kP); @@ -73,6 +76,15 @@ public class Turret extends SubsystemBase { // This method will be called once per scheduler run } + /** + * Set status of turret motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setSoftLimits(boolean set) { + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + public void passRequiredSubsystem(BoomBoom subsystem0, SwerveDrive subsystem1) { m_boomBoomSubsystem = subsystem0; m_sDriveSubsystem = subsystem1; From 14cd85900559c979c14836e26f1bc0008fc3beee Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 17:20:01 -0700 Subject: [PATCH 66/73] hood soft limits + setter method --- src/main/java/frc4388/robot/Constants.java | 7 +++++-- .../java/frc4388/robot/subsystems/Hood.java | 19 +++++++++++++++++-- .../java/frc4388/robot/subsystems/Turret.java | 4 ++-- 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 0434331..59cab07 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -108,6 +108,8 @@ public final class Constants { public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0); public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.05, 0.0, 0.0, 0.0, 0, 0.3); public static final double SHOOTER_TURRET_MIN = -1.0; + public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find + public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find // deadzones public static final double HARD_DEADZONE_LEFT = 0.0; @@ -120,13 +122,14 @@ public final class Constants { public static final int SHOOTER_FALCON_BALLER_FOLLOWER_ID = 0; //"// public static final Gains DRUM_SHOOTER_GAINS = new Gains(0,0,0,0,0,0); // TODO: tune values + /* Hood Constants */ public static final int SHOOTER_ANGLE_ADJUST_ID = 32; public static final double HOOD_MOTOR_ROTS_PER_ROT = 1; //TODO: Find public static final double HOOD_MOTOR_POS_AT_ZERO_ROT = 0; //TODO: Find + public static final float HOOD_FORWARD_LIMIT = 0; //TODO: find + public static final float HOOD_REVERSE_LIMIT = 0; //TODO: find - public static final float TURRET_FORWARD_LIMIT = 0; // TODO: find - public static final float TURRET_REVERSE_LIMIT = 0; // TODO: find } public static final class VisionConstants { diff --git a/src/main/java/frc4388/robot/subsystems/Hood.java b/src/main/java/frc4388/robot/subsystems/Hood.java index 995af74..b3a2f30 100644 --- a/src/main/java/frc4388/robot/subsystems/Hood.java +++ b/src/main/java/frc4388/robot/subsystems/Hood.java @@ -10,6 +10,7 @@ import com.revrobotics.SparkMaxPIDController; import com.revrobotics.SparkMaxLimitSwitch; import com.revrobotics.CANSparkMax.ControlType; import com.revrobotics.CANSparkMax.IdleMode; +import com.revrobotics.CANSparkMax.SoftLimitDirection; import com.revrobotics.CANSparkMaxLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.SubsystemBase; @@ -38,10 +39,14 @@ public double m_fireAngle; public Hood() { m_angleAdjusterMotor.setIdleMode(IdleMode.kBrake); - m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); + // m_hoodUpLimitSwitch = m_angleAdjusterMotor.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); m_hoodDownLimitSwitch = m_angleAdjusterMotor.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - m_hoodUpLimitSwitch.enableLimitSwitch(true); + // m_hoodUpLimitSwitch.enableLimitSwitch(true); m_hoodDownLimitSwitch.enableLimitSwitch(true); + + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT); + m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT); + setHoodSoftLimits(true); } @@ -49,6 +54,16 @@ public double m_fireAngle; public void periodic() { // This method will be called once per scheduler run } + + /** + * Set status of hood motor soft limits. + * @param set Boolean to set soft limits to. + */ + public void setHoodSoftLimits(boolean set) { + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kForward, set); + m_angleAdjusterMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); + } + public void runAngleAdjustPID(double targetAngle) { //Set PID Coefficients diff --git a/src/main/java/frc4388/robot/subsystems/Turret.java b/src/main/java/frc4388/robot/subsystems/Turret.java index a4eca33..3a00f02 100644 --- a/src/main/java/frc4388/robot/subsystems/Turret.java +++ b/src/main/java/frc4388/robot/subsystems/Turret.java @@ -59,7 +59,7 @@ public class Turret extends SubsystemBase { m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT); m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT); - setSoftLimits(true); + setTurretSoftLimits(true); m_boomBoomRotateMotor.setInverted(false); @@ -80,7 +80,7 @@ public class Turret extends SubsystemBase { * Set status of turret motor soft limits. * @param set Boolean to set soft limits to. */ - public void setSoftLimits(boolean set) { + public void setTurretSoftLimits(boolean set) { m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, set); m_boomBoomRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } From e2f637675674a84a0d447bc32527caeb3398a339 Mon Sep 17 00:00:00 2001 From: Aarav Shah Date: Mon, 28 Feb 2022 18:55:42 -0700 Subject: [PATCH 67/73] Update Robot.java --- src/main/java/frc4388/robot/Robot.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index bf9b82c..119a032 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -30,7 +30,6 @@ public class Robot extends TimedRobot { public void robotInit() { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. - //AimToCenterTest.RunAll(); m_robotContainer = new RobotContainer(); } From 7a5aa112156183f12cfb57704784d74134ac6f79 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 18:58:46 -0700 Subject: [PATCH 68/73] merge change 1 --- src/main/java/frc4388/robot/RobotContainer.java | 17 ----------------- 1 file changed, 17 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index bc0c544..a44b32b 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -34,21 +34,7 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* RobotMap */ private final RobotMap m_robotMap = new RobotMap(); - private final TalonFX m_testMotor = new TalonFX(2); - /* - * Subsystems - * private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - * m_robotMap.leftFrontSteerMotor, m_robotMap.leftFrontWheelMotor, - * m_robotMap.rightFrontSteerMotor, m_robotMap.rightFrontWheelMotor, - * m_robotMap.leftBackSteerMotor, m_robotMap.leftBackWheelMotor, - * m_robotMap.rightBackSteerMotor, m_robotMap.rightBackWheelMotor, - * m_robotMap.leftFrontEncoder, - * m_robotMap.rightFrontEncoder, - * m_robotMap.leftBackEncoder, - * m_robotMap.rightBackEncoder - * ); - */ private final LED m_robotLED = new LED(m_robotMap.LEDController); private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight); private final Hood m_robotHood = new Hood(); @@ -106,9 +92,6 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON) - .whileHeld(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.5)); - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); From af8e727381c64c9e8c7ebb0e30c7189b9b24be3b Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 19:08:59 -0700 Subject: [PATCH 69/73] merge change 2 --- src/main/java/frc4388/robot/RobotMap.java | 166 ++++++++-------------- 1 file changed, 62 insertions(+), 104 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 53cfd6e..b48b190 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -27,7 +27,8 @@ public class RobotMap { public RobotMap() { configureLEDMotorControllers(); - // configureSwerveMotorControllers(); + configureSwerveMotorControllers(); + configureShooterMotorControllers(); } /* LED Subsystem */ @@ -38,18 +39,6 @@ public class RobotMap { } /* Swerve Subsystem */ - // public final WPI_TalonFX leftFrontSteerMotor = null; - // public final WPI_TalonFX leftFrontWheelMotor = null; - // public final WPI_TalonFX rightFrontSteerMotor = null; - // public final WPI_TalonFX rightFrontWheelMotor = null; - // public final WPI_TalonFX leftBackSteerMotor = null; - // public final WPI_TalonFX leftBackWheelMotor = null; - // public final WPI_TalonFX rightBackSteerMotor = null; - // public final WPI_TalonFX rightBackWheelMotor = null; - // public final CANCoder leftFrontEncoder = null; - // public final CANCoder rightFrontEncoder = null; - // public final CANCoder leftBackEncoder = null; - // public final CANCoder rightBackEncoder = null; public final WPI_TalonFX leftFrontSteerMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_STEER_CAN_ID); public final WPI_TalonFX leftFrontWheelMotor = new WPI_TalonFX(SwerveDriveConstants.LEFT_FRONT_WHEEL_CAN_ID); @@ -65,84 +54,60 @@ public class RobotMap { public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID); void configureSwerveMotorControllers() { - leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); - leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); - rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); + leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET); + leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET); + rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET); - leftFrontSteerMotor.configFactoryDefault(); - leftFrontWheelMotor.configFactoryDefault(); - rightFrontSteerMotor.configFactoryDefault(); - rightFrontWheelMotor.configFactoryDefault(); - leftBackSteerMotor.configFactoryDefault(); - leftBackWheelMotor.configFactoryDefault(); - rightBackSteerMotor.configFactoryDefault(); - rightBackWheelMotor.configFactoryDefault(); + leftFrontSteerMotor.configFactoryDefault(); + leftFrontWheelMotor.configFactoryDefault(); + rightFrontSteerMotor.configFactoryDefault(); + rightFrontWheelMotor.configFactoryDefault(); + leftBackSteerMotor.configFactoryDefault(); + leftBackWheelMotor.configFactoryDefault(); + rightBackSteerMotor.configFactoryDefault(); + rightBackWheelMotor.configFactoryDefault(); - leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configOpenloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configClosedloopRamp(SwerveDriveConstants.OPEN_LOOP_RAMP_RATE, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS); - // config cancoder as remote encoder for swerve steer motors - leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); - rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), - RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, - SwerveDriveConstants.SWERVE_TIMEOUT_MS); + // config cancoder as remote encoder for swerve steer motors + leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + leftBackSteerMotor.configRemoteFeedbackFilter(leftBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightFrontSteerMotor.configRemoteFeedbackFilter(rightFrontEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); + rightBackSteerMotor.configRemoteFeedbackFilter(rightBackEncoder.getDeviceID(), + RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0, + SwerveDriveConstants.SWERVE_TIMEOUT_MS); } // Shooter Config @@ -153,7 +118,7 @@ public class RobotMap { public final CANSparkMax shooterTurret = new CANSparkMax(ShooterConstants.TURRET_MOTOR_CAN_ID, MotorType.kBrushless); // Create motor CANSparkMax - void ConfigureShooterMotorControllers() { + void configureShooterMotorControllers() { // LEFT FALCON shooterFalconLeft.configFactoryDefault(); @@ -162,12 +127,9 @@ public class RobotMap { shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, - ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); // RIGHT FALCON shooterFalconRight.setInverted(false); @@ -175,18 +137,14 @@ public class RobotMap { shooterFalconRight.configFactoryDefault(); shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - // m_shooterFalconRight.configPeakOutputForward(0, - // ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) - shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, - ShooterConstants.SHOOTER_TIMEOUT_MS); - shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, - ShooterConstants.SHOOTER_TIMEOUT_MS); + // m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS);(comment it in if necessary) + shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configClosedLoopPeriod(0, ShooterConstants.CLOSED_LOOP_TIME_MS, ShooterConstants.SHOOTER_TIMEOUT_MS); + shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); + /* Turret Subsytem */ - - shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); - shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); + shooterFalconRight.configStatorCurrentLimit(new StatorCurrentLimitConfiguration(true, 6, 9, 4.2)); // TODO: dont pull numbers out of our ass anymore + shooterFalconLeft.configSupplyCurrentLimit(new SupplyCurrentLimitConfiguration(true, 12, 13, 0.4)); // TODO: dont pull numbers out of our ass anymore } From 815bb64fe6666aab73e4ff976d7a510f2f73f7c0 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 19:40:09 -0700 Subject: [PATCH 70/73] post-merge fixes, controller stuff left --- .../java/frc4388/robot/RobotContainer.java | 19 +++++-------------- .../frc4388/robot/subsystems/BoomBoom.java | 3 --- .../java/frc4388/robot/subsystems/Vision.java | 2 -- 3 files changed, 5 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 4afde0a..25bfc48 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -56,8 +56,11 @@ public class RobotContainer { private final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive( - m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); + public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, + m_robotMap.leftBack, + m_robotMap.rightFront, + m_robotMap.rightBack, + m_robotMap.gyro); private final TalonFX m_testMotor = new TalonFX(23); @@ -66,18 +69,6 @@ public class RobotContainer { private final Hood m_robotHood = new Hood(); private final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); private final Vision m_robotVison = new Vision(m_robotTurret, m_robotBoomBoom); - private final SwerveDrive m_robotSwerveDrive = new SwerveDrive( m_robotMap.leftFrontSteerMotor, - m_robotMap.leftFrontWheelMotor, - m_robotMap.rightFrontSteerMotor, - m_robotMap.rightFrontWheelMotor, - m_robotMap.leftBackSteerMotor, - m_robotMap.leftBackWheelMotor, - m_robotMap.rightBackSteerMotor, - m_robotMap.rightBackWheelMotor, - m_robotMap.leftFrontEncoder, - m_robotMap.rightFrontEncoder, - m_robotMap.leftBackEncoder, - m_robotMap.rightBackEncoder); /* Controllers */ private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); diff --git a/src/main/java/frc4388/robot/subsystems/BoomBoom.java b/src/main/java/frc4388/robot/subsystems/BoomBoom.java index 9dfd332..7898e16 100644 --- a/src/main/java/frc4388/robot/subsystems/BoomBoom.java +++ b/src/main/java/frc4388/robot/subsystems/BoomBoom.java @@ -22,15 +22,12 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.Constants.ShooterConstants; import frc4388.utility.CSV; import frc4388.utility.Gains; -import frc4388.utility.controller.IHandController; public class BoomBoom extends SubsystemBase { public WPI_TalonFX m_shooterFalconLeft; public WPI_TalonFX m_shooterFalconRight; public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS; public static BoomBoom m_boomBoom; -public static IHandController m_driverController; //not sure if driverController in 2022 = m_controller in 2020 -// BangBangController m_controller = new BangBangController(); double velP; double input; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 686ef2c..748bcd5 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -10,7 +10,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.networktables.NetworkTableEntry; import frc4388.robot.Constants.VisionConstants; -import frc4388.utility.controller.IHandController; public class Vision extends SubsystemBase { //setup @@ -19,7 +18,6 @@ public class Vision extends SubsystemBase { Hood m_hood; NetworkTableEntry xEntry; -IHandController m_driverController; //Aiming double turnAmount = 0; double xAngle = 0; From add365dfc19c8ab9983bd7ed4d3b5952894541fd Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 19:45:29 -0700 Subject: [PATCH 71/73] post-merge controller fixes --- src/main/java/frc4388/robot/RobotContainer.java | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 25bfc48..3d56eca 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -108,7 +108,7 @@ public class RobotContainer { */ private void configureButtonBindings() { /* Driver Buttons */ - // "XboxController.Button.kBack" was undefiened yet, 7 works just fine + // "XboxController.Button.kBack" was undefined yet, 7 works just fine new JoystickButton(getDriverController(), 7) .whenPressed(() -> m_robotSwerveDrive.resetGyro()); @@ -128,23 +128,17 @@ public class RobotContainer { /* Operator Buttons */ // activates "Lit Mode" - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) .whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW)) .whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN)); // activates "BoomBoom" - new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whenPressed(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0.1)) .whenReleased(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)); - /* Driver Buttons */ - // activates intake - new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON); - // .whenPressed() -> m_robot - /* operator button */ // activates hood - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) .whenPressed(() -> m_robotHood.runHood(0.5d)) .whenReleased(() -> m_robotHood.runHood(0.d)); - // new JoystickButton(getOperatorJoystick()); } /** From b249f8f6904372c9c10ff2d1e1f0faab808786ba Mon Sep 17 00:00:00 2001 From: aarav18 Date: Mon, 28 Feb 2022 19:47:47 -0700 Subject: [PATCH 72/73] ALL POST-MERGE ERRORS FIXED --- src/main/java/frc4388/robot/commands/AimToCenter.java | 2 +- src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc4388/robot/commands/AimToCenter.java b/src/main/java/frc4388/robot/commands/AimToCenter.java index 70e65f3..45f0998 100644 --- a/src/main/java/frc4388/robot/commands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/AimToCenter.java @@ -38,7 +38,7 @@ public class AimToCenter extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_targetAngle = angleToCenter(x, y, m_drive.gyro.getAngle()); + m_targetAngle = angleToCenter(x, y, m_drive.getRegGyro().getDegrees()); m_turret.runshooterRotatePID(m_targetAngle); } diff --git a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java index 9fe8f58..a0f6b97 100644 --- a/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java +++ b/src/test/java/frc4388/robot/subsystems/LEDSubsystemTest.java @@ -6,6 +6,7 @@ package frc4388.robot.subsystems; import static org.junit.Assert.assertEquals; // import static org.mockito.Mockito.mock; +import static org.mockito.Mockito.mock; import org.junit.Test; From b01d79ae709b6d4b0889fe1ba6a675a6a9a035bb Mon Sep 17 00:00:00 2001 From: aarav18 Date: Tue, 1 Mar 2022 17:18:25 -0700 Subject: [PATCH 73/73] some changes --- src/main/java/frc4388/robot/commands/Shoot.java | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc4388/robot/commands/Shoot.java b/src/main/java/frc4388/robot/commands/Shoot.java index b962d4b..28a5f3e 100644 --- a/src/main/java/frc4388/robot/commands/Shoot.java +++ b/src/main/java/frc4388/robot/commands/Shoot.java @@ -35,7 +35,7 @@ public class Shoot extends CommandBase { public double error; public double prevError; public double kP, kI, kD; - public double integral, derivative; + public double proportional, integral, derivative; public double time; public double output; public double tolerance = 5.0; @@ -58,6 +58,7 @@ public class Shoot extends CommandBase { kI = 0.0; kD = 0.0; + proportional = 0; integral = 0; derivative = 0; time = 0.02; @@ -77,6 +78,8 @@ public class Shoot extends CommandBase { m_odoY = 0; //TODO: get this value using odometry m_distance = Math.sqrt(Math.pow(m_odoX, 2) + Math.pow(m_odoY, 2)); + m_gyroAngle = m_swerve.getRegGyro().getDegrees(); + // get targets (shooter tables) m_targetVel = m_boomBoom.getVelocity(m_distance); m_targetHood = m_boomBoom.getHood(m_distance); @@ -117,9 +120,11 @@ public class Shoot extends CommandBase { public void runPID() { prevError = error; updateError(); + + proportional = error; integral = integral + error * time; derivative = (error - prevError) / time; - output = kP * error + kI * integral + kD * derivative; + output = kP * proportional + kI * integral + kD * derivative; } // Called every time the scheduler runs while the command is scheduled.