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 add-ramsete-controller-and-trajectories

Browse Source
This commit is contained in:
Keenan D. Buckley
2020-02-21 23:41:01 +00:00
committed by GitHub
parent fee7f99a9f 7eaf7c006a
commit c65b584ade
36 changed files with 479 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+54 -5
View File
@@ -7,6 +7,16 @@
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;
@@ -18,16 +28,20 @@ import com.ctre.phoenix.motorcontrol.SensorTerm;
import com.ctre.phoenix.motorcontrol.StatusFrame;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.music.Orchestra;
import com.ctre.phoenix.sensors.PigeonIMU;
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.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;
@@ -48,6 +62,7 @@ public class Drive extends SubsystemBase {
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;
@@ -67,7 +82,6 @@ public class Drive extends SubsystemBase {
public DoubleSolenoid m_coolFalcon;
public int m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
public long m_lastTime, m_deltaTime; //in milliseconds
public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
@@ -99,7 +113,7 @@ public class Drive extends SubsystemBase {
/* flip input so forward becomes back, etc */
m_leftFrontMotor.setInverted(false);
m_rightFrontMotor.setInverted(true);
m_driveTrain.setRightSideInverted(false);
//m_driveTrain.setRightSideInverted(false);
m_leftBackMotor.setInverted(InvertType.FollowMaster);
m_rightBackMotor.setInverted(InvertType.FollowMaster);
@@ -268,8 +282,22 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
m_lastTime = System.currentTimeMillis();
m_orchestra.addInstrument(m_leftBackMotor);
m_orchestra.addInstrument(m_rightFrontMotor);
m_orchestra.addInstrument(m_rightBackMotor);
m_orchestra.addInstrument(m_leftFrontMotor);
File songsDir = new File(Filesystem.getDeployDirectory().getAbsolutePath() + "/songs");
System.err.println(songsDir.getPath());
String[] songsStrings = songsDir.list();
for (String songString : songsStrings){
m_songChooser.addOption(songString, songsDir.getAbsolutePath() + "/" + songString);
}
Shuffleboard.getTab("Songs").add(m_songChooser);
}
String currentSong = "";
@Override
public void periodic() {
m_currentTimeSec = (int)(System.currentTimeMillis() / 1000);
@@ -326,6 +354,11 @@ public class Drive extends SubsystemBase {
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);
@@ -389,7 +422,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
//m_driveTrain.feedWatchdog();
}
/**
@@ -407,7 +440,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.follow(m_leftFrontMotor);
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
//m_driveTrain.feedWatchdog();
}
/**
@@ -425,6 +458,7 @@ public class Drive extends SubsystemBase {
m_rightBackMotor.follow(m_rightFrontMotor);
m_driveTrain.feedWatchdog();
}
/**
@@ -648,6 +682,21 @@ public class Drive extends SubsystemBase {
}
/*
* 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
*/