mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge branch 'origin/swerve' into robot-logger
This commit is contained in:
nathanrsxtn
@@ -0,0 +1,142 @@
|
||||
// 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 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.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
|
||||
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.CSV;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class BoomBoom extends SubsystemBase {
|
||||
public WPI_TalonFX m_shooterFalconLeft;
|
||||
public WPI_TalonFX m_shooterFalconRight;
|
||||
public static Gains m_drumShooterGains = ShooterConstants.DRUM_SHOOTER_GAINS;
|
||||
public static BoomBoom m_boomBoom;
|
||||
|
||||
double velP;
|
||||
double input;
|
||||
|
||||
public boolean m_isDrumReady = false;
|
||||
public double m_fireVel;
|
||||
|
||||
public Hood m_hoodSubsystem;
|
||||
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
|
||||
*/
|
||||
/** Creates a new BoomBoom. */
|
||||
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
|
||||
m_shooterFalconLeft = shooterFalconLeft;
|
||||
m_shooterFalconRight = shooterFalconRight;
|
||||
|
||||
try {
|
||||
CSV<ShooterTableEntry> 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 <E> Number linearInterpolate(final E[] table, final Number lookupValue, final Function<E, Number> lookupGetter, final Function<E, Number> targetGetter) {
|
||||
final Map.Entry<Integer, E> 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 <E> Optional<Map.Entry<Integer, E>> lookup(final E[] table, final Number value, final Function<E, Number> valueGetter, final boolean exactMatch) {
|
||||
final Optional<Map.Entry<Integer, E>> 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
|
||||
|
||||
}
|
||||
|
||||
public void passRequiredSubsystem(Hood subsystem0, Turret subsystem1) {
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
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
|
||||
m_shooterFalconRight.follow(m_shooterFalconLeft);
|
||||
// New BoomBoom controller stuff
|
||||
// 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(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
// 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.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 frc4388.robot.Constants.ShooterConstants;
|
||||
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 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 boolean m_isHoodReady = false;
|
||||
|
||||
public double m_fireAngle;
|
||||
|
||||
|
||||
/** Creates a new Hood. */
|
||||
public Hood() {
|
||||
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_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_FORWARD_LIMIT);
|
||||
m_angleAdjusterMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_REVERSE_LIMIT);
|
||||
setHoodSoftLimits(true);
|
||||
}
|
||||
|
||||
|
||||
@Override
|
||||
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
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
public void runHood(double input) {
|
||||
// m_angleAdjusterMotor.set(input);
|
||||
}
|
||||
|
||||
public void resetGyroAngleAdj(){
|
||||
// m_angleEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
public double getAnglePosition(){
|
||||
return 0.0;//m_angleEncoder.getPosition();
|
||||
}
|
||||
|
||||
public double getAnglePositionDegrees(){
|
||||
return 0.0;//((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90;
|
||||
}
|
||||
}
|
||||
@@ -54,37 +54,19 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
public boolean ignoreAngles;
|
||||
public Rotation2d rotTarget = new Rotation2d();;
|
||||
public Rotation2d rotTarget = new Rotation2d();
|
||||
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
private final Field2d m_field = new Field2d();
|
||||
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
|
||||
// m_leftFrontSteerMotor = leftFrontSteerMotor;
|
||||
// m_leftFrontWheelMotor = leftFrontWheelMotor;
|
||||
// m_rightFrontSteerMotor = rightFrontSteerMotor;
|
||||
// m_rightFrontWheelMotor = rightFrontWheelMotor;
|
||||
// m_leftBackSteerMotor = leftBackSteerMotor;
|
||||
// m_leftBackWheelMotor = leftBackWheelMotor;
|
||||
// m_rightBackSteerMotor = rightBackSteerMotor;
|
||||
// m_rightBackWheelMotor = rightBackWheelMotor;
|
||||
// m_leftFrontEncoder = leftFrontEncoder;
|
||||
// m_rightFrontEncoder = rightFrontEncoder;
|
||||
// m_leftBackEncoder = leftBackEncoder;
|
||||
// m_rightBackEncoder = rightBackEncoder;
|
||||
|
||||
m_leftFront = leftFront;
|
||||
m_leftBack = leftBack;
|
||||
m_rightFront = rightFront;
|
||||
m_rightBack = rightBack;
|
||||
m_gyro = gyro;
|
||||
|
||||
// modules = new SwerveModule[] {
|
||||
// new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
|
||||
// new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
|
||||
// new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
|
||||
// new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
|
||||
// };
|
||||
|
||||
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
|
||||
|
||||
m_poseEstimator =
|
||||
@@ -154,21 +136,26 @@ public class SwerveDrive extends SubsystemBase {
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state, false);
|
||||
}
|
||||
// modules[0].setDesiredState(desiredStates[0], false);
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
updateOdometry();
|
||||
SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
|
||||
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
|
||||
SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
|
||||
SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
|
||||
SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
|
||||
|
||||
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
updateOdometry();
|
||||
// SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
|
||||
|
||||
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
|
||||
|
||||
// SmartDashboard.putNumber("Front Left", modules[0].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Front Right", modules[1].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Back Left", modules[2].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Back Right", modules[3].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
|
||||
// SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
|
||||
// SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
|
||||
|
||||
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
super.periodic();
|
||||
}
|
||||
|
||||
@@ -208,27 +195,29 @@ public class SwerveDrive extends SubsystemBase {
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry of the robot to (x=0, y=0, theta=0).
|
||||
* Gets the current gyro using regression formula.
|
||||
* @return Rotation2d object holding current gyro in radians
|
||||
*/
|
||||
public Rotation2d getRegGyro() {
|
||||
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
|
||||
return new Rotation2d(regCur * Math.PI / 180);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry of the robot to the given pose.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
// m_odometry.resetPosition(pose, m_gyro.getRotation2d());
|
||||
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/** Updates the field relative position of the robot. */
|
||||
|
||||
/** Updates the field relative position of the robot.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update( m_gyro.getRotation2d(),
|
||||
m_poseEstimator.update( getRegGyro(),
|
||||
modules[0].getState(),
|
||||
modules[1].getState(),
|
||||
modules[2].getState(),
|
||||
modules[3].getState());
|
||||
|
||||
// m_odometry.update( m_gyro.getRotation2d(),
|
||||
// modules[0].getState(),
|
||||
// modules[1].getState(),
|
||||
// modules[2].getState(),
|
||||
// modules[3].getState());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
@@ -242,6 +231,9 @@ public class SwerveDrive extends SubsystemBase {
|
||||
rotTarget = new Rotation2d(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop all four swerve modules.
|
||||
*/
|
||||
public void stopModules() {
|
||||
modules[0].stop();
|
||||
modules[1].stop();
|
||||
|
||||
@@ -7,6 +7,7 @@ package frc4388.robot.subsystems;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
@@ -15,19 +16,27 @@ import com.ctre.phoenix.sensors.CANCoderConfiguration;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX driveMotor;
|
||||
private CANCoder canCoder;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
private SwerveModuleState state;
|
||||
private double canCoderFeedbackCoefficient;
|
||||
|
||||
public long m_currentTime;
|
||||
public long m_lastTime;
|
||||
public double m_deltaTime;
|
||||
|
||||
public double m_currentPos;
|
||||
public double m_lastPos;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
|
||||
@@ -48,21 +57,38 @@ public class SwerveModule extends SubsystemBase {
|
||||
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleTalonFXConfiguration);
|
||||
|
||||
TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
driveTalonFXConfiguration.slot0.kP = 0.15;
|
||||
driveTalonFXConfiguration.slot0.kI = 0.0;
|
||||
driveTalonFXConfiguration.slot0.kD = 0.5;
|
||||
driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
// driveTalonFXConfiguration.slot0.kP = 0.05;
|
||||
// driveTalonFXConfiguration.slot0.kI = 0.0;
|
||||
// driveTalonFXConfiguration.slot0.kD = 0.0;
|
||||
// driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
|
||||
driveMotor.configFactoryDefault();
|
||||
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
|
||||
driveMotor.configNominalOutputForward(0, 30);
|
||||
driveMotor.configNominalOutputReverse(0, 30);
|
||||
driveMotor.configPeakOutputForward(1, 30);
|
||||
driveMotor.configPeakOutputReverse(-1, 30);
|
||||
driveMotor.configAllowableClosedloopError(0, 0, 30);
|
||||
driveMotor.config_kP(0, 0.5, 30);
|
||||
driveMotor.config_kI(0, 0, 30);
|
||||
driveMotor.config_kD(0, 0, 30);
|
||||
// maybe try a feedforward value?
|
||||
|
||||
// driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
|
||||
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
|
||||
canCoderConfiguration.magnetOffsetDegrees = offset;
|
||||
canCoder.configAllSettings(canCoderConfiguration);
|
||||
}
|
||||
|
||||
m_currentTime = System.currentTimeMillis();
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
|
||||
m_lastPos = driveMotor.getSelectedSensorPosition();
|
||||
}
|
||||
|
||||
private Rotation2d getAngle() {
|
||||
// Note: This assumes the CANCoders are setup with the default feedback coefficient
|
||||
// and the sesnor value reports degrees.
|
||||
// and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
@@ -72,8 +98,7 @@ public class SwerveModule extends SubsystemBase {
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
|
||||
Rotation2d currentRotation = getAngle();
|
||||
|
||||
//SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
// SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// Find the difference between our current rotational position + our new rotational position
|
||||
@@ -84,29 +109,62 @@ public class SwerveModule extends SubsystemBase {
|
||||
// Convert the CANCoder from it's position reading back to ticks
|
||||
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
|
||||
double desiredTicks = currentTicks + deltaTicks;
|
||||
|
||||
if (!ignoreAngle){
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
}
|
||||
|
||||
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
|
||||
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
|
||||
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
driveMotor.set(normFtPerSec);// - angleMotor.get());
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
|
||||
// m_currentTime = System.currentTimeMillis();
|
||||
// m_deltaTime = (double) (m_currentTime - m_lastTime);
|
||||
// m_deltaTime = m_deltaTime / 10.0;
|
||||
|
||||
// m_currentPos = driveMotor.getSelectedSensorPosition();
|
||||
|
||||
// double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
|
||||
// double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048;
|
||||
// double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity());
|
||||
// double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
|
||||
// System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
|
||||
// System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
|
||||
// System.out.println("Last Pos: " + m_lastPos);
|
||||
|
||||
// driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/);
|
||||
|
||||
// m_lastTime = m_currentTime;
|
||||
// m_lastPos = m_currentPos;
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns the current state of the module.
|
||||
* Get current module state.
|
||||
*
|
||||
* @return The current state of the module.
|
||||
* @return The current state of the module in m/s.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// return state;
|
||||
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the drive and steer motors of current module.
|
||||
*/
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
@Override
|
||||
public void periodic(){
|
||||
Rotation2d currentRotation = getAngle();
|
||||
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,115 @@
|
||||
// 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.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;
|
||||
import com.revrobotics.SparkMaxPIDController;
|
||||
|
||||
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.robot.commands.Shoot;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
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;
|
||||
SparkMaxLimitSwitch m_boomBoomRightLimit, m_boomBoomLeftLimit;
|
||||
public Gyro m_turretGyro;
|
||||
|
||||
public double m_targetDistance = 0;
|
||||
|
||||
public boolean m_isAimReady = false;
|
||||
|
||||
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
|
||||
|
||||
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);
|
||||
m_boomBoomLeftLimit.enableLimitSwitch(true);
|
||||
SmartDashboard.putBoolean("Right Limit Switch Enabled", m_boomBoomRightLimit.isLimitSwitchEnabled());
|
||||
SmartDashboard.putBoolean("Left Limit Switch Enabled", m_boomBoomLeftLimit.isLimitSwitchEnabled());
|
||||
|
||||
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_FORWARD_LIMIT);
|
||||
m_boomBoomRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_REVERSE_LIMIT);
|
||||
setTurretSoftLimits(true);
|
||||
|
||||
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() {
|
||||
// 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 setTurretSoftLimits(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;
|
||||
}
|
||||
|
||||
public void runTurretWithInput(double input) {
|
||||
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);
|
||||
}
|
||||
|
||||
public void resetGyroShooterRotate() {
|
||||
m_boomBoomRotateEncoder.setPosition(0);
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
// 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;
|
||||
import edu.wpi.first.networktables.NetworkTableInstance;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.networktables.NetworkTableEntry;
|
||||
|
||||
import frc4388.robot.Constants.VisionConstants;
|
||||
|
||||
public class Vision extends SubsystemBase {
|
||||
//setup
|
||||
Turret m_turret;
|
||||
BoomBoom m_boomBoom;
|
||||
Hood m_hood;
|
||||
|
||||
NetworkTableEntry xEntry;
|
||||
//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;
|
||||
public double m_fireAngle;
|
||||
|
||||
|
||||
public Vision(Turret aimSubsystem, BoomBoom boomBoom) {
|
||||
m_turret = aimSubsystem;
|
||||
m_boomBoom = boomBoom;
|
||||
m_hood = m_boomBoom.m_hoodSubsystem;
|
||||
//addRequirements(m_turret);
|
||||
limeOff();
|
||||
changePipeline(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0);
|
||||
NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(3);
|
||||
}
|
||||
|
||||
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;
|
||||
// }
|
||||
// }
|
||||
|
||||
SmartDashboard.putNumber("Distance 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
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user