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

Added Pigeon APIs

Browse Source 
Ability to get Yaw, Pitch, and Roll, and reset the Yaw.
This commit is contained in:
Aarav
2020-01-09 18:05:16 -07:00
parent ae469b5066
commit 87d5c1322f
2 changed files with 28 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Drive.java
+26
View File
@@ -10,6 +10,7 @@ package frc4388.robot.subsystems;
import com.ctre.phoenix.motorcontrol.InvertType;
import com.ctre.phoenix.motorcontrol.NeutralMode;
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.wpilibj2.command.SubsystemBase;
@@ -27,6 +28,7 @@ public class Drive extends SubsystemBase {
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 PigeonIMU m_pigeon = new PigeonIMU(DriveConstants.PIGEON_ID);
public static DifferentialDrive m_driveTrain = new DifferentialDrive(m_leftFrontMotor, m_rightFrontMotor);
@@ -39,6 +41,7 @@ public class Drive extends SubsystemBase {
m_rightFrontMotor.configFactoryDefault();
m_leftBackMotor.configFactoryDefault();
m_rightBackMotor.configFactoryDefault();
m_pigeon.configFactoryDefault();
/* set back motors as followers */
m_leftBackMotor.follow(m_leftFrontMotor);
@@ -63,4 +66,27 @@ public class Drive extends SubsystemBase {
public void driveWithInput(double move, double steer){
m_driveTrain.arcadeDrive(move, steer);
}
public double getGyroYaw() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[0];
}
public double getGyroPitch() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[1];
}
public double getGyroRoll() {
double[] ypr = new double[3];
m_pigeon.getYawPitchRoll(ypr);
return ypr[2];
}
public void resetGyroYaw() {
m_pigeon.setYaw(0);
m_pigeon.setAccumZAngle(0);
}
}