mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 08:48:07 -06:00
Merge branch 'swerve' into Shooter
This commit is contained in:
Aarav Shah
@@ -6,6 +6,13 @@ package frc4388.robot;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.trajectory.TrapezoidProfile;
|
||||
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
@@ -24,13 +31,16 @@ import frc4388.utility.LEDPatterns;
|
||||
*/
|
||||
public final class Constants {
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 0.1;
|
||||
public static final double ROTATION_SPEED = 4;
|
||||
public static final double WHEEL_SPEED = 0.1;
|
||||
public static final double WIDTH = 22;
|
||||
public static final double HEIGHT = 22;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND = 5;
|
||||
public static final double MAX_SPEED_FEET_PER_SEC = 16;
|
||||
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20;
|
||||
public static final double WIDTH = 15.25;
|
||||
public static final double HEIGHT = 15.25;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 11;
|
||||
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 2;
|
||||
public static final double MAX_SPEED_FEET_PER_SEC = 20; // redundant constant?
|
||||
public static final double SPEED_FEET_PER_SECOND_AT_FULL_POWER = 20; // redundant constant?
|
||||
|
||||
//IDs
|
||||
public static final int LEFT_FRONT_STEER_CAN_ID = 2;
|
||||
public static final int LEFT_FRONT_WHEEL_CAN_ID = 3;
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ID = 4;
|
||||
@@ -43,30 +53,58 @@ public final class Constants {
|
||||
public static final int RIGHT_FRONT_STEER_CAN_ENCODER_ID = 11;
|
||||
public static final int LEFT_BACK_STEER_CAN_ENCODER_ID = 12;
|
||||
public static final int RIGHT_BACK_STEER_CAN_ENCODER_ID = 13;
|
||||
// ofsets are in degrees
|
||||
//ofsets are in degrees
|
||||
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.494141;
|
||||
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360. - 59.238281;
|
||||
public static final double LEFT_BACK_ENCODER_OFFSET = 360. - 128.144531;
|
||||
public static final double RIGHT_BACK_ENCODER_OFFSET = 0.933594;
|
||||
// public static final double LEFT_FRONT_ENCODER_OFFSET = 180.0;
|
||||
// public static final double RIGHT_FRONT_ENCODER_OFFSET = 300.0;
|
||||
// public static final double LEFT_BACK_ENCODER_OFFSET = 360.0 - 128.0;
|
||||
// public static final double RIGHT_BACK_ENCODER_OFFSET = 0.0;
|
||||
public static final int GYRO_ID = 14;
|
||||
|
||||
// offsets are in degrees
|
||||
// NATHAN if you truncate or round or simplify these i will cry
|
||||
public static final double LEFT_FRONT_ENCODER_OFFSET = 181.45-3.30;//181.7578125;//180.0;//315.0 +45;//180.0;
|
||||
public static final double RIGHT_FRONT_ENCODER_OFFSET = 360.-59.0625+0.18;//360.-59.0625;//315.0;//224.296875 + 90;//300.0;
|
||||
public static final double LEFT_BACK_ENCODER_OFFSET = 360.-128.222;//308.408203125;//225.0;//45.87890625;//360.0 - 128.0;
|
||||
public static final double RIGHT_BACK_ENCODER_OFFSET = 360.+2.15-3.637;//180-2.021484375;//0.0;//134.384765625 + 45;
|
||||
|
||||
// swerve PID constants
|
||||
public static final int SWERVE_SLOT_IDX = 0;
|
||||
public static final int SWERVE_PID_LOOP_IDX = 1;
|
||||
public static final int SWERVE_TIMEOUT_MS = 30;
|
||||
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 0.0, 0.0, 0, 1.0);
|
||||
public static final Gains SWERVE_GAINS = new Gains(1.0, 0.0, 1.0, 0.0, 0, 1.0);
|
||||
|
||||
// swerve auto constants
|
||||
public static final PIDController X_CONTROLLER = new PIDController(0.5, 0.0, 0.0);
|
||||
public static final PIDController Y_CONTROLLER = new PIDController(2.0, 0.0, 0.0);
|
||||
public static final ProfiledPIDController THETA_CONTROLLER = new ProfiledPIDController(
|
||||
15.0, 0.1, 0.3, new TrapezoidProfile.Constraints(Math.PI, Math.PI));
|
||||
|
||||
public static final double MAX_VEL = 5.0;
|
||||
public static final double MAX_ACC = 5.0;
|
||||
|
||||
// swerve configuration
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
public static final int REMOTE_0 = 0;
|
||||
|
||||
// conversions
|
||||
// gear ratio: 5.14 rev motor = 1 rev wheel
|
||||
// wheel diameter: official = 4 in, measured = 3.8 in
|
||||
/* Ratio Calculation */
|
||||
public static final double MOTOR_REV_PER_STEER_REV = 12.8;
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.142857;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 4.0;
|
||||
public static final double TICKS_PER_MOTOR_REV = 2048;
|
||||
public static final double INCHES_PER_WHEEL_REV = WHEEL_DIAMETER_INCHES * Math.PI;
|
||||
public static final double INCHES_PER_METER = 39.370;
|
||||
public static final double METERS_PER_INCH = 1/INCHES_PER_METER;
|
||||
|
||||
public static final double WHEEL_REV_PER_MOTOR_REV = 1/MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_WHEEL_REV = TICKS_PER_MOTOR_REV * MOTOR_REV_PER_WHEEL_REV;
|
||||
public static final double TICKS_PER_INCH = TICKS_PER_WHEEL_REV/INCHES_PER_WHEEL_REV;
|
||||
public static final double INCHES_PER_TICK = 1/TICKS_PER_INCH;
|
||||
public static final double TICK_TIME_TO_SECONDS = 0.1;
|
||||
public static final double SECONDS_TO_TICK_TIME = 1/TICK_TIME_TO_SECONDS;
|
||||
|
||||
// misc
|
||||
public static final int SMARTDASHBOARD_UPDATE_FRAME = 2;
|
||||
// TODO: put in real numbers for the hub
|
||||
public static final Pose2d HUB_POSE = new Pose2d(new Translation2d(0, 0), new Rotation2d(0));
|
||||
}
|
||||
|
||||
public static final class LEDConstants {
|
||||
@@ -81,6 +119,8 @@ public final class Constants {
|
||||
public static final class OIConstants {
|
||||
public static final int XBOX_DRIVER_ID = 0;
|
||||
public static final int XBOX_OPERATOR_ID = 1;
|
||||
public static final double LEFT_AXIS_DEADBAND = 0.1;
|
||||
public static final double RIGHT_AXIS_DEADBAND = 0.6;
|
||||
}
|
||||
|
||||
public static final class ShooterConstants {
|
||||
|
||||
@@ -4,7 +4,9 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
|
||||
import edu.wpi.first.wpilibj.RobotBase;
|
||||
import frc4388.utility.AnsiLogging;
|
||||
|
||||
/**
|
||||
* Do NOT add any static variables to this class, or any initialization at all.
|
||||
@@ -21,6 +23,9 @@ public final class Main {
|
||||
* <p>If you change your main robot class, change the parameter type.
|
||||
*/
|
||||
public static void main(String... args) {
|
||||
AnsiLogging.systemInstall();
|
||||
RobotBase.startRobot(Robot::new);
|
||||
}
|
||||
}
|
||||
|
||||
// hi ryan -aarav
|
||||
@@ -4,7 +4,12 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.util.logging.Level;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj.DriverStation;
|
||||
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.RobotTime;
|
||||
@@ -17,6 +22,7 @@ import frc4388.utility.RobotTime;
|
||||
* project.
|
||||
*/
|
||||
public class Robot extends TimedRobot {
|
||||
private static final Logger LOGGER = Logger.getLogger(Robot.class.getName());
|
||||
Command m_autonomousCommand;
|
||||
|
||||
private RobotTime m_robotTime = RobotTime.getInstance();
|
||||
@@ -28,6 +34,17 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void robotInit() {
|
||||
if (org.fusesource.jansi.Ansi.isEnabled()) {
|
||||
LOGGER.log(Level.ALL, "Logging Test 1/8");
|
||||
LOGGER.log(Level.SEVERE, "Logging Test 2/8");
|
||||
LOGGER.log(Level.WARNING, "Logging Test 3/8");
|
||||
LOGGER.log(Level.INFO, "Logging Test 4/8");
|
||||
LOGGER.log(Level.CONFIG, "Logging Test 5/8");
|
||||
LOGGER.log(Level.FINE, "Logging Test 6/8");
|
||||
LOGGER.log(Level.FINER, "Logging Test 7/8");
|
||||
LOGGER.log(Level.FINEST, "Logging Test 8/8");
|
||||
}
|
||||
LOGGER.fine("robotInit()");
|
||||
// Instantiate our RobotContainer. This will perform all our button bindings, and put our
|
||||
// autonomous chooser on the dashboard.
|
||||
m_robotContainer = new RobotContainer();
|
||||
@@ -49,6 +66,11 @@ public class Robot extends TimedRobot {
|
||||
// and running subsystem periodic() methods. This must be called from the robot's periodic
|
||||
// block in order for anything in the Command-based framework to work.
|
||||
CommandScheduler.getInstance().run();
|
||||
|
||||
// print odometry data to smart dashboard for debugging (if causing timeout errors, you can comment it)
|
||||
SmartDashboard.putNumber("Odometry X", m_robotContainer.getOdometry().getX());
|
||||
SmartDashboard.putNumber("Odometry Y", m_robotContainer.getOdometry().getY());
|
||||
SmartDashboard.putNumber("Odometry Theta", m_robotContainer.getOdometry().getRotation().getDegrees());
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -58,6 +80,7 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void disabledInit() {
|
||||
LOGGER.fine("disabledInit()");
|
||||
m_robotTime.endMatchTime();
|
||||
}
|
||||
|
||||
@@ -70,19 +93,9 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void autonomousInit() {
|
||||
LOGGER.fine("autonomousInit()");
|
||||
m_autonomousCommand = m_robotContainer.getAutonomousCommand();
|
||||
|
||||
/*String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
|
||||
switch (autoSelected) {
|
||||
case "My Auto":
|
||||
autonomousCommand = new MyAutoCommand();
|
||||
break;
|
||||
case "Default Auto":
|
||||
default:
|
||||
autonomousCommand = new ExampleCommand();
|
||||
break;
|
||||
}*/
|
||||
|
||||
// schedule the autonomous command (example)
|
||||
if (m_autonomousCommand != null) {
|
||||
m_autonomousCommand.schedule();
|
||||
@@ -99,6 +112,8 @@ public class Robot extends TimedRobot {
|
||||
|
||||
@Override
|
||||
public void teleopInit() {
|
||||
LOGGER.fine("teleopInit()");
|
||||
m_robotContainer.m_robotSwerveDrive.m_gyro.addYaw(-1 * m_robotContainer.m_robotSwerveDrive.m_gyro.getYaw());
|
||||
// This makes sure that the autonomous stops running when
|
||||
// teleop starts running. If you want the autonomous to
|
||||
// continue until interrupted by another command, remove
|
||||
@@ -107,6 +122,7 @@ public class Robot extends TimedRobot {
|
||||
m_autonomousCommand.cancel();
|
||||
}
|
||||
m_robotTime.startMatchTime();
|
||||
DriverStation.silenceJoystickConnectionWarning(true);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -114,7 +130,6 @@ public class Robot extends TimedRobot {
|
||||
*/
|
||||
@Override
|
||||
public void teleopPeriodic() {
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -10,6 +10,27 @@ import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.Objects;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFX;
|
||||
import com.pathplanner.lib.PathPlanner;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory;
|
||||
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
|
||||
import com.pathplanner.lib.commands.PPSwerveControllerCommand;
|
||||
|
||||
import edu.wpi.first.math.controller.PIDController;
|
||||
import edu.wpi.first.math.controller.ProfiledPIDController;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.wpilibj.XboxController;
|
||||
import edu.wpi.first.wpilibj2.command.Command;
|
||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
||||
|
||||
import frc4388.robot.Constants.*;
|
||||
@@ -21,8 +42,7 @@ import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.robot.subsystems.Turret;
|
||||
import frc4388.robot.subsystems.Vision;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.controller.IHandController;
|
||||
import frc4388.utility.controller.XboxController;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
* This class is where the bulk of the robot should be declared. Since
|
||||
@@ -35,6 +55,12 @@ public class RobotContainer {
|
||||
/* RobotMap */
|
||||
private final RobotMap m_robotMap = new RobotMap();
|
||||
|
||||
/* Subsystems */
|
||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(
|
||||
m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro);
|
||||
|
||||
private final TalonFX m_testMotor = new TalonFX(23);
|
||||
|
||||
private final LED m_robotLED = new LED(m_robotMap.LEDController);
|
||||
private final BoomBoom m_robotBoomBoom = new BoomBoom(m_robotMap.shooterFalconLeft, m_robotMap.shooterFalconRight);
|
||||
private final Hood m_robotHood = new Hood();
|
||||
@@ -53,31 +79,34 @@ public class RobotContainer {
|
||||
m_robotMap.leftBackEncoder,
|
||||
m_robotMap.rightBackEncoder);
|
||||
/* Controllers */
|
||||
private final XboxController m_driverXbox = new XboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
|
||||
private final XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
|
||||
|
||||
/**
|
||||
* The container for the robot. Contains subsystems, OI devices, and commands.
|
||||
*/
|
||||
public RobotContainer() {
|
||||
configureButtonBindings();
|
||||
|
||||
/* Default Commands */
|
||||
// drives the swerve drive with a two-axis input from the driver controller
|
||||
/*m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(-getDriverController().getLeftXAxis(),
|
||||
getDriverController().getLeftYAxis(), -getDriverController().getRightXAxis(), false), m_robotSwerveDrive));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
// m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
|
||||
*/
|
||||
//Turret default command
|
||||
|
||||
m_robotTurret.setDefaultCommand(new AimToCenter(m_robotTurret, m_robotSwerveDrive));
|
||||
// m_robotTurret.setDefaultCommand(
|
||||
// new RunCommand(() -> m_robotTurret.aimToCenter()));
|
||||
|
||||
m_robotSwerveDrive.setDefaultCommand(
|
||||
new RunCommand(() -> m_robotSwerveDrive.driveWithInput(
|
||||
getDriverController().getLeftX(),
|
||||
getDriverController().getLeftY(),
|
||||
getDriverController().getRightX(),
|
||||
getDriverController().getRightY(),
|
||||
true),
|
||||
m_robotSwerveDrive));
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -88,10 +117,26 @@ public class RobotContainer {
|
||||
*/
|
||||
private void configureButtonBindings() {
|
||||
/* Driver Buttons */
|
||||
// "XboxController.Button.kBack" was undefiened yet, 7 works just fine
|
||||
new JoystickButton(getDriverController(), 7)
|
||||
.whenPressed(() -> m_robotSwerveDrive.resetGyro());
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.LEFT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(false));
|
||||
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value)
|
||||
// new XboxControllerRawButton(m_driverXbox, XboxControllerRaw.RIGHT_BUMPER_BUTTON)
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
//.whenPressed(this::resetOdometry);
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
|
||||
new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON)
|
||||
.whenPressed(() -> m_robotLED.setPattern(LEDPatterns.LAVA_RAINBOW))
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
@@ -111,41 +156,104 @@ public class RobotContainer {
|
||||
// new JoystickButton(getOperatorJoystick());
|
||||
}
|
||||
|
||||
/**
|
||||
* Generate autonomous
|
||||
* @param maxVel max velocity for the path (null to override default value of 5.0)
|
||||
* @param maxAccel max acceleration for the path (null to override default value of 5.0)
|
||||
* @param inputs strings (path names) or commands you want to run (in order)
|
||||
* @return array of commands, which can then be processed in a command group
|
||||
*/
|
||||
public Command[] buildAuto(Double maxVel, Double maxAccel, Object... inputs) {
|
||||
|
||||
// default vel and acc
|
||||
maxVel = Objects.requireNonNullElse(maxVel, SwerveDriveConstants.MAX_VEL);
|
||||
maxAccel = Objects.requireNonNullElse(maxAccel, SwerveDriveConstants.MAX_ACC);
|
||||
|
||||
ArrayList<Command> commands = new ArrayList<Command>();
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
|
||||
|
||||
// pids controlling the path
|
||||
PIDController xController = SwerveDriveConstants.X_CONTROLLER;
|
||||
PIDController yController = SwerveDriveConstants.Y_CONTROLLER;
|
||||
ProfiledPIDController thetaController = SwerveDriveConstants.THETA_CONTROLLER;
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
|
||||
// parse input
|
||||
for (int i=0; i<inputs.length; i++) {
|
||||
|
||||
// if string, process as pathplanner trajectory
|
||||
if (inputs[i] instanceof String) {
|
||||
|
||||
PathPlannerTrajectory traj = PathPlanner.loadPath(inputs[i].toString(), maxVel, maxAccel);
|
||||
PathPlannerState initState = (PathPlannerState) traj.sample(0);
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(new Pose2d(initState.poseMeters.getTranslation(), initState.holonomicRotation))));
|
||||
commands.add(new PPSwerveControllerCommand(
|
||||
traj,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive));
|
||||
}
|
||||
|
||||
// if command, just add it to the array
|
||||
if (inputs[i] instanceof Command) {
|
||||
commands.add((Command) inputs[i]);
|
||||
}
|
||||
}
|
||||
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.stopModules()));
|
||||
Command[] ret = new Command[commands.size()];
|
||||
ret = commands.toArray(ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/**
|
||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||
*
|
||||
* @return the command to run in autonomous
|
||||
*/
|
||||
public Command getAutonomousCommand() {
|
||||
// no auto
|
||||
return new InstantCommand();
|
||||
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
|
||||
return new ParallelCommandGroup(
|
||||
buildAuto(
|
||||
null,
|
||||
null,
|
||||
new SequentialCommandGroup(buildAuto(0.5, 0.5, "Move Forward", "Move Down")),
|
||||
new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2))
|
||||
)
|
||||
);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getDriverController() {
|
||||
public XboxController getDriverController() {
|
||||
return m_driverXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get odometry.
|
||||
* @return Odometry
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
|
||||
/**
|
||||
* Set odometry to given pose.
|
||||
* @param pose Pose to set odometry to.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_robotSwerveDrive.resetOdometry(pose);
|
||||
}
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public IHandController getOperatorController() {
|
||||
public XboxController getOperatorController() {
|
||||
return m_operatorXbox;
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getOperatorJoystick() {
|
||||
return m_operatorXbox.getJoyStick();
|
||||
}
|
||||
|
||||
/**
|
||||
* Add your docs here.
|
||||
*/
|
||||
public Joystick getDriverJoystick() {
|
||||
return m_driverXbox.getJoyStick();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -10,6 +10,8 @@ import com.ctre.phoenix.motorcontrol.StatorCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.SupplyCurrentLimitConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
|
||||
import com.revrobotics.CANSparkMax;
|
||||
import com.revrobotics.CANSparkMaxLowLevel.MotorType;
|
||||
|
||||
@@ -18,6 +20,7 @@ import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.robot.Constants.ShooterConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.SwerveModule;
|
||||
|
||||
/**
|
||||
* Defines and holds all I/O objects on the Roborio. This is useful for unit
|
||||
@@ -53,11 +56,14 @@ public class RobotMap {
|
||||
public final CANCoder leftBackEncoder = new CANCoder(SwerveDriveConstants.LEFT_BACK_STEER_CAN_ENCODER_ID);
|
||||
public final CANCoder rightBackEncoder = new CANCoder(SwerveDriveConstants.RIGHT_BACK_STEER_CAN_ENCODER_ID);
|
||||
|
||||
public final WPI_PigeonIMU gyro = new WPI_PigeonIMU(SwerveDriveConstants.GYRO_ID);
|
||||
|
||||
public SwerveModule leftFront;
|
||||
public SwerveModule leftBack;
|
||||
public SwerveModule rightFront;
|
||||
public SwerveModule rightBack;
|
||||
|
||||
void configureSwerveMotorControllers() {
|
||||
leftFrontEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFrontEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
leftBackEncoder.configMagnetOffset(SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightBackEncoder.configMagnetOffset(SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
leftFrontSteerMotor.configFactoryDefault();
|
||||
leftFrontWheelMotor.configFactoryDefault();
|
||||
@@ -95,6 +101,21 @@ public class RobotMap {
|
||||
rightBackSteerMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
rightBackWheelMotor.configNeutralDeadband(SwerveDriveConstants.NEUTRAL_DEADBAND, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
NeutralMode mode = NeutralMode.Coast;
|
||||
leftFrontSteerMotor.setNeutralMode(mode);
|
||||
leftFrontWheelMotor.setNeutralMode(mode);//Coast
|
||||
rightFrontSteerMotor.setNeutralMode(mode);
|
||||
rightFrontWheelMotor.setNeutralMode(mode);//Coast
|
||||
leftBackSteerMotor.setNeutralMode(mode);
|
||||
leftBackWheelMotor.setNeutralMode(mode);//Coast
|
||||
rightBackSteerMotor.setNeutralMode(mode);
|
||||
rightBackWheelMotor.setNeutralMode(mode);//Coast
|
||||
|
||||
leftFront = new SwerveModule(leftFrontWheelMotor, leftFrontSteerMotor, leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET);
|
||||
leftBack = new SwerveModule(leftBackWheelMotor, leftBackSteerMotor, leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET);
|
||||
rightFront = new SwerveModule(rightFrontWheelMotor, rightFrontSteerMotor, rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET);
|
||||
rightBack = new SwerveModule(rightBackWheelMotor, rightBackSteerMotor, rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET);
|
||||
|
||||
// config cancoder as remote encoder for swerve steer motors
|
||||
leftFrontSteerMotor.configRemoteFeedbackFilter(leftFrontEncoder.getDeviceID(),
|
||||
RemoteSensorSource.CANCoder, SwerveDriveConstants.REMOTE_0,
|
||||
|
||||
@@ -4,10 +4,10 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import frc4388.robot.Constants.LEDConstants;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
|
||||
@@ -27,12 +27,12 @@ public class LED extends SubsystemBase {
|
||||
m_LEDController = LEDController;
|
||||
setPattern(LEDConstants.DEFAULT_PATTERN);
|
||||
updateLED();
|
||||
System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
Logger.getLogger(LED.class.getName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
|
||||
}
|
||||
|
||||
@Override
|
||||
public void periodic(){
|
||||
SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
//SmartDashboard.putNumber("LED", m_currentPattern.getValue());
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -4,182 +4,250 @@
|
||||
|
||||
package frc4388.robot.subsystems;
|
||||
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
import java.util.ArrayList;
|
||||
|
||||
import com.ctre.phoenix.sensors.WPI_PigeonIMU;
|
||||
import com.ctre.phoenix.sensors.PigeonIMU.FusionStatus;
|
||||
|
||||
import edu.wpi.first.math.VecBuilder;
|
||||
import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
|
||||
import edu.wpi.first.math.geometry.Pose2d;
|
||||
import edu.wpi.first.math.geometry.Rotation2d;
|
||||
import edu.wpi.first.math.geometry.Translation2d;
|
||||
import edu.wpi.first.math.kinematics.ChassisSpeeds;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
|
||||
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
|
||||
import edu.wpi.first.math.kinematics.SwerveModuleState;
|
||||
import edu.wpi.first.math.util.Units;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.Field2d;
|
||||
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
|
||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
import frc4388.utility.RobotGyro;
|
||||
|
||||
public class SwerveDrive extends SubsystemBase {
|
||||
SwerveDriveKinematics m_kinematics;
|
||||
private WPI_TalonFX m_leftFrontSteerMotor;
|
||||
private WPI_TalonFX m_leftFrontWheelMotor;
|
||||
private WPI_TalonFX m_rightFrontSteerMotor;
|
||||
private WPI_TalonFX m_rightFrontWheelMotor;
|
||||
private WPI_TalonFX m_leftBackSteerMotor;
|
||||
private WPI_TalonFX m_leftBackWheelMotor;
|
||||
private WPI_TalonFX m_rightBackSteerMotor;
|
||||
private WPI_TalonFX m_rightBackWheelMotor;
|
||||
private CANCoder m_leftFrontEncoder;
|
||||
private CANCoder m_rightFrontEncoder;
|
||||
private CANCoder m_leftBackEncoder;
|
||||
private CANCoder m_rightBackEncoder;
|
||||
|
||||
private SwerveModule m_leftFront;
|
||||
private SwerveModule m_leftBack;
|
||||
private SwerveModule m_rightFront;
|
||||
private SwerveModule m_rightBack;
|
||||
|
||||
double halfWidth = SwerveDriveConstants.WIDTH / 2.d;
|
||||
double halfHeight = SwerveDriveConstants.HEIGHT / 2.d;
|
||||
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
|
||||
Translation2d m_frontLeftLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(halfHeight),
|
||||
Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_frontRightLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(halfHeight),
|
||||
Units.inchesToMeters(-halfWidth));
|
||||
Translation2d m_backLeftLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(-halfHeight),
|
||||
Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_backRightLocation =
|
||||
new Translation2d(
|
||||
Units.inchesToMeters(-halfHeight),
|
||||
Units.inchesToMeters(-halfWidth));
|
||||
//setSwerveGains();
|
||||
Translation2d m_frontLeftLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_frontRightLocation = new Translation2d(Units.inchesToMeters(halfHeight), Units.inchesToMeters(-halfWidth));
|
||||
Translation2d m_backLeftLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(halfWidth));
|
||||
Translation2d m_backRightLocation = new Translation2d(Units.inchesToMeters(-halfHeight), Units.inchesToMeters(-halfWidth));
|
||||
|
||||
private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
public SwerveDriveKinematics m_kinematics = new SwerveDriveKinematics(m_frontLeftLocation, m_frontRightLocation, m_backLeftLocation, m_backRightLocation);
|
||||
|
||||
public SwerveModule[] modules;
|
||||
public RobotGyro gyro; //TODO Add Gyro Lol
|
||||
public WPI_PigeonIMU m_gyro;
|
||||
protected FusionStatus fstatus = new FusionStatus();
|
||||
|
||||
/* Here we use SwerveDrivePoseEstimator so that we can fuse odometry readings. The numbers used
|
||||
below are robot specific, and should be tuned. */
|
||||
public SwerveDrivePoseEstimator m_poseEstimator;
|
||||
public SwerveDriveOdometry m_odometry;
|
||||
|
||||
public SwerveDrive(WPI_TalonFX leftFrontSteerMotor,WPI_TalonFX leftFrontWheelMotor,WPI_TalonFX rightFrontSteerMotor,WPI_TalonFX rightFrontWheelMotor,
|
||||
WPI_TalonFX leftBackSteerMotor,WPI_TalonFX leftBackWheelMotor,WPI_TalonFX rightBackSteerMotor,WPI_TalonFX rightBackWheelMotor, CANCoder leftFrontEncoder,
|
||||
CANCoder rightFrontEncoder,
|
||||
CANCoder leftBackEncoder,
|
||||
CANCoder rightBackEncoder)
|
||||
{
|
||||
m_leftFrontSteerMotor = leftFrontSteerMotor;
|
||||
m_leftFrontWheelMotor = leftFrontWheelMotor;
|
||||
m_rightFrontSteerMotor = rightFrontSteerMotor;
|
||||
m_rightFrontWheelMotor = rightFrontWheelMotor;
|
||||
m_leftBackSteerMotor = leftBackSteerMotor;
|
||||
m_leftBackWheelMotor = leftBackWheelMotor;
|
||||
m_rightBackSteerMotor = rightBackSteerMotor;
|
||||
m_rightBackWheelMotor = rightBackWheelMotor;
|
||||
m_leftFrontEncoder = leftFrontEncoder;
|
||||
m_rightFrontEncoder = rightFrontEncoder;
|
||||
m_leftBackEncoder = leftBackEncoder;
|
||||
m_rightBackEncoder = rightBackEncoder;
|
||||
public double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
public boolean ignoreAngles;
|
||||
public Rotation2d rotTarget = new Rotation2d();
|
||||
|
||||
modules = new SwerveModule[] {
|
||||
new SwerveModule(m_leftFrontWheelMotor, m_leftFrontSteerMotor, m_leftFrontEncoder, SwerveDriveConstants.LEFT_FRONT_ENCODER_OFFSET), // Front Left
|
||||
new SwerveModule(m_rightFrontWheelMotor, m_rightFrontSteerMotor, m_rightFrontEncoder, SwerveDriveConstants.RIGHT_FRONT_ENCODER_OFFSET), // Front Right
|
||||
new SwerveModule(m_leftBackWheelMotor, m_leftBackSteerMotor, m_leftBackEncoder, SwerveDriveConstants.LEFT_BACK_ENCODER_OFFSET), // Back Left
|
||||
new SwerveModule(m_rightBackWheelMotor, m_rightBackSteerMotor, m_rightBackEncoder, SwerveDriveConstants.RIGHT_BACK_ENCODER_OFFSET) // Back Right
|
||||
};
|
||||
//gyro.reset();
|
||||
private final Field2d m_field = new Field2d();
|
||||
|
||||
public SwerveDrive(SwerveModule leftFront, SwerveModule leftBack, SwerveModule rightFront, SwerveModule rightBack, WPI_PigeonIMU gyro) {
|
||||
|
||||
m_leftFront = leftFront;
|
||||
m_leftBack = leftBack;
|
||||
m_rightFront = rightFront;
|
||||
m_rightBack = rightBack;
|
||||
m_gyro = gyro;
|
||||
|
||||
modules = new SwerveModule[] {m_leftFront, m_rightFront, m_leftBack, m_rightBack};
|
||||
|
||||
m_poseEstimator =
|
||||
new SwerveDrivePoseEstimator(
|
||||
m_gyro.getRotation2d(),
|
||||
new Pose2d(),
|
||||
m_kinematics,
|
||||
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)),
|
||||
VecBuilder.fill(Units.degreesToRadians(1)),
|
||||
VecBuilder.fill(1.0, 1.0, Units.degreesToRadians(1)));
|
||||
|
||||
m_odometry = new SwerveDriveOdometry(m_kinematics, m_gyro.getRotation2d());
|
||||
|
||||
m_gyro.reset();
|
||||
SmartDashboard.putData("Field", m_field);
|
||||
}
|
||||
//https://github.com/ZachOrr/MK3-Swerve-Example
|
||||
/**
|
||||
* Method to drive the robot using joystick info.
|
||||
*
|
||||
* @param xSpeed Speed of the robot in the x direction (forward).
|
||||
* @param ySpeed Speed of the robot in the y direction (sideways).
|
||||
* @param speeds[0] Speed of the robot in the x direction (forward).
|
||||
* @param speeds[1] Speed of the robot in the y direction (sideways).
|
||||
* @param rot Angular rate of the robot.
|
||||
* @param fieldRelative Whether the provided x and y speeds are relative to the field.
|
||||
*/
|
||||
public void driveWithInput(double xSpeed, double ySpeed, double rot, boolean fieldRelative)
|
||||
public void driveWithInput(double speedX, double speedY, double rot, boolean fieldRelative)
|
||||
{
|
||||
/*var speeds = new ChassisSpeeds(strafeX, strafeY, rotate * SwerveDriveConstants.ROTATION_SPEED //in rad/s );
|
||||
driveFromSpeeds(speeds);*/
|
||||
double xSpeedMetersPerSecond = xSpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
double ySpeedMetersPerSecond = ySpeed * SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND;
|
||||
SwerveModuleState[] states =
|
||||
kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3, gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot*3));
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(states, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
for (int i = 0; i < states.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = states[i];
|
||||
module.setDesiredState(state);
|
||||
}
|
||||
}
|
||||
//Converts a ChassisSpeed to SwerveModuleStates (targets)
|
||||
public void driveFromSpeeds(ChassisSpeeds speeds)
|
||||
{
|
||||
//https://docs.wpilib.org/en/stable/docs/software/kinematics-and-odometry/swerve-drive-kinematics.html
|
||||
// Convert to module states
|
||||
SwerveModuleState[] moduleStates = m_kinematics.toSwerveModuleStates(speeds);
|
||||
if (speedX == 0 && speedY == 0 && rot == 0) ignoreAngles = true;
|
||||
else ignoreAngles = false;
|
||||
Translation2d speed = new Translation2d(speedX, speedY);
|
||||
double mag = speed.getNorm();
|
||||
speed = speed.times(mag * speedAdjust);
|
||||
|
||||
// Front left module state
|
||||
SwerveModuleState leftFront = SwerveModuleState.optimize(moduleStates[0], Rotation2d.fromDegrees(m_leftFrontEncoder.getPosition()));
|
||||
// Front right module state
|
||||
SwerveModuleState rightFront = SwerveModuleState.optimize(moduleStates[1], Rotation2d.fromDegrees(m_rightFrontEncoder.getPosition()));
|
||||
// Back left module state
|
||||
SwerveModuleState leftBack = SwerveModuleState.optimize(moduleStates[2], Rotation2d.fromDegrees(m_leftBackEncoder.getPosition()));
|
||||
// Back right module state
|
||||
SwerveModuleState rightBack = SwerveModuleState.optimize(moduleStates[3], Rotation2d.fromDegrees(m_rightBackEncoder.getPosition()));
|
||||
|
||||
//Set the motors
|
||||
setSwerveMotors(leftFront, leftBack, rightFront, rightBack);
|
||||
double xSpeedMetersPerSecond = -speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
SwerveModuleState[] states =
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
}
|
||||
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
|
||||
{
|
||||
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
|
||||
Translation2d speed = new Translation2d(leftX, leftY);
|
||||
speed = speed.times(speed.getNorm() * speedAdjust);
|
||||
if (Math.abs(rightX) > OIConstants.RIGHT_AXIS_DEADBAND || Math.abs(rightY) > OIConstants.RIGHT_AXIS_DEADBAND)
|
||||
rotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0, 1));
|
||||
double rot = rotTarget.minus(m_gyro.getRotation2d()).getRadians();
|
||||
double xSpeedMetersPerSecond = -speed.getX();
|
||||
double ySpeedMetersPerSecond = speed.getY();
|
||||
SwerveModuleState[] states =
|
||||
m_kinematics.toSwerveModuleStates(
|
||||
fieldRelative
|
||||
? ChassisSpeeds.fromFieldRelativeSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED, m_gyro.getRotation2d())
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rightX * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
}
|
||||
|
||||
//Sets steering motors to PID values
|
||||
public void setSwerveMotors(SwerveModuleState leftFront, SwerveModuleState leftBack, SwerveModuleState rightFront, SwerveModuleState rightBack)
|
||||
{
|
||||
/*//Set the Wheel motor speeds
|
||||
m_leftFrontWheelMotor.set(m_leftFrontSteerMotor.get() + leftFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightFrontWheelMotor.set(m_rightFrontSteerMotor.get() + rightFront.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_leftBackWheelMotor.set(m_leftBackSteerMotor.get() + leftBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
m_rightBackWheelMotor.set(m_rightBackSteerMotor.get() + rightBack.speedMetersPerSecond * SwerveDriveConstants.WHEEL_SPEED);
|
||||
//PID
|
||||
m_leftFrontSteerMotor.set(TalonFXControlMode.Position, leftFront.angle.getDegrees() * 12000);
|
||||
m_rightFrontSteerMotor.set(TalonFXControlMode.Position, rightFront.angle.getDegrees() * 12000);
|
||||
m_leftBackSteerMotor.set(TalonFXControlMode.Position, leftBack.angle.getDegrees() * 12000);
|
||||
m_rightBackSteerMotor.set(TalonFXControlMode.Position, rightBack.angle.getDegrees());
|
||||
System.out.println("Target: " + leftFront.angle.getDegrees());*/
|
||||
}
|
||||
public void setModuleStates(SwerveModuleState[] desiredStates) {
|
||||
SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC));
|
||||
for (int i = 0; i < desiredStates.length; i++) {
|
||||
SwerveModule module = modules[i];
|
||||
SwerveModuleState state = desiredStates[i];
|
||||
module.setDesiredState(state, false);
|
||||
}
|
||||
// modules[0].setDesiredState(desiredStates[0], false);
|
||||
}
|
||||
|
||||
/*public void setSwerveGains(){
|
||||
m_leftFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightFrontSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightFrontSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_leftBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_leftBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.selectProfileSlot(SwerveDriveConstants.SWERVE_SLOT_IDX, SwerveDriveConstants.SWERVE_PID_LOOP_IDX);
|
||||
m_rightBackSteerMotor.config_kF(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kF, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kP(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kP, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kI(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kI, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
m_rightBackSteerMotor.config_kD(SwerveDriveConstants.SWERVE_SLOT_IDX, m_swerveGains.m_kD, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
}*/
|
||||
@Override
|
||||
public void periodic() {
|
||||
|
||||
updateOdometry();
|
||||
// SmartDashboard.putNumber("Pigeon Fused Heading", m_gyro.getFusedHeading(fstatus));
|
||||
|
||||
SmartDashboard.putNumber("Pigeon Yaw", m_gyro.getYaw());
|
||||
|
||||
// public void driveFieldRelative(double awayFromStation, double towardLeftBoundary, double rotate)
|
||||
// {
|
||||
// var speeds = ChassisSpeeds.fromFieldRelativeSpeeds(awayFromStation, towardLeftBoundary,
|
||||
// rotate * SwerveDriveConstants.RotationSpeed, /*get odometry angle*/)
|
||||
// }
|
||||
// SmartDashboard.putNumber("Front Left", modules[0].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Front Right", modules[1].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Back Left", modules[2].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Back Right", modules[3].driveMotor.getSelectedSensorPosition());
|
||||
// SmartDashboard.putNumber("Pigeon Get Angle", m_gyro.getAngle());
|
||||
// SmartDashboard.putNumber("Pigeon Rotation 2D", m_gyro.getRotation2d().getDegrees());
|
||||
// SmartDashboard.putStringArray("Fusion Status", new String[] {"Is Fusing: "+fstatus.bIsFusing, "Is Valid: "+fstatus.bIsValid, "Heading: "+fstatus.heading});
|
||||
|
||||
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
super.periodic();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the distance between two given poses.
|
||||
* @param p1 The first pose.
|
||||
* @param p2 The second pose.
|
||||
* @return Absolute distance between p1 and p2.
|
||||
*/
|
||||
public double distBtwPoses(Pose2d p1, Pose2d p2) {
|
||||
return Math.sqrt(Math.pow(p1.getX() - p2.getX(), 2) + Math.pow(p1.getY() - p2.getY(), 2));
|
||||
}
|
||||
|
||||
/**
|
||||
* Returns a scalar from your distance to the hub to your target distance.
|
||||
*
|
||||
* @param target_dist The target distance.
|
||||
* @return A scalar that multiplies your distance from the hub to get your target distance.
|
||||
*/
|
||||
public Pose2d poseGivenDist(double target_dist) {
|
||||
Pose2d p1 = m_poseEstimator.getEstimatedPosition();
|
||||
Pose2d p2 = SwerveDriveConstants.HUB_POSE;
|
||||
|
||||
double scalar = target_dist/distBtwPoses(p1, p2);
|
||||
Pose2d new_pose = new Pose2d(p1.getX() * scalar, p1.getY() * scalar, p1.getRotation());
|
||||
|
||||
return new_pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current pose of the robot.
|
||||
* @return Robot's current pose.
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
// return m_odometry.getPoseMeters();
|
||||
return m_poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
/**
|
||||
* Gets the current gyro using regression formula.
|
||||
* @return Rotation2d object holding current gyro in radians
|
||||
*/
|
||||
public Rotation2d getRegGyro() {
|
||||
double regCur = 0.6552670369 + m_gyro.getRotation2d().getDegrees() * 0.9926871527;
|
||||
return new Rotation2d(regCur * Math.PI / 180);
|
||||
}
|
||||
|
||||
/**
|
||||
* Resets the odometry of the robot to the given pose.
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/** Updates the field relative position of the robot.
|
||||
*/
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update( getRegGyro(),
|
||||
modules[0].getState(),
|
||||
modules[1].getState(),
|
||||
modules[2].getState(),
|
||||
modules[3].getState());
|
||||
|
||||
// Also apply vision measurements. We use 0.3 seconds in the past as an example -- on
|
||||
// a real robot, this must be calculated based either on latency or timestamps.
|
||||
// m_poseEstimator.addVisionMeasurement(
|
||||
// m_poseEstimator.getEstimatedPosition(),
|
||||
// Timer.getFPGATimestamp() - 0.1);
|
||||
}
|
||||
|
||||
public void resetGyro(){
|
||||
m_gyro.reset();
|
||||
rotTarget = new Rotation2d(0);
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop all four swerve modules.
|
||||
*/
|
||||
public void stopModules() {
|
||||
modules[0].stop();
|
||||
modules[1].stop();
|
||||
modules[2].stop();
|
||||
modules[3].stop();
|
||||
}
|
||||
|
||||
public void highSpeed(boolean shift){
|
||||
if (shift){
|
||||
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_FAST;
|
||||
}
|
||||
else{
|
||||
speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -7,6 +7,7 @@ package frc4388.robot.subsystems;
|
||||
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
|
||||
import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
|
||||
import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
|
||||
import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
|
||||
import com.ctre.phoenix.sensors.CANCoder;
|
||||
@@ -21,20 +22,28 @@ import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.utility.Gains;
|
||||
|
||||
public class SwerveModule extends SubsystemBase {
|
||||
private WPI_TalonFX driveMotor;
|
||||
private WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX angleMotor;
|
||||
public WPI_TalonFX driveMotor;
|
||||
private CANCoder canCoder;
|
||||
public static Gains m_swerveGains = SwerveDriveConstants.SWERVE_GAINS;
|
||||
|
||||
private static double kEncoderTicksPerRotation = 4096;
|
||||
private SwerveModuleState state;
|
||||
private double canCoderFeedbackCoefficient;
|
||||
|
||||
public long m_currentTime;
|
||||
public long m_lastTime;
|
||||
public double m_deltaTime;
|
||||
|
||||
|
||||
public double m_currentPos;
|
||||
public double m_lastPos;
|
||||
|
||||
/** Creates a new SwerveModule. */
|
||||
public SwerveModule(WPI_TalonFX driveMotor, WPI_TalonFX angleMotor, CANCoder canCoder, double offset) {
|
||||
this.driveMotor = driveMotor;
|
||||
this.angleMotor = angleMotor;
|
||||
this.canCoder = canCoder;
|
||||
canCoderFeedbackCoefficient = canCoder.configGetFeedbackCoefficient();
|
||||
|
||||
TalonFXConfiguration angleTalonFXConfiguration = new TalonFXConfiguration();
|
||||
|
||||
@@ -48,22 +57,38 @@ public class SwerveModule extends SubsystemBase {
|
||||
angleTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
|
||||
angleMotor.configAllSettings(angleTalonFXConfiguration);
|
||||
|
||||
/*TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
driveTalonFXConfiguration.slot0.kP = kDriveP;
|
||||
driveTalonFXConfiguration.slot0.kI = kDriveI;
|
||||
driveTalonFXConfiguration.slot0.kD = kDriveD;
|
||||
driveTalonFXConfiguration.slot0.kF = kDriveF;
|
||||
driveMotor.configAllSettings(driveTalonFXConfiguration);*/
|
||||
// TalonFXConfiguration driveTalonFXConfiguration = new TalonFXConfiguration();
|
||||
// driveTalonFXConfiguration.slot0.kP = 0.05;
|
||||
// driveTalonFXConfiguration.slot0.kI = 0.0;
|
||||
// driveTalonFXConfiguration.slot0.kD = 0.0;
|
||||
// driveTalonFXConfiguration.primaryPID.selectedFeedbackSensor = FeedbackDevice.IntegratedSensor;
|
||||
driveMotor.configFactoryDefault();
|
||||
driveMotor.configSelectedFeedbackSensor(TalonFXFeedbackDevice.IntegratedSensor, 0, 30);
|
||||
driveMotor.configNominalOutputForward(0, 30);
|
||||
driveMotor.configNominalOutputReverse(0, 30);
|
||||
driveMotor.configPeakOutputForward(1, 30);
|
||||
driveMotor.configPeakOutputReverse(-1, 30);
|
||||
driveMotor.configAllowableClosedloopError(0, 0, 30);
|
||||
driveMotor.config_kP(0, 0.5, 30);
|
||||
driveMotor.config_kI(0, 0, 30);
|
||||
driveMotor.config_kD(0, 0, 30);
|
||||
// maybe try a feedforward value?
|
||||
|
||||
// driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
|
||||
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
|
||||
canCoderConfiguration.magnetOffsetDegrees = offset;
|
||||
canCoder.configAllSettings(canCoderConfiguration);
|
||||
|
||||
m_currentTime = System.currentTimeMillis();
|
||||
m_lastTime = System.currentTimeMillis();
|
||||
|
||||
m_lastPos = driveMotor.getSelectedSensorPosition();
|
||||
}
|
||||
|
||||
|
||||
public Rotation2d getAngle() {
|
||||
private Rotation2d getAngle() {
|
||||
// Note: This assumes the CANCoders are setup with the default feedback coefficient
|
||||
// and the sesnor value reports degrees.
|
||||
// and the sensor value reports degrees.
|
||||
return Rotation2d.fromDegrees(canCoder.getAbsolutePosition());
|
||||
}
|
||||
|
||||
@@ -71,10 +96,10 @@ public class SwerveModule extends SubsystemBase {
|
||||
* Set the speed + rotation of the swerve module from a SwerveModuleState object
|
||||
* @param desiredState - A SwerveModuleState representing the desired new state of the module
|
||||
*/
|
||||
public void setDesiredState(SwerveModuleState desiredState) {
|
||||
public void setDesiredState(SwerveModuleState desiredState, boolean ignoreAngle) {
|
||||
Rotation2d currentRotation = getAngle();
|
||||
SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SwerveModuleState state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
// SmartDashboard.putNumber("Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
state = SwerveModuleState.optimize(desiredState, currentRotation);
|
||||
|
||||
// Find the difference between our current rotational position + our new rotational position
|
||||
Rotation2d rotationDelta = state.angle.minus(currentRotation);
|
||||
@@ -82,12 +107,64 @@ public class SwerveModule extends SubsystemBase {
|
||||
// Find the new absolute position of the module based on the difference in rotation
|
||||
double deltaTicks = (rotationDelta.getDegrees() / 360.) * kEncoderTicksPerRotation;
|
||||
// Convert the CANCoder from it's position reading back to ticks
|
||||
double currentTicks = canCoder.getPosition() / canCoder.configGetFeedbackCoefficient();
|
||||
double currentTicks = canCoder.getPosition() / canCoderFeedbackCoefficient;
|
||||
double desiredTicks = currentTicks + deltaTicks;
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
|
||||
if (!ignoreAngle){
|
||||
angleMotor.set(TalonFXControlMode.Position, desiredTicks);
|
||||
}
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
|
||||
double ftPerSec = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
double normFtPerSec = ftPerSec / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC;
|
||||
// double angleCorrection = angleMotor.getSelectedSensorVelocity() * 2.69;
|
||||
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection + (Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
driveMotor.set(normFtPerSec);// - angleMotor.get());
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, angleCorrection); // Ratio between axis = 1/1.75 Ratio of wheel is 5.14/1 ratio of steer is 12.8/1
|
||||
|
||||
// m_currentTime = System.currentTimeMillis();
|
||||
// m_deltaTime = (double) (m_currentTime - m_lastTime);
|
||||
// m_deltaTime = m_deltaTime / 10.0;
|
||||
|
||||
// m_currentPos = driveMotor.getSelectedSensorPosition();
|
||||
|
||||
// double m_desiredCorrectionVel = 3.2 * angleMotor.getSelectedSensorVelocity();
|
||||
// double m_desiredCorrectionPos = (m_deltaTime * m_desiredCorrectionVel) % 2048;
|
||||
// double m_lastPos = (driveMotor.getSelectedSensorPosition() % 2048) - (m_deltaTime * driveMotor.getSelectedSensorVelocity());
|
||||
// double m_actualDesiredPos = m_deltaTime * ((Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
|
||||
// System.out.println("Current Pos: " + driveMotor.getSelectedSensorPosition());
|
||||
// System.out.println("Desired Correction Pos: " + m_desiredCorrectionPos);
|
||||
// System.out.println("Last Pos: " + m_lastPos);
|
||||
|
||||
// driveMotor.set(TalonFXControlMode.Position, 1500/*m_desiredCorrectionPos*/);
|
||||
|
||||
// m_lastTime = m_currentTime;
|
||||
// m_lastPos = m_currentPos;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get current module state.
|
||||
*
|
||||
* @return The current state of the module in m/s.
|
||||
*/
|
||||
public SwerveModuleState getState() {
|
||||
// return state;
|
||||
return new SwerveModuleState(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.INCHES_PER_TICK * SwerveDriveConstants.METERS_PER_INCH * 10, getAngle());
|
||||
}
|
||||
|
||||
/**
|
||||
* Stop the drive and steer motors of current module.
|
||||
*/
|
||||
public void stop() {
|
||||
driveMotor.set(0);
|
||||
angleMotor.set(0);
|
||||
}
|
||||
@Override
|
||||
public void periodic(){
|
||||
Rotation2d currentRotation = getAngle();
|
||||
SmartDashboard.putNumber("Angle Motor " + angleMotor.getDeviceID(), currentRotation.getDegrees());
|
||||
SmartDashboard.putNumber("Drive Motor " + driveMotor.getDeviceID(), ((driveMotor.getSelectedSensorPosition() / 2048) * 360) % 360);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user