Team4388/2023WayOfTheRobot
1
0
Fork 0
mirror of https://github.com/Team4388/2023WayOfTheRobot.git synced 2026-06-09 00:37:59 -06:00
Code Issues Packages Projects Releases Wiki Activity

wpilib example swerve

Browse Source
This commit is contained in:
Abhi
2023-01-24 17:57:48 -07:00
parent 56ad89a974
commit 37cc280c6e
3 changed files with 42 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+17
View File
@@ -11,11 +11,15 @@ import com.ctre.phoenix.motorcontrol.ControlMode;
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.filter.SlewRateLimiter;
import edu.wpi.first.math.util.Units;
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.Constants.SwerveDriveConstants;
import frc4388.robot.commands.AutoBalance;
import frc4388.utility.RobotGyro;
import frc4388.utility.RobotTime;
@@ -60,6 +64,10 @@ public class Robot extends TimedRobot {
}
}
private final SlewRateLimiter xLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter yLimiter = new SlewRateLimiter(3);
private final SlewRateLimiter rotLimiter = new SlewRateLimiter(3);
// private MicroBot bot = new MicroBot();
/**
@@ -157,6 +165,15 @@ public class Robot extends TimedRobot {
// SmartDashboard.putNumber("yaw", m_robotContainer.gyroRef.getAngle());
// SmartDashboard.putNumber("pitch", m_robotContainer.gyroRef.getPitch());
// SmartDashboard.putNumber("roll", m_robotContainer.gyroRef.getRoll());
this.drive(false);
}
private void drive(boolean fieldRelative) {
final double xSpeed = -xLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftYAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
final double ySpeed = -yLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getLeftXAxis(), 0.02) * Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND));
final double rot = -rotLimiter.calculate(MathUtil.applyDeadband(m_robotContainer.getDriverController().getRightXAxis(), 0.02) * Units.feetToMeters(Math.PI));
m_robotContainer.m_robotSwerveDrive.drive(xSpeed, ySpeed, rot, fieldRelative);
}
/**