Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

drum pid + tentative shooting values + get voltage

Browse Source
This commit is contained in:
Ryan Manley
2022-03-13 17:02:55 -06:00
parent 362fb579a9
commit 68c69d7262
9 changed files with 55 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+9 -7
View File
@@ -32,7 +32,7 @@ import frc4388.utility.LEDPatterns;
*/
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = 4.0;
public static final double ROTATION_SPEED = 2.0;
public static final double WIDTH = 23.5;
public static final double HEIGHT = 23.5;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
@@ -183,7 +183,7 @@ public final class Constants {
true, 27, 0, 0);
public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21;
public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22;
public static final double TURRET_SPEED_MULTIPLIER = 0.75d;
public static final double TURRET_SPEED_MULTIPLIER = 0.4d;
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;
@@ -195,19 +195,21 @@ public final class Constants {
/* Turret Constants */
// ID
public static final int TURRET_MOTOR_CAN_ID = 19;
//Gains for turret
public static final Gains SHOOTER_TURRET_GAINS = new Gains(0.6, 0.0, 0.0, 0.0, 0, 1.0);
//Gains for hood
public static final Gains SHOOTER_ANGLE_GAINS = new Gains(0.1, 0.0, 0.0, 0.0, 0, 0.7);
public static final double SHOOTER_TURRET_MIN = -1.0;
public static final double TURRET_FORWARD_LIMIT = 17.0;
public static final double TURRET_REVERSE_LIMIT = -105.0;
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0, 0, 0, 0, 0, 0); // TODO: tune values
public static final double TURRET_FORWARD_LIMIT = 17.0; // TODO: find
public static final double TURRET_REVERSE_LIMIT = -105.0; // TODO: find
//Shooter gains for actual Drum
public static final Gains DRUM_SHOOTER_GAINS = new Gains(0.4, 0.0, 15.0, 0.05, 0, 0); // TODO: tune values
/* Hood Constants */
public static final int SHOOTER_ANGLE_ADJUST_ID = 20;
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 double HOOD_FORWARD_LIMIT = -0.0; // TODO: find
public static final double HOOD_FORWARD_LIMIT = 0.0; // TODO: find
public static final double HOOD_REVERSE_LIMIT = -150; // TODO: find
}
src/main/java/frc4388/robot/Robot.java
+2
View File
@@ -143,6 +143,8 @@ public class Robot extends TimedRobot {
m_robotContainer.m_robotSwerveDrive.getCurrent();
// m_robotContainer.m_robotTurret.getCurrent();
SmartDashboard.putNumber("Total Robot Current Draw", current);
SmartDashboard.putNumber("Drive Train Voltage", m_robotContainer.m_robotSwerveDrive.getVoltage());
SmartDashboard.putNumber("Drive Train Current", m_robotContainer.m_robotSwerveDrive.getCurrent());
// Runs the Scheduler. This is responsible for polling buttons, adding
// newly-scheduled
// commands, running already-scheduled commands, removing finished or
src/main/java/frc4388/robot/RobotContainer.java
+18 -6
View File
@@ -259,9 +259,11 @@ public class RobotContainer {
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
new JoystickButton(getOperatorController(), XboxController.Button.kX.value) //20ft
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25)))
.whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(10000)))
// .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.6)))
// .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
// .whileHeld(new RunCommand(() -> m_robotHood.runAngleAdjustPID(-96)))
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0)));
.whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(0)));
new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value)
.whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage))
@@ -271,13 +273,23 @@ public class RobotContainer {
.whileHeld(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage))
.whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage));
// new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
// .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
new JoystickButton(getOperatorController(), XboxController.Button.kA.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
.whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(500), m_robotBoomBoom));
new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
.whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
.whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
.whenPressed(new InstantCommand(() -> m_robotBoomBoom.updateOffset(-500), m_robotBoomBoom));
// new JoystickButton(getOperatorController(), XboxController.Button.kB.value)
// .whileHeld(new InstantCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender))
// .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender));
// Right Bumper > Storage In
// new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value)
// .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED)))
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+11 -1
View File
@@ -37,6 +37,7 @@ public class BoomBoom extends SubsystemBase {
double velP;
double input;
public double pidOffset = 0;
public boolean m_isDrumReady = false;
public double m_fireVel;
@@ -56,6 +57,7 @@ public class BoomBoom extends SubsystemBase {
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
m_shooterFalconLeft = shooterFalconLeft;
m_shooterFalconRight = shooterFalconRight;
setShooterGains();
try {
// This is a helper class that allows us to read a CSV file into a Java array.
@@ -183,6 +185,9 @@ public class BoomBoom extends SubsystemBase {
public void periodic() {
// This method will be called once per scheduler run
// speed2 = SmartDashboard.getNumber("Shooter Offset", 0.0);
SmartDashboard.putNumber("Shooter Current", getCurrent());
SmartDashboard.putNumber("Shooter Voltage", m_shooterFalconLeft.getMotorOutputVoltage());
SmartDashboard.putNumber("Shooter Actual Velocity", m_shooterFalconLeft.getSelectedSensorVelocity());
}
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
@@ -211,7 +216,8 @@ public class BoomBoom extends SubsystemBase {
}
public void runDrumShooterVelocityPID(double targetVel) {
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init
SmartDashboard.putNumber("Target Drum Velocity", 10000 + pidOffset);
m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel + pidOffset); // Init
// New BoomBoom controller stuff
// Controls a motor with the output of the BangBang controller
@@ -223,6 +229,10 @@ public class BoomBoom extends SubsystemBase {
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
}
public void updateOffset(double change) {
pidOffset = pidOffset + change;
}
public void increaseSpeed(double amount)
{
speed2 = speed2 + amount;
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+6 -2
View File
@@ -112,9 +112,9 @@ public class SwerveDrive extends SubsystemBase {
double ySpeedMetersPerSecond = speed.getY();
chassisSpeeds = fieldRelative
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust, new Rotation2d((360 - m_gyro.getRotation2d().getDegrees() + 90) * (Math.PI/180)))
: new ChassisSpeeds(ySpeedMetersPerSecond, -xSpeedMetersPerSecond,
-rot * SwerveDriveConstants.ROTATION_SPEED);
-rot * SwerveDriveConstants.ROTATION_SPEED * speedAdjust);
SwerveModuleState[] states = m_kinematics.toSwerveModuleStates(chassisSpeeds);
setModuleStates(states);
}
@@ -263,4 +263,8 @@ public class SwerveDrive extends SubsystemBase {
public double getCurrent(){
return m_leftFront.getCurrent() + m_rightFront.getCurrent() + m_rightBack.getCurrent() + m_leftBack.getCurrent();
}
public double getVoltage(){
return m_leftFront.getVoltage() + m_rightFront.getVoltage() + m_rightBack.getVoltage() + m_leftBack.getVoltage();
}
}
src/main/java/frc4388/robot/subsystems/SwerveModule.java
+4
View File
@@ -195,4 +195,8 @@ public class SwerveModule extends SubsystemBase {
public double getCurrent(){
return angleMotor.getSupplyCurrent() + driveMotor.getSupplyCurrent();
}
public double getVoltage(){
return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
}
}
src/main/java/frc4388/robot/subsystems/Turret.java
+1 -5
View File
@@ -7,7 +7,7 @@ package frc4388.robot.subsystems;
import com.revrobotics.CANSparkMax.ControlType;
import com.revrobotics.CANSparkMax.IdleMode;
import com.revrobotics.CANSparkMax.SoftLimitDirection;
import com.revrobotics.SparkMaxRelativeEncoder.Type;
import com.revrobotics.CANSparkMax;
import com.revrobotics.RelativeEncoder;
import com.revrobotics.SparkMaxLimitSwitch;
@@ -60,8 +60,6 @@ public class Turret extends SubsystemBase {
m_boomBoomRotateMotor.setInverted(true);
// m_boomBoomRotateMotor.getAlternateEncoder(4096).setPosition(0);
m_boomBoomRotatePIDController.setP(m_shooterTGains.kP);
m_boomBoomRotatePIDController.setI(m_shooterTGains.kI);
m_boomBoomRotatePIDController.setD(m_shooterTGains.kD);
@@ -104,12 +102,10 @@ public class Turret extends SubsystemBase {
}
public double getboomBoomRotatePosition() {
// return 0.0;
return m_boomBoomRotateEncoder.getPosition();
}
public double getBoomBoomAngleDegrees() {
// return 0.0;
return (m_boomBoomRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360
/ ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT;
}
src/main/java/frc4388/robot/subsystems/VisionOdometry.java
+1 -1
View File
@@ -112,7 +112,7 @@ public class VisionOdometry extends SubsystemBase {
}
guess = correctGuessForCenter(guess, m_shooter.getBoomBoomAngleDegrees());
guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
// guess = correctGuessForGyro(guess, m_drive.getRegGyro().getDegrees());
SmartDashboard.putNumber("Vision ODO x: ", guess.x);
SmartDashboard.putNumber("Vision ODO y: ", guess.y);