Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 00:38:00 -06:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into autonomous-commands

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-26 04:22:16 +00:00
committed by GitHub
parent f1edcbc29c ee39181c7e
commit 6939bc449d
16 changed files with 477 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+37 -12
View File
@@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
@@ -33,6 +34,7 @@ import com.ctre.phoenix.sensors.PigeonIMU;
import com.ctre.phoenix.sensors.PigeonIMU_StatusFrame;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -126,16 +128,24 @@ public class Drive extends SubsystemBase {
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_rightFrontMotor.setInverted(false);
//m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
setDriveTrainNeutralMode(NeutralMode.Coast);
float rampRate = 0.1f;
m_rightFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_rightBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftFrontMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configOpenloopRamp(rampRate, DriveConstants.DRIVE_TIMEOUT_MS);
//SupplyCurrentLimitConfiguration c = new SupplyCurrentLimitConfiguration(true, 40, 35, 0.01);
//m_rightFrontMotor.configSupplyCurrentLimit(c);
//m_leftFrontMotor.configSupplyCurrentLimit(c);
/* 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_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
@@ -312,9 +322,11 @@ public class Drive extends SubsystemBase {
-getDistanceInches(m_rightFrontMotor));
try {
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
//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());
@@ -322,18 +334,30 @@ public class Drive extends SubsystemBase {
//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 Front Motor Current Stator ", m_rightFrontMotor.getStatorCurrent());
//SmartDashboard.putNumber("Left Front Motor Current Stator", m_leftFrontMotor.getSupplyCurrent());
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));
@@ -342,10 +366,10 @@ public class Drive extends SubsystemBase {
//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.putString("Odometry Values Meters", getPose().toString());
//SmartDashboard.putNumber("Odometry Heading", getHeading());
SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTime);
if (currentSong != m_songChooser.getSelected()){
@@ -584,6 +608,7 @@ public class Drive extends SubsystemBase {
* Returns the current yaw of the pigeon
*/
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);