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

Merge branch 'configure-normal-mode-for-drivetrain' of https://github.com/Team4388/RiseOfRidgebotics2020 into configure-normal-mode-for-drivetrain

Browse Source
This commit is contained in:
Aarav
2020-01-11 13:00:19 -07:00
parent d4f55b93d6 8554beb065
commit 9c998a997b
1 changed files with 29 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+29 -7
View File
@@ -9,26 +9,31 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
import com.ctre.phoenix.sensors.PigeonIMU;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile.State;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem;
import frc4388.robot.Constants.DriveConstants;
/**
* Add your docs here.
*/
public class Drive extends SubsystemBase {
public class Drive extends ProfiledPIDSubsystem {
// Put methods for controlling this subsystem
// here. Call these from Commands.
public static WPI_TalonSRX m_leftFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
public static WPI_TalonSRX m_rightFrontMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
public static WPI_TalonSRX m_leftBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
public static WPI_TalonSRX m_rightBackMotor = new WPI_TalonSRX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
public static WPI_TalonFX m_leftFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_FRONT_CAN_ID);
public static WPI_TalonFX m_rightFrontMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_FRONT_CAN_ID);
public static WPI_TalonFX m_leftBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_LEFT_BACK_CAN_ID);
public static WPI_TalonFX m_rightBackMotor = new WPI_TalonFX(DriveConstants.DRIVE_RIGHT_BACK_CAN_ID);
public static PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
@@ -36,7 +41,10 @@ public class Drive extends SubsystemBase {
/**
* Add your docs here.
*/
public Drive(){
public Drive() {
/* */
super(new ProfiledPIDController(0, 0, 0, new TrapezoidProfile.Constraints(0, 0)), 0);
/* factory default values */
m_leftFrontMotor.configFactoryDefault();
m_rightFrontMotor.configFactoryDefault();
@@ -65,6 +73,8 @@ public class Drive extends SubsystemBase {
SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
SmartDashboard.putNumber("Pigeon Roll", getGyroRoll());
SmartDashboard.putNumber("Left Motor Velocity Raw", m_leftFrontMotor.getSelectedSensorVelocity());
SmartDashboard.putNumber("Right Motor Velocity Raw", m_rightFrontMotor.getSelectedSensorVelocity());
} catch (Exception e) {
System.err.println("The operation failed successfully.");
}
@@ -99,7 +109,7 @@ public class Drive extends SubsystemBase {
m_pigeon.getYawPitchRoll(ypr);
return ypr[1];
}
public double getGyroRoll() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
@@ -110,4 +120,16 @@ public class Drive extends SubsystemBase {
m_pigeon.setYaw(0);
m_pigeon.setAccumZAngle(0);
}
@Override
protected void useOutput(double output, TrapezoidProfile.State setpoint) {
// TODO Auto-generated method stub
}
@Override
protected double getMeasurement() {
// TODO Auto-generated method stub
return 0;
}
}