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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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/59] 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 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 36/59] 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 37/59] 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 ed8cf1443cd7093c9844a918d149974ccf7ceee1 Mon Sep 17 00:00:00 2001 From: Abhishrek05 <90010729+Abhishrek05@users.noreply.github.com> Date: Thu, 17 Feb 2022 19:54:24 -0700 Subject: [PATCH 38/59] :)) --- .../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 39/59] 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 40/59] 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 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 41/59] 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 42/59] 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 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 43/59] 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 44/59] 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 45/59] 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 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 46/59] 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 47/59] 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 48/59] 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 49/59] 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 50/59] 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 51/59] 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 52/59] 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 53/59] 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 54/59] 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 55/59] 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 56/59] 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 57/59] 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 58/59] 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 59/59] 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 }