diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c86560c..c6f72b1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -7,12 +7,16 @@ 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; @@ -29,8 +33,34 @@ public class Robot extends TimedRobot { private RobotTime m_robotTime = RobotTime.getInstance(); private RobotContainer m_robotContainer; - private WPI_Pigeon2 pigeon = new WPI_Pigeon2(14); - private RobotGyro gyro = new RobotGyro(pigeon); + 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 @@ -41,6 +71,9 @@ 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.frontLeft, + bot.frontRight, bot.backLeft, bot.backRight, bot.gyro, bot)); } /** @@ -122,9 +155,6 @@ public class Robot extends TimedRobot { */ @Override public void teleopPeriodic() { - SmartDashboard.putNumber("Gyro Yaw", gyro.getYaw()); - SmartDashboard.putNumber("Gyro Pitch", gyro.getPitch()); - SmartDashboard.putNumber("Gyro Roll", gyro.getRoll()); 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/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 23312a8..a7c4517 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.PigeonIMU; import com.ctre.phoenix.sensors.WPI_Pigeon2; @@ -26,6 +27,8 @@ 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 RobotMap() { configureLEDMotorControllers(); configureDriveMotorControllers(); diff --git a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java index 33a672e..4299737 100644 --- a/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java +++ b/src/main/java/frc4388/robot/commands/AutoBalanceTF2.java @@ -4,26 +4,51 @@ package frc4388.robot.commands; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.TalonFXControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import com.ctre.phoenix.sensors.WPI_Pigeon2; + +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.PIDCommand; +import frc4388.robot.Robot; +import frc4388.utility.RobotGyro; // 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 { + // private TalonSRX frontLeft; + // private TalonSRX frontRight; + // private TalonSRX backLeft; + // private TalonSRX backRight; + /** Creates a new AutoBalanceTF2. */ - public AutoBalanceTF2() { + public AutoBalanceTF2(TalonSRX frontLeft, TalonSRX frontRight, TalonSRX backLeft, TalonSRX backRight, RobotGyro gyro, Robot.MicroBot bot) { super( // The controller that the command will use - new PIDController(0, 0, 0), + new PIDController(.7, .02, .1), // This should return the measurement - () -> 0, + () -> Math.abs(gyro.getPitch()) < 3 ? 0 : gyro.getPitch(), // This should return the setpoint (can also be a constant) () -> 0, // This uses the output output -> { // Use the output here + double out2 = -MathUtil.clamp(output / 20, -1, 1); + if (Math.abs(gyro.getPitch()) < 3) out2 = 0; + + SmartDashboard.putNumber("Output", out2); + frontLeft.set(ControlMode.PercentOutput, out2); + frontRight.set(ControlMode.PercentOutput, out2); + backRight.set(ControlMode.PercentOutput, out2); + backLeft.set(ControlMode.PercentOutput, out2); }); + + gyro.reset(); + addRequirements(bot); // Use addRequirements() here to declare subsystem dependencies. // Configure additional PID options by calling `getController` here. }