Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 08:48:07 -06:00
Code Issues Packages Projects Releases Wiki Activity

gains n stuff

Browse Source
This commit is contained in:
aarav18
2022-03-18 17:37:11 -06:00
parent 43e3a14c4d
commit eb34d03c3c
3 changed files with 53 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Constants.java
+3 -3
View File
@@ -185,7 +185,7 @@ public final class Constants {
public static final double MAX_ARM_LENGTH = 53;
public static final double MIN_ARM_LENGTH = 1;
public static final double MOVE_SPEED = .1; // cm per second
public static final double MOVE_SPEED = 30000; // ticks per second
public static final double SHOULDER_RESTING_ANGLE = 0;
public static final double ELBOW_RESTING_ANGLE = 0;
@@ -208,8 +208,8 @@ public final class Constants {
public static final int ELBOW_SLOT_IDX = 0;
public static final int ELBOW_PID_LOOP_IDX = 1;
public static final Gains SHOULDER_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains ELBOW_GAINS = new Gains(.4, 0.0, 0.0, 0.0, 0, 1.0);
public static final Gains SHOULDER_GAINS = new Gains(0.5, 0.0, 0.0, 0.0, 0, 0.2);
public static final Gains ELBOW_GAINS = new Gains(0.5, 0.0, 30.0, 0.0, 0, 1.0); //Prev P 1 d 0.4
public static final int CLIMBER_TIMEOUT_MS = 50;
src/main/java/frc4388/robot/RobotContainer.java
+21 -20
View File
@@ -166,9 +166,9 @@ public class RobotContainer {
// IK command
// m_robotClimber.setDefaultCommand(
// new RunCommand(() -> m_robotClimber.controlWithInput(getOperatorController().getLeftX(),
// getOperatorController().getLeftY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
m_robotClimber.setDefaultCommand(
new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(),
getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand"));
// Swerve Drive with Input
m_robotSwerveDrive.setDefaultCommand(
@@ -208,23 +208,24 @@ public class RobotContainer {
// new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()),
// m_robotTurret).withName("Turret runTurretWithInput defaultCommand"));
m_robotTurret.setDefaultCommand(
new RunCommand(() -> {
if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
}, m_robotTurret));
// m_robotTurret.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentMode.equals(Mode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); }
// if (this.currentMode.equals(Mode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); }
// }, m_robotTurret));
m_robotHood.setDefaultCommand(
new RunCommand(() -> {
if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); }
}, m_robotHood));
// m_robotHood.setDefaultCommand(
// new RunCommand(() -> {
// if (this.currentMode.equals(Mode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); }
// if (this.currentMode.equals(Mode.CLIMBER)) { m_robotHood.runHood(0); }
// }, m_robotHood));
m_robotClimber.setDefaultCommand(
new RunCommand(() -> {
if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
if (this.currentMode.equals(Mode.CLIMBER)) { m_robotClimber.setMotors(-getOperatorController().getLeftX(), -getOperatorController().getRightY()); }
}, m_robotClimber));
// new RunCommand(() -> {
// if (this.currentMode.equals(Mode.SHOOTER)) { m_robotClimber.setMotors(0, 0); }
/*if (this.currentMode.equals(Mode.CLIMBER)) { */new RunCommand(() -> m_robotClimber.controlJointsWithInput(getOperatorController().getLeftX(),
getOperatorController().getRightY()), m_robotClimber).withName("Climber controlWithInput defaultCommand")); //}
// }, m_robotClimber));
// m_robotTurret.setDefaultCommand(
// new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry));
@@ -336,7 +337,7 @@ public class RobotContainer {
.whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender))
.whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber));
// new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value)
// .whenPressed(new InstantCommand(() -> m_robotTurret.setDefaultCommand(null)))
// .whenPressed(new InstantCommand(() -> m_robotHood.setDefaultCommand(null)))
@@ -349,8 +350,8 @@ public class RobotContainer {
// .whenReleased(new InstantCommand(() -> m_robotHood.setDefaultCommand(
// new RunCommand(() -> m_robotHood.runHood(getOperatorController().getRightY()), m_robotHood))));
// .whenPressed(new InstantCommand(() -> this.currentMode = CurrentMode.CLIMBER))
// .whenReleased(new InstantCommand(() -> this.currentMode = CurrentMode.TURRET));
.whenPressed(new InstantCommand(() -> this.currentMode = Mode.CLIMBER))
.whenReleased(new InstantCommand(() -> this.currentMode = Mode.SHOOTER));
// .whenReleased(EnableClimber()));
// control turret manual mode
src/main/java/frc4388/robot/subsystems/Climber.java
+29 -16
View File
@@ -4,6 +4,7 @@
package frc4388.robot.subsystems;
import java.util.Arrays;
import java.util.HashMap;
import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
@@ -35,6 +36,7 @@ public class Climber extends SubsystemBase {
private WPI_Pigeon2 m_gyro;
private Point tPoint;
private double[] tJointAngles;
private boolean groundRelative;
private double shoulderSpeedLimiter;
@@ -50,7 +52,9 @@ public class Climber extends SubsystemBase {
m_elbow.configFactoryDefault();
m_elbow.setNeutralMode(NeutralMode.Brake);
// setClimberGains();
tJointAngles = new double[] {m_shoulder.getSelectedSensorPosition(), m_elbow.getSelectedSensorPosition()};
setClimberGains();
// shoulderStartPosition = m_shoulder.getSelectedSensorPosition();
// elbowStartPosition = m_elbow.getSelectedSensorPosition();
@@ -144,44 +148,53 @@ public class Climber extends SubsystemBase {
}
public void setJointAngles(double[] angles) {
System.out.println(angles);
System.out.println(Arrays.toString(angles));
setJointAngles(angles[0], angles[1]);
}
public void setJointAngles(double shoulderAngle, double elbowAngle) {
shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
// shoulderAngle = shoulderAngle < ClimberConstants.SHOULDER_RESTING_ANGLE ? ClimberConstants.SHOULDER_RESTING_ANGLE : shoulderAngle;
// elbowAngle = elbowAngle < ClimberConstants.ELBOW_RESTING_ANGLE ? ClimberConstants.ELBOW_RESTING_ANGLE : elbowAngle;
shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
// shoulderAngle = shoulderAngle > ClimberConstants.SHOULDER_MAX_ANGLE ? ClimberConstants.SHOULDER_MAX_ANGLE : shoulderAngle;
// elbowAngle = elbowAngle > ClimberConstants.ELBOW_MAX_ANGLE ? ClimberConstants.ELBOW_MAX_ANGLE : elbowAngle;
// Convert radians to ticks
System.out.println("angles: " + shoulderAngle + ", " + elbowAngle);
// // Convert radians to ticks
SmartDashboard.putString("angles", shoulderAngle + ", " + elbowAngle);
double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
// double shoulderPosition = (shoulderAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
// double elbowPosition = (elbowAngle * (Constants.TICKS_PER_ROTATION_FX/2.d)) / Math.PI;
shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO;
elbowPosition *= ClimberConstants.ELBOW_GB_RATIO;
// shoulderPosition *= ClimberConstants.SHOULDER_GB_RATIO;
// elbowPosition *= ClimberConstants.ELBOW_GB_RATIO;
// shoulderPosition += m_shoulderOffset;
// elbowPosition += m_elbowOffset;
m_shoulder.set(TalonFXControlMode.Position, shoulderPosition);
m_elbow.set(TalonFXControlMode.Position, elbowPosition);
m_shoulder.set(TalonFXControlMode.Position, shoulderAngle);
m_elbow.set(TalonFXControlMode.Position, elbowAngle);
}
public void controlWithInput(double xInput, double yInput) {
public void controlPointWithInput(double xInput, double yInput) {
tPoint.x += xInput * ClimberConstants.MOVE_SPEED * .02;
tPoint.y += yInput * ClimberConstants.MOVE_SPEED * .02;
}
public void controlJointsWithInput(double shoulderInput, double elbowInput) {
tJointAngles[0] += shoulderInput * ClimberConstants.MOVE_SPEED * .02;
tJointAngles[1] += elbowInput * ClimberConstants.MOVE_SPEED * .02;
}
int pCount = 0;
@Override
public void periodic() {
SmartDashboard.putNumber("Elbow", m_elbow.getSelectedSensorPosition());
SmartDashboard.putNumber("Shoulder", m_shoulder.getSelectedSensorPosition());
// double[] jointAngles = getTargetJointAngles(tPoint, 0.d);
// setJointAngles(jointAngles);
if(pCount % 1 == 0)
setJointAngles(tJointAngles);
pCount ++;
// * speed limiting near ELBOW soft limits. tolerance (distance when ramping starts) is 20000 rotations. speed at hard limits is 0.2 (percent output).
double currentElbowPos = this.m_elbow.getSelectedSensorPosition();