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

Merge pull request #35 from Team4388/arm-command

Browse Source 
Arm command --> Master
This commit is contained in:
Aarav Shah
2023-03-14 15:37:18 -06:00
committed by GitHub
parent d17084b113 02b5f62d3c
commit e8da81ca61
13 changed files with 273 additions and 225 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -3
View File
@@ -24,8 +24,8 @@ import frc4388.utility.LEDPatterns;
public final class Constants {
public static final class SwerveDriveConstants {
public static final double MAX_ROT_SPEED = -0.7;
public static final double MIN_ROT_SPEED = -0.3;
public static final double MAX_ROT_SPEED = 1.5;
public static final double MIN_ROT_SPEED = 0.8;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static final class IDs {
@@ -65,7 +65,7 @@ public final class Constants {
public static final class Conversions {
public static final int CANCODER_TICKS_PER_ROTATION = 4096;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 5.8;
public static final double JOYSTICK_TO_METERS_PER_SECOND_FAST = 4.8;
public static final double JOYSTICK_TO_METERS_PER_SECOND_SLOW = 0.8;
public static final double MOTOR_REV_PER_WHEEL_REV = 5.12;
src/main/java/frc4388/robot/Robot.java
+17 -15
View File
@@ -10,15 +10,22 @@ package frc4388.robot;
import java.lang.System;
import java.lang.reflect.Array;
import java.util.Arrays;
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.DemandType;
import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
import java.io.File;
import java.io.IOException;
import java.io.PrintWriter;
import edu.wpi.first.math.geometry.Translation2d;
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.DeferredBlock;
import frc4388.utility.RobotTime;
import frc4388.robot.subsystems.Location;
@@ -38,9 +45,6 @@ public class Robot extends TimedRobot {
private RobotTime m_robotTime = RobotTime.getInstance();
private RobotContainer m_robotContainer;
private Location location = new Location();
/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
@@ -65,18 +69,13 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
// Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled
// commands, running already-scheduled commands, removing finished or interrupted commands,
// 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();
final Tag pos = location.getPosRot();
if (pos != null) {
SmartDashboard.putNumber("x position", pos.x);
}
//ystem.out.print(apriltagPos[0]);
}
/**
@@ -90,7 +89,12 @@ public class Robot extends TimedRobot {
}
@Override
public void disabledPeriodic() {
public void disabledPeriodic() {}
@Override
public void disabledExit() {
DeferredBlock.execute();
super.disabledExit();
}
/**
@@ -134,13 +138,11 @@ public class Robot extends TimedRobot {
* This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
}
public void teleopPeriodic() {}
/**
* This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
public void testPeriodic() {}
}
src/main/java/frc4388/robot/RobotContainer.java
+29 -10
View File
@@ -7,7 +7,10 @@
package frc4388.robot;
import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
@@ -17,8 +20,10 @@ import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.subsystems.Arm;
import frc4388.robot.subsystems.Claw;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.commands.PivotCommand;
import frc4388.robot.commands.PlaybackChooser;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -43,6 +48,8 @@ public class RobotContainer {
public final Arm m_robotArm = new Arm(m_robotMap.pivot, m_robotMap.tele, m_robotMap.pivotEncoder);
public final Claw m_robotClaw = new Claw(m_robotMap.servo);
/* Controllers */
private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
private final DeadbandedXboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID);
@@ -63,8 +70,6 @@ public class RobotContainer {
// private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
private PlaybackChooser playbackChooser;
private PWM servo = new PWM(0);
private boolean servo_open = true;
/**
* The container for the robot. Contains subsystems, OI devices, and commands.
*/
@@ -113,8 +118,8 @@ public class RobotContainer {
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetOdometry(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive));
// // .onFalse()
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -134,15 +139,29 @@ public class RobotContainer {
.onFalse(new InstantCommand());
// * Operator Buttons
// new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
// // .onTrue(new InstantCommand(() -> System.out.println("Claw Button")));
// .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> {
servo.setRaw(servo_open ? 1000 : 2000);
servo_open = !servo_open;
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit()));
.onTrue(new PivotCommand(m_robotArm, 135));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new PivotCommand(m_robotArm, 210));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
// .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));
}
/**
src/main/java/frc4388/robot/RobotMap.java
+10
View File
@@ -12,6 +12,7 @@ import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
import com.ctre.phoenix.sensors.CANCoder;
import com.ctre.phoenix.sensors.WPI_Pigeon2;
import edu.wpi.first.wpilibj.PWM;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.robot.subsystems.SwerveModule;
@@ -115,6 +116,11 @@ public class RobotMap {
rightBackSteer.configNeutralDeadband(SwerveDriveConstants.Configurations.NEUTRAL_DEADBAND, SwerveDriveConstants.TIMEOUT_MS);
// set neutral mode
leftFrontWheel.setNeutralMode(NeutralMode.Brake);
rightFrontWheel.setNeutralMode(NeutralMode.Brake);
leftBackWheel.setNeutralMode(NeutralMode.Brake);
rightBackWheel.setNeutralMode(NeutralMode.Brake);
leftFrontSteer.setNeutralMode(NeutralMode.Brake);
rightFrontSteer.setNeutralMode(NeutralMode.Brake);
leftBackSteer.setNeutralMode(NeutralMode.Brake);
@@ -160,4 +166,8 @@ public class RobotMap {
tele.configForwardSoftLimitEnable(false);
tele.configReverseSoftLimitEnable(false);
}
// claw stuff (WHAT IS A SOAP ENGINEER)
PWM servo = new PWM(0);
}
src/main/java/frc4388/robot/commands/AutoBalance.java
+2 -2
View File
@@ -14,9 +14,9 @@ public class AutoBalance extends PelvicInflammatoryDisease {
RobotGyro gyro;
SwerveDrive drive;
/** Creates a new AutoBalanceTF2. */
/** Creates a new AutoBalance. */
public AutoBalance(RobotGyro gyro, SwerveDrive drive) {
super(0.6, 0, 0, 0);
super(0.6, 0, 0, 0, 0);
this.gyro = gyro;
this.drive = drive;
src/main/java/frc4388/robot/commands/LimeAlign.java
+34
View File
@@ -0,0 +1,34 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
public class LimeAlign extends CommandBase {
public LimeAlign(SwerveDrive drive) {
addRequirements(drive);
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
}
}
src/main/java/frc4388/robot/commands/PelvicInflammatoryDisease.java
+13 -11
View File
@@ -4,20 +4,24 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.utility.Gains;
public abstract class PelvicInflammatoryDisease extends CommandBase {
protected Gains gains;
private double output = 0;
protected Gains gains;
private double output = 0;
private double tolerance = 0;
/** Creates a new PelvicInflammatoryDisease. */
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf) {
gains = new Gains(kp, ki, kd, kf, 0);
public PelvicInflammatoryDisease(double kp, double ki, double kd, double kf, double tolerance) {
gains = new Gains(kp, ki, kd, kf, 0);
this.tolerance = tolerance;
}
public PelvicInflammatoryDisease(Gains gains) {
this.gains = gains;
public PelvicInflammatoryDisease(Gains gains, double tolerance) {
this.gains = gains;
this.tolerance = tolerance;
}
/** produces the error from the setpoint */
@@ -48,11 +52,9 @@ public abstract class PelvicInflammatoryDisease extends CommandBase {
runWithOutput(output);
}
// Called once the command ends or is interrupted.
@Override
public void end(boolean interrupted) {}
// Returns true when the command should end.
@Override
public boolean isFinished() { return false; }
public boolean isFinished() {
return Math.abs(getError()) < tolerance;
}
}
src/main/java/frc4388/robot/commands/PivotCommand.java
+32
View File
@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import frc4388.robot.subsystems.Arm;
public class PivotCommand extends PelvicInflammatoryDisease {
private final Arm arm;
private final double target;
/** Creates a new ArmCommand. */
public PivotCommand(Arm arm, double target) {
super(6, 1.5, 0, 0, 0.015);
this.arm = arm;
this.target = target;
addRequirements(arm);
}
@Override
public double getError() {
return (target - arm.getArmRotation()) / 360;
}
@Override
public void runWithOutput(double output) {
SmartDashboard.putNumber("pivot output", output);
arm.setRotVel(output);
}
}
src/main/java/frc4388/robot/commands/TeleCommand.java
+32
View File
@@ -0,0 +1,32 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package frc4388.robot.commands;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.subsystems.Arm;
public class TeleCommand extends PelvicInflammatoryDisease {
private final Arm arm;
private final double target;
/** Creates a new ArmCommand. */
public TeleCommand(Arm arm, double target) {
super(0.6, 0, 0, 0, 0);
this.arm = arm;
this.target = target;
addRequirements(arm);
}
@Override
public double getError() {
return (arm.getArmLength() - target) /
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
}
@Override
public void runWithOutput(double output) {
arm.setTeleVel(output);
}
}
src/main/java/frc4388/robot/subsystems/Arm.java
+53 -49
View File
@@ -5,18 +5,22 @@ import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
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;
import com.ctre.phoenix.sensors.CANCoderConfiguration;
import frc4388.robot.Constants.ArmConstants;
import frc4388.robot.Constants.SwerveDriveConstants;
import frc4388.utility.DeferredBlock;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Arm extends SubsystemBase {
private WPI_TalonFX m_tele;
private WPI_TalonFX m_pivot;
public WPI_TalonFX m_pivot;
private CANCoder m_pivotEncoder;
private boolean m_debug;
@@ -27,22 +31,12 @@ public class Arm extends SubsystemBase {
m_pivotEncoder = encoder;
m_tele.configFactoryDefault();
// m_tele.configReverseLimitSwitchSource(null, null);
m_tele.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
m_tele.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
m_pivot.configFactoryDefault();
TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
pivotConfig.slot0.kP = ArmConstants.kP;
pivotConfig.slot0.kI = ArmConstants.kI;
pivotConfig.slot0.kD = ArmConstants.kD;
pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
m_pivot.configAllSettings(pivotConfig);
resetTeleSoftLimit();
// * Example of deferred code
new DeferredBlock(() -> resetTeleSoftLimit());
CANCoderConfiguration config = new CANCoderConfiguration();
config.magnetOffsetDegrees = ArmConstants.OFFSET;
@@ -54,11 +48,21 @@ public class Arm extends SubsystemBase {
}
public void setRotVel(double vel) {
m_pivot.set(ControlMode.PercentOutput, vel / 5);
var degrees = Math.abs(getArmRotation()) - 135;
SmartDashboard.putNumber("arm degrees", degrees);
SmartDashboard.putNumber("arm rot vel", vel);
if ((degrees < 2 && vel < 0) || (degrees > 110 && vel > 0)) {
m_pivot.set(ControlMode.PercentOutput, 0);
} else if (degrees > 90 && vel > 0) {
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
} else {
m_pivot.set(ControlMode.PercentOutput, .3 * vel);
}
}
public void setTeleVel(double vel) {
m_tele.set(ControlMode.PercentOutput, -0.25 * vel);
m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
}
public void armSetRotation(double rot) {
@@ -82,21 +86,21 @@ public class Arm extends SubsystemBase {
}
public double getArmLength() {
return (m_tele.getSelectedSensorPosition() - ArmConstants.TELE_FORWARD_SOFT_LIMIT) /
(ArmConstants.TELE_REVERSE_SOFT_LIMIT - ArmConstants.TELE_FORWARD_SOFT_LIMIT);
return m_tele.getSelectedSensorPosition();
}
public double getArmRotation() {
return (m_pivotEncoder.getAbsolutePosition() - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT) /
(ArmConstants.PIVOT_REVERSE_SOFT_LIMIT - ArmConstants.PIVOT_FORWARD_SOFT_LIMIT);
return m_pivotEncoder.getAbsolutePosition();
}
public void runPivotTele(double pivot, double tele) {
double rot = 0;
double abs_pivot = Math.toRadians(getArmRotation() - 135);
double abs_tele = (getArmLength() - ArmConstants.TELE_REVERSE_SOFT_LIMIT) /
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
if (checkLimits(tele, rot)) {
armSetRotation(pivot);
armSetLength(tele);
if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
setRotVel(pivot);
setTeleVel(tele);
}
}
@@ -112,10 +116,7 @@ public class Arm extends SubsystemBase {
var y = ArmConstants.ARM_HEIGHT + len * Math.sin(_theta);
var minHeight = Math.pow(ArmConstants.CURVE_POWER, Math.abs(x));
if (y < minHeight)
return false;
return true;
return y < minHeight;
}
boolean tele_softLimit = false;
@@ -134,33 +135,36 @@ public class Arm extends SubsystemBase {
tele_softLimit = !tele_softLimit;
}
boolean resetable = true;
boolean resetable = true;
boolean tele_reset = true;
@Override
public void periodic() {
double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135);
if (degrees < 2 && resetable) {
var pivot_soft = m_pivot.getSelectedSensorPosition();
var tele_soft = m_tele.getSelectedSensorPosition();
SmartDashboard.putNumber("start pivot", pivot_soft);
SmartDashboard.putNumber("start tele", tele_soft);
m_pivot.configForwardSoftLimitEnable(true);
m_pivot.configReverseSoftLimitEnable(true);
SmartDashboard.putNumber("fwd err", m_pivot.configForwardSoftLimitThreshold(1200 + pivot_soft).value);
SmartDashboard.putNumber("rvs err", m_pivot.configReverseSoftLimitThreshold(pivot_soft).value);
resetable = false;
} else if (degrees > 2) {
resetable = true;
if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
var tele_soft = m_tele.getSelectedSensorPosition();
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
m_tele.configForwardSoftLimitEnable(true);
m_tele.configReverseSoftLimitEnable(true);
tele_reset = false;
} else if (m_tele.isFwdLimitSwitchClosed() == 0) {
tele_reset = true;
}
double x = Math.cos(Math.toRadians(degrees));
SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED + x * (SwerveDriveConstants.MAX_ROT_SPEED - SwerveDriveConstants.MIN_ROT_SPEED);
// if (m_debug)
// SmartDashboard.putNumber("Arm Motor", m_tele.getSelectedSensorPosition());
SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition());
SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition());
// double x = Math.cos(Math.toRadians(degrees));
}
boolean soft_limits = true;
public void killSoftLimits() {
resetTeleSoftLimit();
var pivot_soft = m_pivot.getSelectedSensorPosition();
var tele_soft = m_tele.getSelectedSensorPosition();
m_pivot.configForwardSoftLimitEnable(!soft_limits);
m_pivot.configReverseSoftLimitEnable(!soft_limits);
soft_limits = !soft_limits;
}
}
src/main/java/frc4388/robot/subsystems/Claw.java
+12 -4
View File
@@ -1,9 +1,12 @@
package frc4388.robot.subsystems;
import edu.wpi.first.hal.PWMJNI;
import edu.wpi.first.wpilibj.PWM;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
public class Claw {
public class Claw extends SubsystemBase {
private PWM m_clawMotor;
private boolean m_open = false;
private boolean m_open = false;
private boolean m_disabled = false;
// Opens claw
public Claw(PWM m_clawMotor) {
@@ -14,10 +17,15 @@ public class Claw {
public void setClaw(boolean open) {
// Open claw
m_open = open;
m_clawMotor.setRaw(open ? 0 : 2000);
m_clawMotor.setRaw(m_open ? 1000 : 2000);
}
public void toggle() {
setClaw(!m_open);
}
public boolean isClawOpen() {
return m_open;
}
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+13 -131
View File
@@ -35,10 +35,6 @@ public class SwerveDrive extends SubsystemBase {
private RobotGyro gyro;
// private SwerveDriveOdometry odometry;
private SwerveDrivePoseEstimator poseEstimator;
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public Rotation2d rotTarget = new Rotation2d();
@@ -53,48 +49,26 @@ public class SwerveDrive extends SubsystemBase {
this.gyro = gyro;
// this.odometry = new SwerveDriveOdometry(
// kinematics,
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// getOdometry()
// );
this.poseEstimator = new SwerveDrivePoseEstimator(
kinematics,
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
},
new Pose2d(0,0, new Rotation2d(0))
);
this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack};
}
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.1) {
rotTarget = gyro.getRotation2d();
rot = rightStick.getX();
} else {
rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
}
double rot = rightStick.getX();
// Use the left joystick to set speed. Apply a quadratic curve and the set max speed.
// Use the left joystick to set speed. Apply a cubic curve and the set max speed.
Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
// Convert field-relative speeds to robot-relative speeds.
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -121,82 +95,8 @@ public class SwerveDrive extends SubsystemBase {
public void resetGyro() {
gyro.reset();
// setOdometry(getOdometry());
rotTarget = new Rotation2d(0);
}
/**
* Updates the odometry of the SwerveDrive.
*/
// public void updateOdometry() {
// odometry.update(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// }
// );
// }
public void updatePoseEstimator() {
poseEstimator.update(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
}
);
}
/**
* Gets the odometry of the SwerveDrive.
* @return The odometry of the SwerveDrive as a Pose2d object (xMeters, yMeters, theta).
*/
// public Pose2d getOdometry() {
// return odometry.getPoseMeters();
// }
public Pose2d getPoseEstimator() {
return poseEstimator.getEstimatedPosition();
}
/**
* Sets the odometry of the SwerveDrive.
* @param pose Pose to set the odometry to.
*/
// public void setOdometry(Pose2d pose) {
// odometry.resetPosition(
// gyro.getRotation2d(),
// new SwerveModulePosition[] {
// leftFront.getPosition(),
// rightFront.getPosition(),
// leftBack.getPosition(),
// rightBack.getPosition()
// },
// pose
// );
// }
public void setPoseEstimator(Pose2d pose) {
poseEstimator.resetPosition(
gyro.getRotation2d(),
new SwerveModulePosition[] {
leftFront.getPosition(),
rightFront.getPosition(),
leftBack.getPosition(),
rightBack.getPosition()
},
pose
);
}
public void resetPoseEstimator() {
setPoseEstimator(new Pose2d());
}
public void stopModules() {
for (SwerveModule module : this.modules) {
@@ -204,14 +104,6 @@ public class SwerveDrive extends SubsystemBase {
}
}
/**
* Resets the odometry of the SwerveDrive to 0.
* *NOTE: If you reset your gyro, this method MUST be called with the new gyro angle and wheel encoder positions.
*/
// public void resetOdometry() {
// setOdometry(new Pose2d());
// }
public SwerveDriveKinematics getKinematics() {
return this.kinematics;
}
@@ -219,24 +111,14 @@ public class SwerveDrive extends SubsystemBase {
@Override
public void periodic() {
// This method will be called once per scheduler run
// updateOdometry();
updatePoseEstimator();
// SmartDashboard.putNumber("Odo X (ft)", Units.metersToFeet(this.getOdometry().getX()));
// SmartDashboard.putNumber("Odo Y (ft)", Units.metersToFeet(this.getOdometry().getY()));
// SmartDashboard.putNumber("Odo Theta", this.getOdometry().getRotation().getDegrees());
// SmartDashboard.putNumber("Gyro Angle", getGyroAngle());
// SmartDashboard.putNumber("rotTarget", this.rotTarget.getDegrees());
}
/**
* Shifts gear from high to low, or vice versa.
* @param shift true to shift to high, false to shift to low
*/
public void highSpeed(boolean shift) {
this.speedAdjust = shift ? SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST : SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
public void toggleGear() {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
}
}
}
src/main/java/frc4388/utility/DeferredBlock.java
+23
View File
@@ -0,0 +1,23 @@
package frc4388.utility;
import java.util.ArrayList;
public class DeferredBlock {
private static ArrayList<Runnable> m_blocks = new ArrayList<>();
private static boolean m_hasRun = false;
public DeferredBlock(Runnable block) {
m_blocks.add(block);
}
public static void execute() {
if (m_hasRun) return;
for (Runnable block : m_blocks) {
block.run();
}
m_blocks.clear(); // for garbage collection
m_hasRun = true;
}
}