Skip to content

Commit 091606c

Browse files
spderman3333spderman3333
committed
Elevator tuning, still needs some more.
1 parent 45abc4a commit 091606c

7 files changed

Lines changed: 36 additions & 20 deletions

File tree

‎src/main/java/frc/robot/Constants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -68,7 +68,7 @@ public enum Mode {
6868
// Mechanisms
6969

7070
// Elevator
71-
public static final int ELEVATOR_ID = 14;
71+
public static final int ELEVATOR_ID = 23;
7272

7373
// Arm
7474
public static final int ARM_PIVOT_ID = 15;

‎src/main/java/frc/robot/RobotContainer.java‎

Lines changed: 12 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
import edu.wpi.first.wpilibj.GenericHID;
1818
import edu.wpi.first.wpilibj.XboxController;
1919
import edu.wpi.first.wpilibj2.command.Command;
20-
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
20+
import edu.wpi.first.wpilibj2.command.button.CommandPS4Controller;
2121
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
2222
import frc.robot.commands.DriveCommands;
2323
import frc.robot.generated.TunerConstants;
@@ -50,7 +50,7 @@ public class RobotContainer {
5050
private final Arm arm;
5151

5252
// Controller
53-
private final CommandXboxController controller = new CommandXboxController(0);
53+
private final CommandPS4Controller controller = new CommandPS4Controller(0);
5454

5555
// Dashboard inputs
5656
private final LoggedDashboardChooser<Command> autoChooser;
@@ -127,7 +127,7 @@ private void configureButtonBindings() {
127127
// Default command, normal field-relative drive.
128128
// Xbox controller that I got is busted or something, the getRightY() binds to a trigger for some reason.
129129
drive.setDefaultCommand(DriveCommands.joystickDrive(
130-
drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRawAxis(3)));
130+
drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> controller.getRightX()));
131131

132132
// TODO: Enable later
133133
// Lock to 0° when A button is held
@@ -153,10 +153,15 @@ private void configureButtonBindings() {
153153
controller.povLeft().onTrue(elevator.setElevatorPositionCommand(Elevator.ElevatorHeight.MIDDLE));
154154
controller.povDown().onTrue(elevator.setElevatorPositionCommand(Elevator.ElevatorHeight.DOWN));
155155

156-
controller.y().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.NORTH));
157-
controller.b().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.EAST));
158-
controller.a().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.SOUTH));
159-
controller.x().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.WEST));
156+
// controller.y().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.NORTH));
157+
// controller.b().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.EAST));
158+
// controller.a().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.SOUTH));
159+
// controller.x().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.WEST));
160+
161+
controller.triangle().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.NORTH));
162+
controller.circle().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.EAST));
163+
controller.cross().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.SOUTH));
164+
controller.square().onTrue(arm.setArmPositionCommand(Arm.ArmPositions.WEST));
160165
}
161166

