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

added requests

Browse Source 
non field relative, but secretly field relative rotation
Arm speeds
This commit is contained in:
Aarav
2023-03-06 16:35:20 -07:00
parent 9111eefe68
commit 7d7d9898c9
5 changed files with 66 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+6 -4
View File
@@ -24,7 +24,9 @@ import frc4388.utility.LEDPatterns;
public final class Constants {
public static final class SwerveDriveConstants {
public static final double ROTATION_SPEED = -0.7;
public static final double MAX_ROT_SPEED = -0.7;
public static final double MIN_ROT_SPEED = -0.3;
public static double ROTATION_SPEED = MAX_ROT_SPEED;
public static final class IDs {
public static final int LEFT_FRONT_WHEEL_ID = 2;
@@ -114,10 +116,10 @@ public final class Constants {
public static final double TELE_TICKS_PER_SECOND = (-5);
public static final double PIVOT_FORWARD_SOFT_LIMIT = -1.0; // TODO: find actual value
public static final double PIVOT_REVERSE_SOFT_LIMIT = -1.0; // TODO: find actual value
public static final double PIVOT_FORWARD_SOFT_LIMIT = 100; // TODO: find actual value
public static final double PIVOT_REVERSE_SOFT_LIMIT = 0; // TODO: find actual value
public static final double TELE_FORWARD_SOFT_LIMIT = 0;
public static final double TELE_FORWARD_SOFT_LIMIT = 100;
public static final double TELE_REVERSE_SOFT_LIMIT = 0;
public static final double kP = 0;
src/main/java/frc4388/robot/RobotContainer.java
+8
View File
@@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
import frc4388.robot.commands.AutoBalance;
import frc4388.robot.commands.JoystickPlayback;
import frc4388.robot.subsystems.Arm;
import frc4388.robot.subsystems.SwerveDrive;
import frc4388.robot.commands.JoystickRecorder;
@@ -128,6 +129,10 @@ public class RobotContainer {
"Blue1Path.txt"))
.onFalse(new InstantCommand());
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
.onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
.onFalse(new InstantCommand());
// * Operator Buttons
new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
@@ -135,6 +140,9 @@ public class RobotContainer {
servo.setRaw(servo_open ? 1000 : 2000);
servo_open = !servo_open;
}));
new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
.onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit()));
}
/**
src/main/java/frc4388/robot/commands/PlaybackChooser.java
+1 -1
View File
@@ -42,7 +42,7 @@ public class PlaybackChooser {
nextChooser();
Shuffleboard.getTab("Auto Chooser")
.add("Add Sequence", new InstantCommand(() -> {}))
.add("Add Sequence", new InstantCommand(() -> nextChooser()))
.withPosition(4, 0);
return this;
}
src/main/java/frc4388/robot/subsystems/Arm.java
+47 -10
View File
@@ -8,6 +8,7 @@ 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 edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -23,6 +24,9 @@ public class Arm extends SubsystemBase {
m_pivot = pivot;
m_pivotEncoder = encoder;
tele.configFactoryDefault();
m_pivot.configFactoryDefault();
TalonFXConfiguration pivotConfig = new TalonFXConfiguration();
pivotConfig.slot0.kP = ArmConstants.kP;
pivotConfig.slot0.kI = ArmConstants.kI;
@@ -31,19 +35,13 @@ public class Arm extends SubsystemBase {
pivotConfig.remoteFilter0.remoteSensorDeviceID = encoder.getDeviceID();
pivotConfig.remoteFilter0.remoteSensorSource = RemoteSensorSource.CANCoder;
pivotConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
pivot.configAllSettings(pivotConfig);
m_pivot.configAllSettings(pivotConfig);
pivot.configForwardSoftLimitThreshold(ArmConstants.TELE_FORWARD_SOFT_LIMIT);
pivot.configReverseSoftLimitThreshold(ArmConstants.TELE_REVERSE_SOFT_LIMIT);
pivot.configForwardSoftLimitEnable(true);
pivot.configReverseSoftLimitEnable(true);
resetTeleSoftLimit();
CANCoderConfiguration config = new CANCoderConfiguration();
config.magnetOffsetDegrees = ArmConstants.OFFSET;
m_pivotEncoder.configAllSettings(config);
tele.configFactoryDefault();
pivot.configFactoryDefault();
}
public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder) {
@@ -55,7 +53,7 @@ public class Arm extends SubsystemBase {
}
public void setTeleVel(double vel) {
m_tele.set(ControlMode.PercentOutput, vel);
m_tele.set(ControlMode.PercentOutput, -0.25 * vel);
}
public void armSetRotation(double rot) {
@@ -115,10 +113,49 @@ public class Arm extends SubsystemBase {
return true;
}
boolean tele_softLimit = false;
public void resetTeleSoftLimit() {
if (!tele_softLimit) {
var tele_soft = m_tele.getSelectedSensorPosition();
m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
m_tele.configReverseSoftLimitThreshold(tele_soft);
m_tele.configForwardSoftLimitEnable(true);
m_tele.configReverseSoftLimitEnable(true);
} else {
m_tele.configForwardSoftLimitEnable(false);
m_tele.configReverseSoftLimitEnable(false);
}
tele_softLimit = !tele_softLimit;
}
boolean resetable = 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;
}
double x = Math.cos(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 AbsPos", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putNumber("Pivot CANCoder", m_pivotEncoder.getAbsolutePosition());
SmartDashboard.putNumber("Pivot IntegratedSensor", m_pivot.getSelectedSensorPosition());
SmartDashboard.putNumber("Telescope Encoder", m_tele.getSelectedSensorPosition());
}
}
src/main/java/frc4388/robot/subsystems/SwerveDrive.java
+4 -3
View File
@@ -83,16 +83,17 @@ public class SwerveDrive extends SubsystemBase {
public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
if (fieldRelative) {
if (rightStick.getNorm() > 0.1) {
rotTarget = new Rotation2d(rightStick.getX(), -rightStick.getY()).minus(new Rotation2d(0, 1));
rotTarget = gyro.getRotation2d();
}
double 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.
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 * speed.getX(), speed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
} else {
// Create robot-relative speeds.