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);
}
/**
src/main/java/frc4388/robot/RobotContainer.java
+11 -9
View File
@@ -32,7 +32,7 @@ public class RobotContainer {
private final RobotMap m_robotMap = new RobotMap();
/* Subsystems */
private final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.rightFront, m_robotMap.leftBack, m_robotMap.rightBack);//, m_robotMap.gyro);
// private final LED m_robotLED = new LED(m_robotMap.LEDController);
@@ -65,16 +65,18 @@ public class RobotContainer {
// 0, false), m_robotSwerveDrive)
// );
m_robotSwerveDrive.setDefaultCommand(
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
-0.3 * getDriverController().getLeftXAxis(),
0.3 * getDriverController().getLeftYAxis(),
0.3 * getDriverController().getRightXAxis(),
0.3 * getDriverController().getRightYAxis(),
true),
m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
// m_robotSwerveDrive.setDefaultCommand(
// new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
// -0.3 * getDriverController().getLeftXAxis(),
// 0.3 * getDriverController().getLeftYAxis(),
// 0.3 * getDriverController().getRightXAxis(),
// 0.3 * getDriverController().getRightYAxis(),
// true),
// m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand"));
}
/**
* Use this method to define your button->command mappings. Buttons can be
* created by instantiating a {@link GenericHID} or one of its subclasses
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+14 -1
View File
@@ -4,6 +4,7 @@
package frc4388.robot.subsystems;
import edu.wpi.first.math.filter.SlewRateLimiter;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.math.geometry.Translation2d;
@@ -28,7 +29,9 @@ public class SwerveDrive extends SubsystemBase {
private SwerveModule[] modules;
// private RobotGyro gyro;
// private RobotGyro gyro3
private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
private Translation2d rightFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH));
@@ -98,6 +101,16 @@ public class SwerveDrive extends SubsystemBase {
setModuleStates(states);
}
// ! experimental WPILib swerve drive example
public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelative) {
// SwerveModuleState[] states = kinematics.toSwerveModuleStates(
// fieldRelative ? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeed, ySpeed, rot, gyro.getRotation2d())
// : new ChassisSpeeds(xSpeed, ySpeed, rot)
// );
SwerveModuleState[] states = kinematics.toSwerveModuleStates(new ChassisSpeeds(xSpeed, ySpeed, rot * SwerveDriveConstants.ROTATION_SPEED));
setModuleStates(states);
}
/**
* Set each module of the swerve drive to the corresponding desired state.
* @param desiredStates Array of module states to set.