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

Merge pull request #36 from Team4388/arm-command

Browse Source 
Arm command --> Master
This commit is contained in:
Aarav Shah
2023-03-16 16:29:02 -06:00
committed by GitHub
parent 9d784bf88e da2255b180
commit eb2a54992b
8 changed files with 122 additions and 44 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+7 -3
View File
@@ -24,9 +24,13 @@ import frc4388.utility.LEDPatterns;
public final class Constants {
public static final class SwerveDriveConstants {
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 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 double ROT_CORRECTION_SPEED = 10; // MIN_ROT_SPEED;
public static final double CORRECTION_MIN = 10;
public static final double CORRECTION_MAX = 50;
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
src/main/java/frc4388/robot/RobotContainer.java
+19 -5
View File
@@ -14,7 +14,10 @@ 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;
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.WaitCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoBalance;
@@ -25,6 +28,7 @@ import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.JoystickRecorder;
import frc4388.robot.commands.PivotCommand;
import frc4388.robot.commands.PlaybackChooser;
import frc4388.robot.commands.RunArmIn;
import frc4388.utility.controller.DeadbandedXboxController;
import frc4388.utility.controller.XboxController;
@@ -85,8 +89,8 @@ public class RobotContainer {
.withName("SwerveDrive DefaultCommand"));
m_robotArm.setDefaultCommand(new RunCommand(() -> {
m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY());
m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY());
m_robotArm.setRotVel(getDeadbandedOperatorController().getLeftY(), false);
m_robotArm.setTeleVel(getDeadbandedOperatorController().getRightY(), false);
}, m_robotArm)
.withName("Arm DefaultCommand"));
@@ -119,7 +123,7 @@ public class RobotContainer {
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(), m_robotSwerveDrive));
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.toggleGear(m_robotArm.getArmRotation()-135), m_robotSwerveDrive));
// // .onFalse()
new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
@@ -149,13 +153,23 @@ public class RobotContainer {
.onTrue(new PivotCommand(m_robotArm, 135));
new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
.onTrue(new PivotCommand(m_robotArm, 210));
.onTrue(new PivotCommand(m_robotArm, 225));
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
.onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
.onTrue(new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw));
new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
.onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
// TODO: put this into a variable
new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new ParallelCommandGroup(
new InstantCommand(() -> m_robotClaw.toggle()),
new SequentialCommandGroup(
new RunArmIn(m_robotArm),
new PivotCommand(m_robotArm, 135)
)
));
// new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
// .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
src/main/java/frc4388/robot/commands/ExtendArmTo.java
+36
View File
@@ -0,0 +1,36 @@
// 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.Arm;
public class ExtendArmTo extends CommandBase {
private final Arm m_arm;
private final double m_target;
private final boolean m_direction;
public ExtendArmTo(Arm arm, double units) {
m_arm = arm;
m_target = units;
m_direction = m_target > 1;
addRequirements(m_arm);
}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {
m_arm.setTeleVel(m_direction ? 1 : -1, false);
}
// Returns true when the command should end.
@Override
public boolean isFinished() {
if (m_direction)
return m_arm.getTeleUnit() > m_target;
else
return m_arm.getTeleUnit() < m_target;
}
}
src/main/java/frc4388/robot/commands/PivotCommand.java
+1 -1
View File
@@ -27,6 +27,6 @@ public class PivotCommand extends PelvicInflammatoryDisease {
@Override
public void runWithOutput(double output) {
SmartDashboard.putNumber("pivot output", output);
arm.setRotVel(output);
arm.setRotVel(output, true);
}
}
src/main/java/frc4388/robot/commands/LimeAlign.java → src/main/java/frc4388/robot/commands/RunArmIn.java
+12 -9
View File
@@ -5,22 +5,25 @@
package frc4388.robot.commands;
import edu.wpi.first.wpilibj2.command.CommandBase;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.subsystems.Arm;
public class LimeAlign extends CommandBase {
public LimeAlign(SwerveDrive drive) {
addRequirements(drive);
public class RunArmIn extends CommandBase {
private final Arm m_arm;
public RunArmIn(Arm arm) {
m_arm = arm;
addRequirements();
}
// Called when the command is initially scheduled.
@Override
public void initialize() {
}
public void initialize() {}
// Called every time the scheduler runs while the command is scheduled.
@Override
public void execute() {}
public void execute() {
m_arm.setTeleVel(1, true);
}
// Called once the command ends or is interrupted.
@Override
@@ -29,6 +32,6 @@ public class LimeAlign extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
return false;
return m_arm.isTeleIn();
}
}
src/main/java/frc4388/robot/commands/TeleCommand.java
+1 -1
View File
@@ -27,6 +27,6 @@ public class TeleCommand extends PelvicInflammatoryDisease {
@Override
public void runWithOutput(double output) {
arm.setTeleVel(output);
arm.setTeleVel(output, true);
}
}
src/main/java/frc4388/robot/subsystems/Arm.java
+23 -15
View File
@@ -47,7 +47,7 @@ public class Arm extends SubsystemBase {
this(pivot, tele, encoder, false);
}
public void setRotVel(double vel) {
public void setRotVel(double vel, boolean fast) {
var degrees = Math.abs(getArmRotation()) - 135;
SmartDashboard.putNumber("arm degrees", degrees);
SmartDashboard.putNumber("arm rot vel", vel);
@@ -55,14 +55,23 @@ public class Arm extends SubsystemBase {
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);
m_pivot.set(ControlMode.PercentOutput, .2 * vel);
} else {
m_pivot.set(ControlMode.PercentOutput, .3 * vel);
m_pivot.set(ControlMode.PercentOutput, (fast ? .8 : .3) * vel);
}
}
public void setTeleVel(double vel) {
m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
public void setTeleVel(double vel, boolean speed) {
m_tele.set(ControlMode.PercentOutput, (speed ? -.9 : .7) * vel);
}
public boolean isTeleIn() {
return m_tele.isRevLimitSwitchClosed() == 1 ||
m_tele.getSelectedSensorPosition() < reverse_tele;
}
public double getTeleUnit() {
return m_tele.getSelectedSensorPosition() - reverse_tele;
}
public void armSetRotation(double rot) {
@@ -99,8 +108,8 @@ public class Arm extends SubsystemBase {
(ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
if (pivot > 0 || tele < 0 || checkLimits(abs_tele, abs_pivot)) {
setRotVel(pivot);
setTeleVel(tele);
setRotVel(pivot, false);
setTeleVel(tele, false);
}
}
@@ -127,6 +136,8 @@ public class Arm extends SubsystemBase {
m_tele.configReverseSoftLimitThreshold(tele_soft);
m_tele.configForwardSoftLimitEnable(true);
m_tele.configReverseSoftLimitEnable(true);
reverse_tele = tele_soft;
} else {
m_tele.configForwardSoftLimitEnable(false);
m_tele.configReverseSoftLimitEnable(false);
@@ -135,8 +146,9 @@ public class Arm extends SubsystemBase {
tele_softLimit = !tele_softLimit;
}
boolean resetable = true;
boolean tele_reset = true;
boolean resetable = true;
boolean tele_reset = true;
double reverse_tele = 0;
@Override
public void periodic() {
@@ -148,6 +160,8 @@ public class Arm extends SubsystemBase {
m_tele.configReverseSoftLimitThreshold(1000 - tele_soft);
m_tele.configForwardSoftLimitEnable(true);
m_tele.configReverseSoftLimitEnable(true);
reverse_tele = 1000 - tele_soft;
tele_reset = false;
} else if (m_tele.isFwdLimitSwitchClosed() == 0) {
tele_reset = true;
@@ -159,12 +173,6 @@ public class Arm extends SubsystemBase {
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/SwerveDrive.java
+23 -10
View File
@@ -12,6 +12,7 @@ import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import frc4388.robot.Constants.SwerveDriveConstants;
@@ -37,7 +38,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public Rotation2d rotTarget = new Rotation2d();
public double rotTarget = 0;
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
@@ -56,19 +57,23 @@ public class SwerveDrive extends SubsystemBase {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.1) {
rotTarget = gyro.getRotation2d();
rot = rightStick.getX();
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
SmartDashboard.putBoolean("drift correction", false);
} else {
rot = rotTarget.minus(gyro.getRotation2d()).getRadians();
SmartDashboard.putBoolean("drift correction", true);
rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_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 speed = leftStick.times(speedAdjust * leftStick.getNorm());
// Translation2d speed = leftStick.times(speedAdjust / leftStick.getNorm());
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(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.
chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -95,7 +100,7 @@ public class SwerveDrive extends SubsystemBase {
public void resetGyro() {
gyro.reset();
rotTarget = new Rotation2d(0);
rotTarget = 0;
}
public void stopModules() {
@@ -113,11 +118,19 @@ public class SwerveDrive extends SubsystemBase {
// This method will be called once per scheduler run
}
public void toggleGear() {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) {
public void toggleGear(double angle) {
if (this.speedAdjust == SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW && Math.abs(angle) < 2) {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST;
SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MAX_ROT_SPEED;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MAX;
} else {
this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW;
SwerveDriveConstants.ROTATION_SPEED = SwerveDriveConstants.MIN_ROT_SPEED;
SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN;
}
}