162167
/**

‎src/main/java/frc/robot/subsystems/arm/ArmIOReal.java‎

Lines changed: 0 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,6 @@
1212
import edu.wpi.first.units.measure.Voltage;
1313
import frc.robot.Constants;
1414

15-
import static frc.robot.subsystems.arm.ArmConstants.*;
16-
1715
// TODO: Test this class on LePrawn, ensure the arm won't rotate more than once in any direction.
1816
public class ArmIOReal implements ArmIO {
1917

@@ -97,5 +95,4 @@ public Angle getArmPosition() {
9795
public void setIntakeMotorVoltage(Voltage voltage) {
9896
intakeMotor.setVoltage(voltage.magnitude());
9997
}
100-
10198
}

‎src/main/java/frc/robot/subsystems/elevator/Elevator.java‎

Lines changed: 2 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -51,7 +51,7 @@ public Command setElevatorPositionCommand(ElevatorHeight height) {
5151
public enum ElevatorHeight {
5252
// Positions taken from offseason bot code, inturn taken from onshape.
5353
UP(Inches.of(56.0)),
54-
MIDDLE(Inches.of(28)),
54+
MIDDLE(Inches.of(28.0)),
5555
DOWN(Inches.of(0.0));
5656

5757
public final Distance position;
@@ -66,8 +66,7 @@ public Distance getPosition() {
6666

6767
public Angle getPositionAngle() {
6868
// NOTE: yes this code is bs, but i wanna see it work first.
69-
return Rotations.of(
70-
getPosition().in(Inches) / (ElevatorConstants.ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION));
69+
return Radians.of(getPosition().in(Inches) / (ElevatorConstants.ELEVATOR_HEIGHT_CHANGE_PER_MOTOR_ROTATION));
7170
}
7271
}
7372
}

‎src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -24,10 +24,10 @@ public class ElevatorConstants {
2424
public static final Distance ELEVATOR_MIN_HEIGHT = Inches.of(0.0);
2525
public static final Distance ELEVATOR_MAX_HEIGHT = Inches.of(56.0);
2626

27-
// public static final double ELEVATOR_kG = 0.29;
28-
// public static final double ELEVATOR_kS = 0.11;
29-
// public static final double ELEVATOR_kV = 0.1;
30-
// public static final double ELEVATOR_kA = 0.0;
27+
public static final double ELEVATOR_kG = 0.29;
28+
public static final double ELEVATOR_kS = 0.11;
29+
public static final double ELEVATOR_kV = 0.1;
30+
public static final double ELEVATOR_kA = 0.0;
3131
public static final double ELEVATOR_kP = 0.4;
3232
public static final double ELEVATOR_kI = 0.0;
3333
public static final double ELEVATOR_kD = 0.0;

‎src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
package frc.robot.subsystems.elevator;
22

3-
import static edu.wpi.first.units.Units.Inches;
4-
import static edu.wpi.first.units.Units.Revolution;
3+
import static edu.wpi.first.units.Units.*;
54
import static frc.robot.subsystems.elevator.ElevatorConstants.*;
65

76
import com.ctre.phoenix6.configs.MotorOutputConfigs;
@@ -14,6 +13,7 @@
1413
import edu.wpi.first.units.measure.Distance;
1514
import edu.wpi.first.units.measure.Voltage;
1615
import frc.robot.Constants;
16+
import org.littletonrobotics.junction.Logger;
1717

1818
public class ElevatorIOReal implements ElevatorIO {
1919

@@ -22,6 +22,8 @@ public class ElevatorIOReal implements ElevatorIO {
2222

2323
private final PositionVoltage positionControl = new PositionVoltage(0);
2424

25+
private Angle currentMotorSetpoint = Rotations.of(0);
26+
2527
public ElevatorIOReal() {
2628
motor = new TalonFX(Constants.ELEVATOR_ID);
2729
motor.setNeutralMode(NeutralModeValue.Brake);
@@ -33,6 +35,10 @@ public ElevatorIOReal() {
3335
slot0PIDConfig.kP = ELEVATOR_kP;
3436
slot0PIDConfig.kI = ELEVATOR_kI;
3537
slot0PIDConfig.kD = ELEVATOR_kD;
38+
slot0PIDConfig.kS = ELEVATOR_kS;
39+
slot0PIDConfig.kV = ELEVATOR_kV;
40+
slot0PIDConfig.kA = ELEVATOR_kA;
41+
slot0PIDConfig.kG = ELEVATOR_kG;
3642

3743
// Config motor inversion.
3844
motorConfig.withMotorOutput(new MotorOutputConfigs().withInverted(InvertedValue.Clockwise_Positive));
@@ -48,10 +54,13 @@ public void updateState(ElevatorIOInputs inputs) {
4854
inputs.motorVelocityRotsPerSecond = motor.getVelocity().getValueAsDouble();
4955
inputs.motorCurrent = motor.getStatorCurrent().getValueAsDouble();
5056
inputs.motorVoltage = motor.getMotorVoltage().getValueAsDouble();
57+
58+
Logger.recordOutput("Elevator/motor/Setpoint", currentMotorSetpoint);
5159
}
5260

5361
@Override
5462
public void setMotorSetpoint(Angle setpoint) {
63+
currentMotorSetpoint = setpoint;
5564
motor.setControl(positionControl.withPosition(setpoint));
5665
}
5766

‎src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@ public class ElevatorIOSim implements ElevatorIO {
3939
// Used for actually moving the motor to a given position with PID applied to a voltage input.
4040
private final PositionVoltage positionControl = new PositionVoltage(0);
4141

42+
private Angle currentMotorSetpoint = Rotations.of(0);
43+
4244
public ElevatorIOSim() {
4345
motor = new TalonFX(Constants.ELEVATOR_ID);
4446
motor.setNeutralMode(NeutralModeValue.Brake);
@@ -78,6 +80,9 @@ private void updateSim() {
7880

7981
// Logs to "Real Outputs" NT
8082
Logger.recordOutput("Simulated Elevator/motorSim/Voltage", motorSim.getMotorVoltage());
83+
Logger.recordOutput("Simulated Elevator/motor/Setpoint", currentMotorSetpoint);
84+
Logger.recordOutput("Simulated Elevator/elevatorSim/hitsUpperLimit", elevatorSim.hasHitUpperLimit());
85+
Logger.recordOutput("Simulated Elevator/elevatorSim/hitsLowerLimit", elevatorSim.hasHitLowerLimit());
8186

8287
elevatorSim.update(0.02); // Same update cycle as an actual robot, 20 ms.
8388

@@ -93,6 +98,7 @@ private void updateSim() {
9398

9499
@Override
95100
public void setMotorSetpoint(Angle setpoint) {
101+
currentMotorSetpoint = setpoint;
96102
motor.setControl(positionControl.withPosition(setpoint));
97103
}
98104

0 commit comments

Comments
 (0)