mirror of
https://github.com/Team4388/2022NoWayHome.git
synced 2026-06-09 00:38:05 -06:00
Merge remote-tracking branch 'origin/swerve' into robot-logger
This commit is contained in:
nathanrsxtn
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":8.334720117716095,"y":1.881355486848905},"prevControl":null,"nextControl":{"x":8.61849999562068,"y":1.250733535949832},"holonomicAngle":-90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":8.019409142266559,"y":0.26275914620794727},"prevControl":{"x":8.43030614852931,"y":0.2186359777502019},"nextControl":{"x":6.453364630867194,"y":0.430924999781035},"holonomicAngle":178.16716049405798,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.118548168124668,"y":1.9969695111773362},"prevControl":{"x":5.520125502663889,"y":1.354445775900491},"nextControl":{"x":4.750685363439692,"y":2.5855499986862043},"holonomicAngle":124.12854707414519,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":1.3768579261296454,"y":1.187671340859924},"prevControl":{"x":0.7357256093822531,"y":0.7147048776856182},"nextControl":{"x":2.0179902428770378,"y":1.6606378040342298},"holonomicAngle":-147.6118851465991,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.971403046254368,"y":6.06448109447943},"prevControl":{"x":3.647096949366312,"y":4.256698168568751},"nextControl":null,"holonomicAngle":47.68377515946898,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
@@ -0,0 +1,37 @@
|
||||
{
|
||||
"waypoints": [
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 5.004218182347978,
|
||||
"y": 2.0
|
||||
},
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
},
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 5.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.991875448297241,
|
||||
"y": 5.0
|
||||
},
|
||||
"nextControl": null,
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
}
|
||||
],
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"isReversed": null
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
{
|
||||
"waypoints": [
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 2.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"prevControl": null,
|
||||
"nextControl": {
|
||||
"x": 3.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
},
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 2.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.000000000000002,
|
||||
"y": 2.0
|
||||
},
|
||||
"nextControl": {
|
||||
"x": 5.004218182347978,
|
||||
"y": 2.0
|
||||
},
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
},
|
||||
{
|
||||
"anchorPoint": {
|
||||
"x": 5.0,
|
||||
"y": 5.0
|
||||
},
|
||||
"prevControl": {
|
||||
"x": 4.991875448297241,
|
||||
"y": 5.0
|
||||
},
|
||||
"nextControl": null,
|
||||
"holonomicAngle": 0.0,
|
||||
"isReversal": false,
|
||||
"velOverride": null,
|
||||
"isLocked": false
|
||||
}
|
||||
],
|
||||
"maxVelocity": 1.0,
|
||||
"maxAcceleration": 1.0,
|
||||
"isReversed": null
|
||||
}
|
||||
@@ -1 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.034465241344275,"y":5.002934143799322},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":4.992423777951004,"y":2.9954542667706034},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
{"waypoints":[{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":null,"nextControl":{"x":5.055485973040912,"y":4.3828225587485665},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":5.0,"y":3.1241654489412927},"nextControl":{"x":5.0,"y":3.1241654489412927},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":5.0},"prevControl":{"x":5.0,"y":3.0836227700747108},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
@@ -1 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":2.0390109745736735,"y":2.9639231692256494},"prevControl":null,"nextControl":{"x":2.049521340421991,"y":2.9534128033773315},"holonomicAngle":1.2188752351312955,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":3.0},"prevControl":{"x":5.0029341437993216,"y":2.995454266770603},"nextControl":null,"holonomicAngle":-178.60254149056965,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
{"waypoints":[{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":null,"nextControl":{"x":2.014863901928194,"y":2.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.0,"y":2.0},"prevControl":{"x":4.99458955727988,"y":2.0},"nextControl":{"x":4.99458955727988,"y":2.0},"holonomicAngle":0.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":2.0},"prevControl":{"x":1.4597663280316806,"y":2.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":1.0,"maxAcceleration":1.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":3.1215786569504176,"y":1.6921689015791839},"prevControl":null,"nextControl":{"x":1.7026792674275004,"y":1.7552310966690905},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.20566158373696,"y":3.3948481690066825},"prevControl":{"x":1.9969695111804016,"y":3.342296339765093},"nextControl":{"x":4.204717742092026,"y":3.4382853932829898},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":5.076506704737548,"y":2.701164023017701},"prevControl":{"x":4.771706095136329,"y":3.037495730163874},"nextControl":{"x":5.592408962233515,"y":2.131892566470425},"holonomicAngle":92.0952525645957,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.884289630648579,"y":1.7762518283676958},"prevControl":{"x":5.57049389960884,"y":1.7342103649744238},"nextControl":{"x":8.154698525195206,"y":1.816904912993188},"holonomicAngle":-177.33699923393286,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":6.8002067038620355,"y":3.4473999982502432},"prevControl":{"x":8.681562190710942,"y":3.5525036567334225},"nextControl":{"x":4.918851217013129,"y":3.3422963397670644},"holonomicAngle":-177.2241965512169,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.111068291102451,"y":1.681658535732834},"prevControl":{"x":5.139568899827807,"y":1.8393140234576038},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}],"maxVelocity":5.0,"maxAcceleration":5.0,"isReversed":null}
|
||||
@@ -0,0 +1 @@
|
||||
{"waypoints":[{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":null,"nextControl":{"x":3.0000000000000013,"y":3.0},"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":3.0,"y":5.0},"prevControl":{"x":3.0,"y":4.0},"nextControl":{"x":3.0,"y":4.0},"holonomicAngle":-90.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":4.0,"y":3.0},"prevControl":{"x":3.0,"y":3.0},"nextControl":{"x":3.0,"y":3.0},"holonomicAngle":180.0,"isReversal":true,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":2.0,"y":3.0},"prevControl":{"x":3.0271430576167138,"y":3.0},"nextControl":null,"holonomicAngle":0.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
|
||||
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
File diff suppressed because one or more lines are too long
@@ -4,9 +4,12 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
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;
|
||||
|
||||
@@ -27,8 +30,8 @@ public final class Constants {
|
||||
public static final class SwerveDriveConstants {
|
||||
public static final double ROTATION_SPEED = 4;
|
||||
public static final double WHEEL_SPEED = 0.1;
|
||||
public static final double WIDTH = 15.27;
|
||||
public static final double HEIGHT = 15.27;
|
||||
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?
|
||||
@@ -61,6 +64,12 @@ public final class Constants {
|
||||
public static final int SWERVE_TIMEOUT_MS = 30;
|
||||
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));
|
||||
|
||||
// swerve configuration
|
||||
public static final double NEUTRAL_DEADBAND = 0.04;
|
||||
public static final double OPEN_LOOP_RAMP_RATE = 0.2;
|
||||
@@ -71,7 +80,7 @@ public final class Constants {
|
||||
// wheel diameter: official = 4 in, measured = 3.8 in
|
||||
/* Ratio Calculation */
|
||||
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
|
||||
public static final double WHEEL_DIAMETER_INCHES = 3.8;
|
||||
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;
|
||||
|
||||
@@ -127,17 +127,6 @@ public class Robot extends TimedRobot {
|
||||
e.printStackTrace();
|
||||
}
|
||||
|
||||
/*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();
|
||||
|
||||
@@ -4,33 +4,34 @@
|
||||
|
||||
package frc4388.robot;
|
||||
|
||||
import java.io.File;
|
||||
import java.io.IOException;
|
||||
import java.util.Arrays;
|
||||
import java.util.Comparator;
|
||||
import java.util.ArrayList;
|
||||
|
||||
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.math.trajectory.TrapezoidProfile;
|
||||
import edu.wpi.first.wpilibj.Filesystem;
|
||||
import edu.wpi.first.wpilibj.GenericHID;
|
||||
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.LEDConstants;
|
||||
import frc4388.robot.Constants.OIConstants;
|
||||
import frc4388.robot.Constants.SwerveDriveConstants;
|
||||
import frc4388.robot.subsystems.LED;
|
||||
import frc4388.robot.subsystems.SwerveDrive;
|
||||
import frc4388.utility.LEDPatterns;
|
||||
import frc4388.utility.PathPlannerTrajectoryUtil;
|
||||
import frc4388.utility.controller.DeadbandedXboxController;
|
||||
|
||||
/**
|
||||
@@ -48,6 +49,8 @@ public class RobotContainer {
|
||||
private 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);
|
||||
|
||||
/* Controllers */
|
||||
@@ -72,6 +75,8 @@ public class RobotContainer {
|
||||
|
||||
// continually sends updates to the Blinkin LED controller to keep the lights on
|
||||
m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED));
|
||||
|
||||
m_testMotor.set(TalonFXControlMode.PercentOutput, 0);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -97,9 +102,7 @@ public class RobotContainer {
|
||||
.whenPressed(() -> m_robotSwerveDrive.highSpeed(true));
|
||||
|
||||
new JoystickButton(getDriverController(), XboxController.Button.kA.value)
|
||||
|
||||
.whenPressed(() -> zeroOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
// .whenPressed(this::resetOdometry);
|
||||
.whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0))));
|
||||
|
||||
/* Operator Buttons */
|
||||
// activates "Lit Mode"
|
||||
@@ -109,6 +112,46 @@ public class RobotContainer {
|
||||
.whenReleased(() -> m_robotLED.setPattern(LEDConstants.DEFAULT_PATTERN));
|
||||
}
|
||||
|
||||
public Command[] buildAuto(double maxVel, double maxAccel, Object... inputs) {
|
||||
|
||||
ArrayList<Command> commands = new ArrayList<Command>();
|
||||
commands.add(new InstantCommand(() -> m_robotSwerveDrive.m_gyro.reset()));
|
||||
|
||||
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 (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 (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.
|
||||
*
|
||||
@@ -118,34 +161,9 @@ public class RobotContainer {
|
||||
public Command getAutonomousCommand() throws IOException {
|
||||
// https://github.com/mjansen4857/pathplanner/wiki <-- Pathplanner Wiki
|
||||
|
||||
try (
|
||||
PIDController xController = new PIDController(10.0, 0.0, 0.0);
|
||||
PIDController yController = new PIDController(1.3, 0.0, 0.0)) {
|
||||
ProfiledPIDController thetaController = new ProfiledPIDController(
|
||||
10.0, 0.0, 0.0, new TrapezoidProfile.Constraints(2 * Math.PI, Math.PI));
|
||||
thetaController.enableContinuousInput(-Math.PI, Math.PI);
|
||||
PathPlannerTrajectory ppRecorded = PathPlannerTrajectoryUtil
|
||||
.fromPathweaverJson(Arrays.stream(Filesystem.getDeployDirectory().listFiles())
|
||||
.max(Comparator.comparingLong(File::lastModified)).orElseThrow().toPath(), 1.0, 1.0);
|
||||
|
||||
PathPlannerTrajectory ppCurrentPath = ppRecorded; // change this to change auto
|
||||
|
||||
PPSwerveControllerCommand ppSwerveControllerCommand = new PPSwerveControllerCommand(
|
||||
ppCurrentPath,
|
||||
m_robotSwerveDrive::getOdometry,
|
||||
m_robotSwerveDrive.m_kinematics,
|
||||
xController,
|
||||
yController,
|
||||
thetaController,
|
||||
m_robotSwerveDrive::setModuleStates,
|
||||
m_robotSwerveDrive);
|
||||
|
||||
return new SequentialCommandGroup(
|
||||
new InstantCommand(m_robotSwerveDrive.m_gyro::reset),
|
||||
new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(ppCurrentPath.getInitialPose())),
|
||||
ppSwerveControllerCommand,
|
||||
new InstantCommand(m_robotSwerveDrive::stopModules));
|
||||
}
|
||||
Command[] trajCommands = buildAuto(0.5, 0.5, "Move Forward", "Move Down");
|
||||
SequentialCommandGroup ret = new SequentialCommandGroup(trajCommands);
|
||||
return (new ParallelCommandGroup(buildAuto(0.1, 0.1, ret, new InstantCommand(() -> m_testMotor.set(TalonFXControlMode.PercentOutput, 0.2)))));
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -159,7 +177,7 @@ public class RobotContainer {
|
||||
return m_robotSwerveDrive.getOdometry();
|
||||
}
|
||||
|
||||
public void zeroOdometry(Pose2d pose) {
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
m_robotSwerveDrive.resetOdometry(pose);
|
||||
}
|
||||
|
||||
|
||||
@@ -14,6 +14,7 @@ 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.wpilibj.smartdashboard.Field2d;
|
||||
@@ -50,9 +51,12 @@ public class SwerveDrive extends SubsystemBase {
|
||||
/* 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 double speedAdjust = SwerveDriveConstants.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
|
||||
public boolean ignoreAngles;
|
||||
public Rotation2d rotTarget = new Rotation2d();;
|
||||
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
private final Field2d m_field = new Field2d();
|
||||
|
||||
@@ -89,9 +93,12 @@ public class SwerveDrive extends SubsystemBase {
|
||||
m_gyro.getRotation2d(),
|
||||
new Pose2d(),
|
||||
m_kinematics,
|
||||
VecBuilder.fill(0.05, 0.05, Units.degreesToRadians(5)),
|
||||
VecBuilder.fill(Units.degreesToRadians(0.01)),
|
||||
VecBuilder.fill(0.5, 0.5, Units.degreesToRadians(30)));
|
||||
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);
|
||||
}
|
||||
@@ -121,8 +128,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
: new ChassisSpeeds(xSpeedMetersPerSecond, ySpeedMetersPerSecond, rot * SwerveDriveConstants.ROTATION_SPEED));
|
||||
setModuleStates(states);
|
||||
}
|
||||
private Rotation2d rotTarget = new Rotation2d();
|
||||
private ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
|
||||
|
||||
public void driveWithInput(double leftX, double leftY, double rightX, double rightY, boolean fieldRelative)
|
||||
{
|
||||
ignoreAngles = leftX == 0 && leftY == 0 && rightX == 0 && rightY == 0;
|
||||
@@ -153,7 +159,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
|
||||
@Override
|
||||
public void periodic() {
|
||||
//System.err.println(m_gyro.getFusedHeading() +" aaa");
|
||||
// System.err.println(m_gyro.getFusedHeading() +" aaa");
|
||||
updateOdometry();
|
||||
// m_gyro.setFusedHeadingToCompass();
|
||||
// m_gyro.setYawToCompass();
|
||||
@@ -182,7 +188,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_6_SensorFusion, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
// m_gyro.setStatusFramePeriod(PigeonIMU_StatusFrame.CondStatus_1_General, 1, SwerveDriveConstants.SWERVE_TIMEOUT_MS);
|
||||
|
||||
m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
// m_field.setRobotPose(m_poseEstimator.getEstimatedPosition());
|
||||
super.periodic();
|
||||
}
|
||||
|
||||
@@ -217,6 +223,7 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* @return Robot's current pose.
|
||||
*/
|
||||
public Pose2d getOdometry() {
|
||||
// return m_odometry.getPoseMeters();
|
||||
return m_poseEstimator.getEstimatedPosition();
|
||||
}
|
||||
|
||||
@@ -224,16 +231,24 @@ public class SwerveDrive extends SubsystemBase {
|
||||
* Resets the odometry of the robot to (x=0, y=0, theta=0).
|
||||
*/
|
||||
public void resetOdometry(Pose2d pose) {
|
||||
// m_odometry.resetPosition(pose, m_gyro.getRotation2d());
|
||||
m_poseEstimator.resetPosition(pose, m_gyro.getRotation2d());
|
||||
}
|
||||
|
||||
/** Updates the field relative position of the robot. */
|
||||
|
||||
public void updateOdometry() {
|
||||
m_poseEstimator.update( m_gyro.getRotation2d(),
|
||||
modules[0].getState(),
|
||||
modules[1].getState(),
|
||||
modules[2].getState(),
|
||||
modules[3].getState());
|
||||
|
||||
// m_odometry.update( m_gyro.getRotation2d(),
|
||||
// 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.
|
||||
@@ -242,6 +257,11 @@ public class SwerveDrive extends SubsystemBase {
|
||||
// Timer.getFPGATimestamp() - 0.1);
|
||||
}
|
||||
|
||||
public void resetGyro(){
|
||||
m_gyro.reset();
|
||||
rotTarget = new Rotation2d(0);
|
||||
}
|
||||
|
||||
public void stopModules() {
|
||||
modules[0].stop();
|
||||
modules[1].stop();
|
||||
|
||||
@@ -48,12 +48,11 @@ 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.15;
|
||||
driveTalonFXConfiguration.slot0.kI = 0.0;
|
||||
driveTalonFXConfiguration.slot0.kD = 0.5;
|
||||
driveMotor.configAllSettings(driveTalonFXConfiguration);
|
||||
|
||||
CANCoderConfiguration canCoderConfiguration = new CANCoderConfiguration();
|
||||
canCoderConfiguration.magnetOffsetDegrees = offset;
|
||||
@@ -91,7 +90,8 @@ public class SwerveModule extends SubsystemBase {
|
||||
|
||||
|
||||
double feetPerSecond = Units.metersToFeet(state.speedMetersPerSecond);
|
||||
driveMotor.set(angleMotor.get() + feetPerSecond / SwerveDriveConstants.SPEED_FEET_PER_SECOND_AT_FULL_POWER);
|
||||
// driveMotor.set(TalonFXControlMode.Velocity, /*angleMotor.get() + */(Units.metersToInches(state.speedMetersPerSecond) * SwerveDriveConstants.TICKS_PER_INCH) / 10);
|
||||
driveMotor.set(feetPerSecond / SwerveDriveConstants.MAX_SPEED_FEET_PER_SEC);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
Reference in New Issue
Block a user