mirror of
https://github.com/Team4388/RiseOfRidgebotics2020.git
synced 2026-06-09 08:48:01 -06:00
Merge branch 'master' into add-shooter
This commit is contained in:
ryan123rudder
@@ -8,20 +8,11 @@
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.FilenameFilter;
|
||||
import java.io.IOException;
|
||||
import java.nio.file.Files;
|
||||
import java.nio.file.Path;
|
||||
import java.nio.file.Paths;
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
import java.util.stream.Collectors;
|
||||
import java.util.stream.Stream;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.ControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.DemandType;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.FollowerType;
|
||||
import com.ctre.phoenix.motorcontrol.InvertType;
|
||||
import com.ctre.phoenix.motorcontrol.NeutralMode;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.SensorTerm;
|
||||
@@ -34,59 +25,73 @@ import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
|
||||
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.DoubleSolenoid;
|
||||
|
||||
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
|
||||
import edu.wpi.first.wpilibj.geometry.Pose2d;
|
||||
import edu.wpi.first.wpilibj.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
|
||||
import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
|
||||
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpiutil.math.MathUtil;
|
||||
|
||||
import frc4388.robot.Constants.DriveConstants;
|
||||
import frc4388.robot.Gains;
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public class Drive extends SubsystemBase {
|
||||
// Put methods for controlling this subsystem
|
||||
// here. Call these from Commands.
|
||||
|
||||
/* Create Motors, Gyros, Solenoids, etc */
|
||||
public WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
|
||||
public WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
|
||||
public WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
|
||||
public WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
|
||||
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
|
||||
public Orchestra m_orchestra = new Orchestra();
|
||||
|
||||
public double m_rightFrontMotorPos;
|
||||
|
||||
public double m_rightFrontMotorVel;
|
||||
|
||||
public DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
|
||||
|
||||
SendableChooser<Gains> m_chooser = new SendableChooser<Gains>();
|
||||
public static Gains m_gainsDistance = DriveConstants.DRIVE_DISTANCE_GAINS;
|
||||
public static Gains m_gainsVelocity = DriveConstants.DRIVE_VELOCITY_GAINS;
|
||||
public static Gains m_gainsTurning = DriveConstants.DRIVE_TURNING_GAINS;
|
||||
//public static Gains m_gainsMotionMagic = DriveConstants.DRIVE_MOTION_MAGIC_GAINS;
|
||||
public DoubleSolenoid m_speedShift = new DoubleSolenoid(7,0,1);
|
||||
public DoubleSolenoid m_coolFalcon = new DoubleSolenoid(7,3,2);
|
||||
|
||||
/* Drive objects to manage Drive Train */
|
||||
public DifferentialDrive m_driveTrain;
|
||||
public final DifferentialDriveOdometry m_odometry;
|
||||
|
||||
public DoubleSolenoid m_speedShift;
|
||||
public DoubleSolenoid m_coolFalcon;
|
||||
public Orchestra m_orchestra;
|
||||
|
||||
/* Low Gear Gains */
|
||||
public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW;
|
||||
public static Gains m_gainsVelocityLow = DriveConstants.DRIVE_VELOCITY_GAINS_LOW;
|
||||
public static Gains m_gainsTurningLow = DriveConstants.DRIVE_TURNING_GAINS_LOW;
|
||||
public static Gains m_gainsMotionMagicLow = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_LOW;
|
||||
|
||||
/* High Gear Gains */
|
||||
public static Gains m_gainsDistanceHigh = DriveConstants.DRIVE_DISTANCE_GAINS_HIGH;
|
||||
public static Gains m_gainsVelocityHigh = DriveConstants.DRIVE_VELOCITY_GAINS_HIGH;
|
||||
public static Gains m_gainsTurningHigh = DriveConstants.DRIVE_TURNING_GAINS_HIGH;
|
||||
public static Gains m_gainsMotionMagicHigh = DriveConstants.DRIVE_MOTION_MAGIC_GAINS_HIGH;
|
||||
|
||||
/* Timey Whimey */
|
||||
public long m_currentTimeMs = System.currentTimeMillis();
|
||||
public long m_lastTimeMs = m_currentTimeMs;
|
||||
public long m_deltaTimeMs = 0;
|
||||
public long m_currentTimeSec = m_currentTimeMs / 1000;
|
||||
|
||||
/* Position Tracking */
|
||||
public double m_rightFrontMotorPos = 0;
|
||||
public double m_rightFrontMotorVel = 0;
|
||||
|
||||
public double m_totalLeftDistanceInches = 0;
|
||||
public double m_totalRightDistanceInches = 0;
|
||||
|
||||
public double m_currentLeftPosTicks = 0;
|
||||
public double m_currentRightPosTicks = 0;
|
||||
public double m_lastLeftPosTicks = 0;
|
||||
public double m_lastRightPosTicks = 0;
|
||||
|
||||
public double m_lastAngleYaw = 0;
|
||||
public double m_currentAngleYaw = 0;
|
||||
|
||||
/* Smart Dashboard Objects */
|
||||
SendableChooser<String> m_songChooser = new SendableChooser<String>();
|
||||
|
||||
public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
|
||||
public long m_lastTime, m_deltaTime; //in milliseconds
|
||||
|
||||
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
|
||||
|
||||
/* Misc */
|
||||
public boolean m_isSpeedShiftHigh;
|
||||
String m_currentSong = "";
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
@@ -100,81 +105,50 @@ public class Drive extends SubsystemBase {
|
||||
m_pigeon.configFactoryDefault();
|
||||
resetGyroYaw();
|
||||
|
||||
m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()),
|
||||
new Pose2d(0, 0, new Rotation2d()) );
|
||||
|
||||
m_speedShift = new DoubleSolenoid(7,0,1);
|
||||
m_coolFalcon = new DoubleSolenoid(7,3,2);
|
||||
|
||||
coolFalcon(false);
|
||||
|
||||
/* set back motors as followers */
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
/* flip input so forward becomes back, etc */
|
||||
m_leftFrontMotor.setInverted(false);
|
||||
m_rightFrontMotor.setInverted(true);
|
||||
//m_driveTrain.setRightSideInverted(false);
|
||||
m_leftBackMotor.setInverted(InvertType.FollowMaster);
|
||||
m_rightBackMotor.setInverted(InvertType.FollowMaster);
|
||||
m_leftFrontMotor.setInverted(DriveConstants.isLeftMotorInverted);
|
||||
m_rightFrontMotor.setInverted(DriveConstants.isRightMotorInverted);
|
||||
m_leftBackMotor.setInverted(DriveConstants.isLeftMotorInverted);
|
||||
m_rightBackMotor.setInverted(DriveConstants.isRightMotorInverted);
|
||||
m_driveTrain.setRightSideInverted(DriveConstants.isRightArcadeInverted);
|
||||
|
||||
setDriveTrainNeutralMode(NeutralMode.Coast);
|
||||
/* Config Open Loop Ramp so we don't make sudden output changes */
|
||||
m_rightFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configOpenloopRamp(DriveConstants.OPEN_LOOP_RAMP_RATE, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* deadbands */
|
||||
m_leftBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
m_rightBackMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
|
||||
//m_leftFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // DO NOT CHANGE
|
||||
//m_rightFrontMotor.configNeutralDeadband(0.0, DriveConstants.DRIVE_TIMEOUT_MS); // Ensures motors run at the same speed
|
||||
/* Config Supply Current Limit (Use only for debugging) */
|
||||
m_rightFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
|
||||
m_leftFrontMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
|
||||
m_rightBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
|
||||
m_leftBackMotor.configSupplyCurrentLimit(DriveConstants.SUPPLY_CURRENT_LIMIT_CONFIG);
|
||||
|
||||
/* Config deadbands so that */
|
||||
m_leftBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configNeutralDeadband(DriveConstants.NEUTRAL_DEADBAND, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* PID for Front Motor Control in Teleop */
|
||||
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
//m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
setRightMotorGains(false);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurning.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistance.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
//m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
//m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagic.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
//m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* PID for Back Motor control in Auto */
|
||||
/* PID for Back Motor Control in Tank Drive Vel */
|
||||
m_rightBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocity.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sensors for WPI_TalonFXs */
|
||||
/* Reset Sensors for WPI_TalonFXs */
|
||||
resetEncoders();
|
||||
|
||||
/* Configure the left Talon's selected sensor as local QuadEncoder */
|
||||
@@ -194,50 +168,43 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
/* Configure the Remote Talon's selected sensor as a remote sensor for the right Talon */
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_leftFrontMotor.getDeviceID(), // Device ID of Source
|
||||
RemoteSensorSource.TalonSRX_SelectedSensor, DriveConstants.REMOTE_0, // Source number [0, 1]
|
||||
RemoteSensorSource.TalonSRX_SelectedSensor,
|
||||
DriveConstants.REMOTE_0, // Source number [0, 1]
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
|
||||
/* Diff Signal signal to be used for Distance*/
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Configure Diff [Sum of both QuadEncoders] to be used for Primary PID Index */
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.SensorDifference,
|
||||
DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Configure the Pigeon IMU to the other Remote Slot available on the right Talon */
|
||||
m_rightFrontMotor.configRemoteFeedbackFilter( m_pigeon.getDeviceID(), RemoteSensorSource.Pigeon_Yaw,
|
||||
DriveConstants.REMOTE_1, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Setup Sum signal to be used for Distance */
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum0, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Sum1, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Diff Signal */
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff1, FeedbackDevice.RemoteSensor0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configSensorTerm(SensorTerm.Diff0, FeedbackDevice.IntegratedSensor, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Configure Sum [Sum of both QuadEncoders] to be used for Primary PID Index */
|
||||
configMotorSensor(FeedbackDevice.SensorDifference);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
/*
|
||||
m_rightFrontMotor.configSelectedFeedbackCoefficient( 1, // Coefficient
|
||||
DriveConstants.PID_PRIMARY, // PID Slot of Source
|
||||
DriveConstants.DRIVE_TIMEOUT_MS); // Configuration Timeout
|
||||
*/
|
||||
|
||||
/* Config Remote1 to be used for Aux PID Index */
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( FeedbackDevice.RemoteSensor1,
|
||||
DriveConstants.PID_TURN,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//m_rightFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_TURN, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Don't scale the Feedback Sensor (use 1 for 1:1 ratio) DOESN'T WORK */
|
||||
//m_leftFrontMotor.configSelectedFeedbackCoefficient(1, DriveConstants.PID_PRIMARY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
/**
|
||||
* configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
|
||||
* output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
|
||||
* local output is PID0 - PID1, and other side Talon is PID0 + PID1
|
||||
*/
|
||||
m_rightFrontMotor.configAuxPIDPolarity(DriveConstants.isAuxPIDInverted, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Set status frame periods to ensure we don't have stale data */
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_12_Feedback1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.setStatusFramePeriod(StatusFrame.Status_14_Turn_PIDF1, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftFrontMotor.setStatusFramePeriod(StatusFrame.Status_2_Feedback0, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.setStatusFramePeriod(StatusFrame.Status_13_Base_PIDF0, 20, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_pigeon.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_9_SixDeg_YPR, 5, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* Max out the peak output (for all modes). However you can limit the output of
|
||||
@@ -259,37 +226,39 @@ public class Drive extends SubsystemBase {
|
||||
* derivative error never gets large enough to be useful. - sensor movement is
|
||||
* very slow causing the derivative error to be near zero.
|
||||
*/
|
||||
int closedLoopTimeMs = 1;
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.CLOSED_LOOP_TIME_MS,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.configClosedLoopPeriod( DriveConstants.PID_TURN,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.CLOSED_LOOP_TIME_MS,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
DriveConstants.CLOSED_LOOP_TIME_MS,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_leftBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
closedLoopTimeMs,
|
||||
m_rightBackMotor.configClosedLoopPeriod( DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.CLOSED_LOOP_TIME_MS,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* configAuxPIDPolarity(boolean invert, int timeoutMs) false means talon's local
|
||||
* output is PID0 + PID1, and other side Talon is PID0 - PID1 true means talon's
|
||||
* local output is PID0 - PID1, and other side Talon is PID0 + PID1
|
||||
*/
|
||||
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/* Set up Differential Drive */
|
||||
m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
|
||||
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
/* Set up Differential Drive Odometry. */
|
||||
m_odometry = new DifferentialDriveOdometry( Rotation2d.fromDegrees(getHeading()),
|
||||
new Pose2d(0, 0, new Rotation2d()));
|
||||
|
||||
/* Set up Orchestra */
|
||||
m_orchestra = new Orchestra();
|
||||
|
||||
/* Set up music for drive train */
|
||||
m_orchestra.addInstrument(m_leftBackMotor);
|
||||
m_orchestra.addInstrument(m_rightFrontMotor);
|
||||
m_orchestra.addInstrument(m_rightBackMotor);
|
||||
m_orchestra.addInstrument(m_leftFrontMotor);
|
||||
|
||||
/* Create chooser to choose song to play */
|
||||
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
|
||||
System.err.println(songsDir.getPath());
|
||||
String[] songsStrings = songsDir.list();
|
||||
@@ -297,90 +266,38 @@ public class Drive extends SubsystemBase {
|
||||
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
|
||||
}
|
||||
Shuffleboard.getTab("Songs").add(m_songChooser);
|
||||
}
|
||||
|
||||
String currentSong = "";
|
||||
/* Start counting time */
|
||||
m_lastTimeMs = System.currentTimeMillis();
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
|
||||
SmartDashboard.putNumber("Time Seconds", System.currentTimeMillis());
|
||||
m_lastTimeMs = m_currentTimeMs;
|
||||
m_currentTimeMs = System.currentTimeMillis();
|
||||
m_currentTimeSec = m_currentTimeMs / 1000;
|
||||
m_deltaTimeMs = m_currentTimeMs - m_lastTimeMs;
|
||||
|
||||
if (m_currentTimeSec % 30 == 0) {
|
||||
coolFalcon(true);
|
||||
SmartDashboard.putBoolean("Solenoid", true);
|
||||
} else if ((m_currentTimeSec - 1) % 30 == 0) {
|
||||
coolFalcon(false);
|
||||
SmartDashboard.putBoolean("Solenoid", false);
|
||||
}
|
||||
|
||||
m_deltaTime = System.currentTimeMillis() - m_lastTime;
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
m_lastAngleYaw = m_currentAngleYaw;
|
||||
m_currentAngleYaw = getGyroYaw();
|
||||
|
||||
m_rightFrontMotorPos = m_rightFrontMotor.getSelectedSensorPosition();
|
||||
m_rightFrontMotorVel = m_rightFrontMotor.getSelectedSensorVelocity();
|
||||
|
||||
try {
|
||||
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
|
||||
SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
|
||||
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
|
||||
SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
m_lastRightPosTicks = m_currentRightPosTicks;
|
||||
m_lastLeftPosTicks = m_currentLeftPosTicks;
|
||||
m_currentRightPosTicks = m_rightFrontMotor.getSensorCollection().getIntegratedSensorPosition();
|
||||
m_currentLeftPosTicks = m_leftFrontMotor.getSensorCollection().getIntegratedSensorPosition();
|
||||
|
||||
SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
|
||||
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
|
||||
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
|
||||
//SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
|
||||
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
|
||||
|
||||
//SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
|
||||
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
|
||||
SmartDashboard.putNumber("Odometry Heading", getHeading());
|
||||
|
||||
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
|
||||
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
|
||||
|
||||
if (currentSong != m_songChooser.getSelected()){
|
||||
currentSong = m_songChooser.getSelected();
|
||||
selectSong(currentSong);
|
||||
System.err.println(currentSong);
|
||||
}
|
||||
} catch (Exception e) {
|
||||
System.err.println("Error in the Drive Subsystem");
|
||||
// e.printStackTrace(System.err);
|
||||
}
|
||||
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
|
||||
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
|
||||
|
||||
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
|
||||
inchesToMeters(getDistanceInches(m_leftBackMotor)),
|
||||
-inchesToMeters(getDistanceInches(m_rightBackMotor)));
|
||||
}
|
||||
getDistanceInches(m_leftFrontMotor),
|
||||
-getDistanceInches(m_rightFrontMotor));
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
*
|
||||
* @param mode NeutralMode to set motors to
|
||||
*/
|
||||
public void setDriveTrainNeutralMode(NeutralMode mode) {
|
||||
m_leftFrontMotor.setNeutralMode(mode);
|
||||
m_rightFrontMotor.setNeutralMode(mode);
|
||||
m_leftBackMotor.setNeutralMode(mode);
|
||||
m_rightBackMotor.setNeutralMode(mode);
|
||||
runFalconCooling();
|
||||
updateSmartDashboard();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -388,7 +305,7 @@ public class Drive extends SubsystemBase {
|
||||
* using the Differential Drive class to manage the two inputs
|
||||
*/
|
||||
public void driveWithInput(double move, double steer) {
|
||||
m_driveTrain.arcadeDrive(move, steer);
|
||||
m_driveTrain.arcadeDrive(steer, move);
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
}
|
||||
@@ -424,7 +341,7 @@ public class Drive extends SubsystemBase {
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
//m_driveTrain.feedWatchdog();
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -442,7 +359,7 @@ public class Drive extends SubsystemBase {
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
//m_driveTrain.feedWatchdog();
|
||||
m_driveTrain.feedWatchdog();
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -456,6 +373,7 @@ public class Drive extends SubsystemBase {
|
||||
|
||||
m_rightFrontMotor.set(ControlMode.MotionMagic, targetPos, DemandType.AuxPID, targetGyro);
|
||||
m_leftFrontMotor.follow(m_rightFrontMotor, FollowerType.AuxOutput1);
|
||||
|
||||
m_leftBackMotor.follow(m_leftFrontMotor);
|
||||
m_rightBackMotor.follow(m_rightFrontMotor);
|
||||
|
||||
@@ -469,9 +387,9 @@ public class Drive extends SubsystemBase {
|
||||
* @param targetAngle target angle in degrees
|
||||
*/
|
||||
public void runTurningPID(double targetAngle) {
|
||||
double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
//double targetGyro = (targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
|
||||
|
||||
runDriveVelocityPID(0, targetGyro);
|
||||
runDriveVelocityPID(0, targetAngle);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -514,12 +432,100 @@ public class Drive extends SubsystemBase {
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects the feedback device for the motors.
|
||||
* @param feedbackDevice The feedback device to set it to, usually SensorDifference or
|
||||
* Set to high or low gear based on boolean state, true = high, false = low
|
||||
* @param state Chooses between high or low gear
|
||||
*/
|
||||
public void configMotorSensor(FeedbackDevice type) {
|
||||
m_rightFrontMotor.configSelectedFeedbackSensor( type, DriveConstants.PID_PRIMARY,
|
||||
DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
public void setShiftState(boolean state) {
|
||||
if (state == true) {
|
||||
m_speedShift.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
if (state == false) {
|
||||
m_speedShift.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
setRightMotorGains(state);
|
||||
m_isSpeedShiftHigh = state;
|
||||
}
|
||||
|
||||
/**
|
||||
* Set to open or close solenoid that cools the falcon, true = open, false = close
|
||||
* @param state Chooses between open and close
|
||||
*/
|
||||
public void coolFalcon(boolean state) {
|
||||
if (state == true) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
*
|
||||
*/
|
||||
public void runFalconCooling() {
|
||||
if (m_currentTimeSec % 30 == 0) {
|
||||
coolFalcon(true);
|
||||
SmartDashboard.putBoolean("Solenoid", true);
|
||||
} else if ((m_currentTimeSec - 1) % 30 == 0) {
|
||||
coolFalcon(false);
|
||||
SmartDashboard.putBoolean("Solenoid", false);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects a song to play!
|
||||
* @param song The name of the song to be played
|
||||
*/
|
||||
public void selectSong(String song) {
|
||||
SmartDashboard.putString("Selected Song", song);
|
||||
m_orchestra.loadMusic(song);
|
||||
}
|
||||
|
||||
/*
|
||||
* Plays Music!
|
||||
*/
|
||||
public void playSong() {
|
||||
m_orchestra.play();
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the encoders for both motors.
|
||||
*/
|
||||
public void resetEncoders() {
|
||||
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_totalLeftDistanceInches = 0;
|
||||
m_totalRightDistanceInches = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void setOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the yaw of the pigeon
|
||||
*/
|
||||
public void resetGyroYaw() {
|
||||
m_pigeon.setYaw(0);
|
||||
m_pigeon.setAccumZAngle(0);
|
||||
resetGyroAngles();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add docs here
|
||||
*/
|
||||
public void resetGyroAngles() {
|
||||
m_lastAngleYaw = 0;
|
||||
m_currentAngleYaw = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -553,29 +559,13 @@ public class Drive extends SubsystemBase {
|
||||
return ypr[2];
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the yaw of the pigeon
|
||||
*/
|
||||
public void resetGyroYaw() {
|
||||
m_pigeon.setYaw(0);
|
||||
m_pigeon.setAccumZAngle(0);
|
||||
resetGyroAngles();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add docs here
|
||||
*/
|
||||
public void resetGyroAngles() {
|
||||
m_lastAngleYaw = 0;
|
||||
m_currentAngleYaw = 0;
|
||||
m_kinematicsTargetAngle = 0;
|
||||
}
|
||||
//lol
|
||||
//sko
|
||||
//ridge
|
||||
/**
|
||||
//brayden=bad coder
|
||||
* Returns the heading of the robot
|
||||
|
||||
/**
|
||||
* Returns the heading of the robot
|
||||
* @return The robot's heading in degrees, from -180 to 180
|
||||
*/
|
||||
public double getHeading() {
|
||||
@@ -589,7 +579,7 @@ public class Drive extends SubsystemBase {
|
||||
*/
|
||||
public double getTurnRate() {
|
||||
double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
|
||||
double turnRate = 1000 * deltaYaw / m_deltaTime;
|
||||
double turnRate = 1000 * deltaYaw / m_deltaTimeMs;
|
||||
return turnRate;
|
||||
}
|
||||
|
||||
@@ -606,28 +596,8 @@ public class Drive extends SubsystemBase {
|
||||
* @return The current wheel speeds.
|
||||
*/
|
||||
public DifferentialDriveWheelSpeeds getWheelSpeeds() {
|
||||
return new DifferentialDriveWheelSpeeds( inchesToMeters(getVelocityInchesPerSecond(m_leftBackMotor)),
|
||||
-inchesToMeters(getVelocityInchesPerSecond(m_rightBackMotor)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the encoders for both motors.
|
||||
*/
|
||||
public void resetEncoders() {
|
||||
m_leftFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_leftBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightBackMotor.getSensorCollection().setIntegratedSensorPosition(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry to the specified pose.
|
||||
*
|
||||
* @param pose The pose to which to set the odometry.
|
||||
*/
|
||||
public void setOdometry(Pose2d pose) {
|
||||
resetEncoders();
|
||||
m_odometry.resetPosition(pose, Rotation2d.fromDegrees(getHeading()));
|
||||
return new DifferentialDriveWheelSpeeds( getVelocityInchesPerSecond(m_leftBackMotor),
|
||||
-getVelocityInchesPerSecond(m_rightBackMotor));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -654,7 +624,11 @@ public class Drive extends SubsystemBase {
|
||||
* @return The converted value in inches
|
||||
*/
|
||||
public double ticksToInches(double ticks) {
|
||||
return ticks * DriveConstants.INCHES_PER_TICK;
|
||||
if (m_isSpeedShiftHigh) {
|
||||
return ticks * DriveConstants.INCHES_PER_TICK_HIGH;
|
||||
} else {
|
||||
return ticks * DriveConstants.INCHES_PER_TICK_LOW;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -663,7 +637,11 @@ public class Drive extends SubsystemBase {
|
||||
* @return The converted value in ticks
|
||||
*/
|
||||
public double inchesToTicks(double inches) {
|
||||
return inches * DriveConstants.TICKS_PER_INCH;
|
||||
if (m_isSpeedShiftHigh) {
|
||||
return inches * DriveConstants.TICKS_PER_INCH_HIGH;
|
||||
} else {
|
||||
return inches * DriveConstants.TICKS_PER_INCH_LOW;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -683,46 +661,145 @@ public class Drive extends SubsystemBase {
|
||||
public double metersToInches(double meters) {
|
||||
return meters * DriveConstants.INCHES_PER_METER;
|
||||
}
|
||||
|
||||
/*
|
||||
* Plays Music!
|
||||
*/
|
||||
public void playSong() {
|
||||
m_orchestra.play();
|
||||
}
|
||||
|
||||
/**
|
||||
* Selects a song to play!
|
||||
* @param song The name of the song to be played
|
||||
*/
|
||||
public void selectSong(String song) {
|
||||
SmartDashboard.putString("Selected Song", song);
|
||||
m_orchestra.loadMusic(song);
|
||||
}
|
||||
/**
|
||||
* Set to high or low gear based on boolean state, true = high, false = low
|
||||
* @param state Chooses between high or low gear
|
||||
*/
|
||||
public void setShiftState(boolean state) {
|
||||
if (state == true) {
|
||||
m_speedShift.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
m_speedShift.set(DoubleSolenoid.Value.kReverse);
|
||||
}
|
||||
}
|
||||
public void setRightMotorGains(boolean isHighGear) {
|
||||
if (!isHighGear) {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
/**
|
||||
* Set to open or close solenoid that cools the falcon, true = open, false = close
|
||||
* @param state Chooses between open and close
|
||||
*/
|
||||
public void coolFalcon(boolean state) {
|
||||
if (state == true) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kForward);
|
||||
}
|
||||
if (state == false) {
|
||||
m_coolFalcon.set(DoubleSolenoid.Value.kReverse);
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicLow.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
} else {
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_VELOCITY, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_TURNING, DriveConstants.PID_TURN);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_TURNING, m_gainsTurningHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_DISTANCE, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_DISTANCE, m_gainsDistanceHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
|
||||
m_rightFrontMotor.selectProfileSlot(DriveConstants.SLOT_MOTION_MAGIC, DriveConstants.PID_PRIMARY);
|
||||
m_rightFrontMotor.config_kF(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kF, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kP(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kP, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kI(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.config_kD(DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configClosedLoopPeakOutput( DriveConstants.SLOT_MOTION_MAGIC, m_gainsMotionMagicHigh.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionCruiseVelocity(DriveConstants.DRIVE_CRUISE_VELOCITY_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionAcceleration(DriveConstants.DRIVE_ACCELERATION_HIGH, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
m_rightFrontMotor.configMotionSCurveStrength(0, DriveConstants.DRIVE_TIMEOUT_MS);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Sets Motors to a NeutralMode.
|
||||
*
|
||||
* @param mode NeutralMode to set motors to
|
||||
*/
|
||||
public void setDriveTrainNeutralMode(NeutralMode mode) {
|
||||
m_leftFrontMotor.setNeutralMode(mode);
|
||||
m_rightFrontMotor.setNeutralMode(mode);
|
||||
m_leftBackMotor.setNeutralMode(mode);
|
||||
m_rightBackMotor.setNeutralMode(mode);
|
||||
}
|
||||
|
||||
public void updateSmartDashboard() {
|
||||
try {
|
||||
//SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
|
||||
//SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
|
||||
//SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
|
||||
SmartDashboard.putNumber("Left Front Output", m_leftFrontMotor.get());
|
||||
SmartDashboard.putNumber("Right Front Output", m_rightFrontMotor.get());
|
||||
SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
|
||||
SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
|
||||
|
||||
//SmartDashboard.putNumber("Left Back Motor Velocity Raw", m_leftBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Right Back Motor Velocity Raw", m_rightBackMotor.getSelectedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Left Motor Position Raw", m_leftFrontMotor.getSelectedSensorPosition());
|
||||
//SmartDashboard.putNumber("Right Motor Position Raw", m_rightFrontMotor.getSelectedSensorPosition(0));
|
||||
|
||||
//SmartDashboard.putNumber("Right Motor Velocity Int Sensor", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
//SmartDashboard.putNumber("Left Motor Velocity Int Sensor", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Motor Pos Inches", getDistanceInches(m_rightFrontMotor));
|
||||
SmartDashboard.putNumber("Right Motor Pos Inches", getDistanceInches(m_leftFrontMotor));
|
||||
|
||||
/*SmartDashboard.putNumber("Right Front Velocity", m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Front Velocity", m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Right Back Velocity", m_rightBackMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
SmartDashboard.putNumber("Left Back Velocity", m_leftBackMotor.getSensorCollection().getIntegratedSensorVelocity());
|
||||
*/
|
||||
|
||||
SmartDashboard.putNumber("Right Motor Temp", m_rightFrontMotor.getTemperature());
|
||||
SmartDashboard.putNumber("Left Motor Temp", m_leftFrontMotor.getTemperature());
|
||||
|
||||
SmartDashboard.putNumber("Right Front Motor Current Supply", m_rightFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Front Motor Current Supply", m_leftFrontMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Right Back Motor Current Supply", m_rightBackMotor.getSupplyCurrent());
|
||||
SmartDashboard.putNumber("Left Back Motor Current Supply", m_leftBackMotor.getSupplyCurrent());
|
||||
|
||||
SmartDashboard.putNumber("Right Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
|
||||
SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getStatorCurrent());
|
||||
SmartDashboard.putNumber("Right Back Motor Current Stator ", m_rightBackMotor.getStatorCurrent());
|
||||
SmartDashboard.putNumber("Left Back Motor Current Stator", m_leftBackMotor.getStatorCurrent());
|
||||
|
||||
//SmartDashboard.putNumber("PID 0 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Error", m_rightFrontMotor.getClosedLoopError(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Target", m_rightFrontMotor.getClosedLoopTarget(DriveConstants.PID_TURN));
|
||||
//SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
|
||||
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
|
||||
|
||||
//SmartDashboard.putString("Odometry Values Meters", getPose().toString());
|
||||
//SmartDashboard.putNumber("Odometry Heading", getHeading());
|
||||
|
||||
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
|
||||
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
|
||||
|
||||
if (m_currentSong != m_songChooser.getSelected()){
|
||||
m_currentSong = m_songChooser.getSelected();
|
||||
selectSong(m_currentSong);
|
||||
//System.err.println(m_currentSong);
|
||||
}
|
||||
} catch (Exception e) {
|
||||
System.err.println("Error while using Drive SmartDashboard");
|
||||
// e.printStackTrace(System.err);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,6 +24,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Gains;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.utility.ShooterTables;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
|
||||
public class Shooter extends SubsystemBase {
|
||||
@@ -41,6 +42,9 @@ public class Shooter extends SubsystemBase {
|
||||
|
||||
double velP;
|
||||
double input;
|
||||
|
||||
ShooterTables m_shooterTable;
|
||||
|
||||
public boolean velReached;
|
||||
public double m_fireVel;
|
||||
public double m_fireAngle;
|
||||
@@ -65,6 +69,18 @@ public class Shooter extends SubsystemBase {
|
||||
int closedLoopTimeMs = 1;
|
||||
m_shooterFalcon.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
m_shooterFalcon.configClosedLoopPeriod(1, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS);
|
||||
|
||||
m_shooterTable = new ShooterTables();
|
||||
|
||||
SmartDashboard.putNumber("CSV 10", m_shooterTable.getVelocity(10));
|
||||
SmartDashboard.putNumber("CSV 200", m_shooterTable.getVelocity(200));
|
||||
SmartDashboard.putNumber("CSV 250", m_shooterTable.getVelocity(250));
|
||||
SmartDashboard.putNumber("CSV 500", m_shooterTable.getVelocity(500));
|
||||
|
||||
SmartDashboard.putNumber("CSV A -30", m_shooterTable.getAngleDisplacement(-30));
|
||||
SmartDashboard.putNumber("CSV A 10", m_shooterTable.getAngleDisplacement(10));
|
||||
SmartDashboard.putNumber("CSV A 5", m_shooterTable.getAngleDisplacement(5));
|
||||
SmartDashboard.putNumber("CSV A 30", m_shooterTable.getAngleDisplacement(30));
|
||||
}
|
||||
|
||||
@Override
|
||||
|
||||
Reference in New Issue
Block a user