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/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.