diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index e145b38..7e548a1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,9 +7,17 @@ package frc4388.robot; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +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 edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc4388.robot.commands.AutoBalanceTF2; +import frc4388.utility.RobotGyro; import frc4388.utility.RobotTime; /** @@ -25,6 +33,35 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; + public static class MicroBot extends SubsystemBase { + public WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); + public RobotGyro gyro = new RobotGyro(pigeon); + + public TalonSRX frontLeft = new TalonSRX(2); + public TalonSRX backLeft = new TalonSRX(3); + public TalonSRX backRight = new TalonSRX(5); + public TalonSRX frontRight = new TalonSRX(4); + + public MicroBot() { + frontRight.configFactoryDefault(); + frontLeft.configFactoryDefault(); + backLeft.configFactoryDefault(); + backRight.configFactoryDefault(); + + frontLeft.setInverted(true); + backLeft.setInverted(true); + } + + public void setOutput(double output) { + frontRight.set(ControlMode.PercentOutput, output); + frontLeft.set(ControlMode.PercentOutput, output); + backLeft.set(ControlMode.PercentOutput, output); + backRight.set(ControlMode.PercentOutput, output); + } + } + + private MicroBot bot = new MicroBot(); + /** * This function is run when the robot is first started up and should be * used for any initialization code. @@ -34,6 +71,8 @@ public class Robot extends TimedRobot { // Instantiate our RobotContainer. This will perform all our button bindings, and put our // autonomous chooser on the dashboard. m_robotContainer = new RobotContainer(); + + bot.setDefaultCommand(new AutoBalanceTF2(bot)); } /** @@ -106,6 +145,8 @@ public class Robot extends TimedRobot { m_autonomousCommand.cancel(); } m_robotTime.startMatchTime(); + + m_robotContainer.gyroRef.reset(); } /** @@ -113,7 +154,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 9674e56..4ae2868 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -16,6 +16,7 @@ import frc4388.robot.Constants.*; import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.LEDPatterns; +import frc4388.utility.RobotGyro; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -39,6 +40,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 308a377..e1a44a5 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -9,6 +9,7 @@ package frc4388.robot; import com.ctre.phoenix.motorcontrol.InvertType; import com.ctre.phoenix.motorcontrol.NeutralMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; import com.ctre.phoenix.sensors.CANCoder; import com.ctre.phoenix.sensors.PigeonIMU; @@ -27,6 +28,10 @@ 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); + + private TalonSRX backleft = new TalonSRX(-1); public SwerveModule leftFront; public SwerveModule rightFront; 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..d06631c --- /dev/null +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -0,0 +1,46 @@ +// 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.MathUtil; +import frc4388.robot.Robot; + +// 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 PelvicInflamitoryDisease { + Robot.MicroBot bot; + + /** Creates a new AutoBalanceTF2. */ + public AutoBalanceTF2(Robot.MicroBot bot) { + super(.7, .1, 15, 0); + addRequirements(bot); + this.bot = bot; + } + + @Override + public double getError() { + return bot.gyro.getPitch(); + } + + @Override + public void runWithOutput(double output) { + double out2 = MathUtil.clamp(output / 40, -.5, .5); + if (Math.abs(bot.gyro.getPitch()) < 3) out2 = 0; + bot.setOutput(out2); + } + + @Override + public void initialize() { + super.initialize(); + this.bot.gyro.reset(); + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java new file mode 100644 index 0000000..e377c0a --- /dev/null +++ b/src/main/java/frc4388/robot/commands/PelvicInflamitoryDisease.java @@ -0,0 +1,59 @@ +// 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.wpilibj2.command.CommandBase; +import frc4388.utility.Gains; + +public abstract class PelvicInflamitoryDisease extends CommandBase { + protected Gains gains; + private double output = 0; + + /** Creates a new PelvicInflamitoryDisease. */ + public PelvicInflamitoryDisease(double kp, double ki, double kd, double kf) { + gains = new Gains(kp, ki, kd, kf, 0); + } + + public PelvicInflamitoryDisease(Gains gains) { + this.gains = gains; + } + + /** produces the error from the setpoint */ + public abstract double getError(); + /** figure it out bitch */ + public abstract void runWithOutput(double output); + + // Called when the command is initially scheduled. + @Override + public void initialize() { + output = 0; + } + + private double prevError, cumError = 0; + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + double error = getError(); + cumError += error * .02; // 20 ms + double delta = error - prevError; + + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; + + runWithOutput(output); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // 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 ef239ca..3261421 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -7,7 +7,9 @@ package frc4388.utility; +import com.ctre.phoenix.sensors.WPI_Pigeon2; 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,18 +23,21 @@ 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 pitchZero = 0; + private double rollZero = 0; + /** * 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; } @@ -46,6 +51,16 @@ public class RobotGyro implements Gyro { m_isGyroAPigeon = false; } + /** + * Resets yaw, pitch, and roll. + */ + public void resetZeroValues() { + if (!m_isGyroAPigeon) return; + + pitchZero = m_pigeon.getPitch(); + rollZero = m_pigeon.getRoll(); + } + /** * Run in periodic if you are using a pigeon. Updates a delta angle so that it can calculate getRate(). Note * that the getRate() method for a navX will likely be much more accurate than for a pigeon. @@ -75,7 +90,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(); } @@ -83,6 +98,8 @@ public class RobotGyro implements Gyro { @Override public void reset() { + resetZeroValues(); + if (m_isGyroAPigeon) { m_pigeon.setYaw(0); } else { @@ -99,9 +116,10 @@ public class RobotGyro implements Gyro { * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { - double[] angles = new double[3]; - m_pigeon.getYawPitchRoll(angles); - return angles; + double[] ypr = new double[3]; + m_pigeon.getYawPitchRoll(ypr); + + return new double[] {ypr[0], (ypr[1] - pitchZero), (ypr[2] - rollZero)}; } @Override @@ -113,6 +131,10 @@ public class RobotGyro implements Gyro { } } + public double getYaw() { + return this.getAngle(); + } + /** * Gets an absolute heading of the robot * @return heading from -180 to 180 degrees @@ -166,7 +188,7 @@ public class RobotGyro implements Gyro { } } - public PigeonIMU getPigeon(){ + public WPI_Pigeon2 getPigeon(){ return m_pigeon; }