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 branch 'master' into arm-command

Browse Source
This commit is contained in:
Aarav Shah
2023-03-16 16:40:33 -06:00
committed by GitHub
parent e2ab14ef5c a9e846cde3
commit 08f60864b4
8 changed files with 36 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/subsystems/Arm.java
+15 -23
View File
@@ -47,7 +47,7 @@ public class Arm extends SubsystemBase {
this(pivot, tele, encoder, false);
}
public void setRotVel(double vel, boolean fast) {
public void setRotVel(double vel) {
var degrees = Math.abs(getArmRotation()) - 135;
SmartDashboard.putNumber("arm degrees", degrees);
SmartDashboard.putNumber("arm rot vel", vel);
@@ -55,23 +55,14 @@ 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, .2 * vel);
m_pivot.set(ControlMode.PercentOutput, .15 * vel);
} else {
m_pivot.set(ControlMode.PercentOutput, (fast ? .8 : .3) * vel);
m_pivot.set(ControlMode.PercentOutput, .3 * 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 setTeleVel(double vel) {
m_tele.set(ControlMode.PercentOutput, -0.5 * vel);
}
public void armSetRotation(double rot) {
@@ -108,8 +99,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, false);
setTeleVel(tele, false);
setRotVel(pivot);
setTeleVel(tele);
}
}
@@ -136,8 +127,6 @@ 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);
@@ -146,9 +135,8 @@ public class Arm extends SubsystemBase {
tele_softLimit = !tele_softLimit;
}
boolean resetable = true;
boolean tele_reset = true;
double reverse_tele = 0;
boolean resetable = true;
boolean tele_reset = true;
@Override
public void periodic() {
@@ -160,8 +148,6 @@ 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;
@@ -173,6 +159,12 @@ 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
+5 -5
View File
@@ -12,7 +12,6 @@ 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;
@@ -38,7 +37,7 @@ public class SwerveDrive extends SubsystemBase {
public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; // * slow by default
public double rotTarget = 0;
public Rotation2d rotTarget = new Rotation2d();
public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
/** Creates a new SwerveDrive. */
@@ -58,6 +57,7 @@ public class SwerveDrive extends SubsystemBase {
if (fieldRelative) {
double rot = 0;
if (rightStick.getNorm() > 0.05) {
rotTarget = gyro.getAngle();
rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED;
@@ -71,15 +71,15 @@ public class SwerveDrive extends SubsystemBase {
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 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, 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);
@@ -106,7 +106,7 @@ public class SwerveDrive extends SubsystemBase {
public void resetGyro() {
gyro.reset();
rotTarget = 0;
rotTarget = new Rotation2d(0);
}
public void stopModules() {