diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e145b38..8db637a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,9 +7,13 @@ package frc4388.robot; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; +import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; /** @@ -106,6 +110,8 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); + + m_robotContainer.gyroRef.reset(); } /** @@ -113,7 +119,9 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - + SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle()); + SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch()); + SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll()); } /** diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 99f5cb1..16c43d3 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.*; import frc4388.robot.subsystems.LED; import frc4388.utility.LEDPatterns; +import frc4388.utility.RobotGyro; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -36,6 +37,8 @@ public class RobotContainer { private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID); private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); + public RobotGyro gyroRef = m_robotMap.gyro; + /** * The container for the robot. Contains subsystems, OI devices, and commands. */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5f17853..23312a8 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -11,6 +11,7 @@ import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj.drive.DifferentialDrive; @@ -22,6 +23,8 @@ import frc4388.utility.RobotGyro; * testing and modularization. */ public class RobotMap { + private WPI_Pigeon2 m_pigeon2 = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(m_pigeon2); public RobotMap() { configureLEDMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java new file mode 100644 index 0000000..33a672e --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -0,0 +1,36 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc4388.robot.commands; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj2.command.PIDCommand; + +// NOTE: Consider using this command inline, rather than writing a subclass. For more +// information, see: +// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html +public class AutoBalanceTF2 extends PIDCommand { + /** Creates a new AutoBalanceTF2. */ + public AutoBalanceTF2() { + super( + // The controller that the command will use + new PIDController(0, 0, 0), + // This should return the measurement + () -> 0, + // This should return the setpoint (can also be a constant) + () -> 0, + // This uses the output + output -> { + // Use the output here + }); + // Use addRequirements() here to declare subsystem dependencies. + // Configure additional PID options by calling `getController` here. + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index d3761c6..eb8621c 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -8,6 +8,7 @@ package frc4388.utility; import com.ctre.phoenix.sensors.PigeonIMU; +import com.ctre.phoenix.sensors.WPI_Pigeon2; import com.ctre.phoenix.sensors.PigeonIMU.CalibrationMode; import com.kauailabs.navx.frc.AHRS; @@ -21,14 +22,13 @@ import edu.wpi.first.math.MathUtil; public class RobotGyro implements Gyro { private RobotTime m_robotTime = RobotTime.getInstance(); - private PigeonIMU m_pigeon = null; + private WPI_Pigeon2 m_pigeon = null; private AHRS m_navX = null; public boolean m_isGyroAPigeon; //true if pigeon, false if navX private double m_lastPigeonAngle; private double m_deltaPigeonAngle; - private double yawZero = 0; private double pitchZero = 0; private double rollZero = 0; @@ -36,7 +36,7 @@ public class RobotGyro implements Gyro { * Creates a Gyro based on a pigeon * @param gyro the gyroscope to use for Gyro */ - public RobotGyro(PigeonIMU gyro) { + public RobotGyro(WPI_Pigeon2 gyro) { m_pigeon = gyro; m_isGyroAPigeon = true; } @@ -53,10 +53,9 @@ public class RobotGyro implements Gyro { /** * Resets yaw, pitch, and roll. */ - public void resetAllAngles() { + public void resetZeroValues() { if (!m_isGyroAPigeon) return; - yawZero = m_pigeon.getYaw(); pitchZero = m_pigeon.getPitch(); rollZero = m_pigeon.getRoll(); } @@ -90,7 +89,7 @@ public class RobotGyro implements Gyro { @Override public void calibrate() { if (m_isGyroAPigeon) { - m_pigeon.enterCalibrationMode(CalibrationMode.Temperature); + m_pigeon.calibrate(); } else { m_navX.calibrate(); } @@ -98,6 +97,8 @@ public class RobotGyro implements Gyro { @Override public void reset() { + resetZeroValues(); + if (m_isGyroAPigeon) { m_pigeon.setYaw(0); } else { @@ -117,7 +118,7 @@ public class RobotGyro implements Gyro { double[] ypr = new double[3]; m_pigeon.getYawPitchRoll(ypr); - return new double[] {(ypr[0] - yawZero), (ypr[1] - pitchZero), (ypr[2] - rollZero)}; + return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; } @Override @@ -182,7 +183,7 @@ public class RobotGyro implements Gyro { } } - public PigeonIMU getPigeon(){ + public WPI_Pigeon2 getPigeon(){ return m_pigeon; }