Skip to content

Commit 42c1fbe

Browse files
claynicholsonclaynicholson
committed
Making robot work yk
1 parent 58fa52e commit 42c1fbe

File tree

13 files changed

+94
-51
lines changed

13 files changed

+94
-51
lines changed

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

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -55,8 +55,8 @@ public class Robot extends LoggedRobot {
5555
// the main mechanism object
5656

5757
private Command autonomousCommand;
58-
private RobotContainer robotContainer;
59-
private AutoFactory autoFactory ;
58+
private RobotContainer robotContainer;
59+
private AutoFactory autoFactory;
6060
private AutoChooser autoChooser;
6161

6262
private final Optional<Trajectory<SwerveSample>> trajectory =

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

Lines changed: 22 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -18,10 +18,13 @@
1818

1919
import edu.wpi.first.wpilibj.GenericHID;
2020
import edu.wpi.first.wpilibj.XboxController;
21+
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
2122
import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
2223
import frc.robot.constants.Constants;
2324
import frc.robot.constants.EndEffectorConstants;
2425
import frc.robot.constants.SwerveConstants;
26+
import frc.robot.subsystems.climber.Climber;
27+
import frc.robot.subsystems.climber.ClimberIOReal;
2528
import frc.robot.subsystems.drive.Drive;
2629
import frc.robot.subsystems.drive.imu.GyroIO;
2730
import frc.robot.subsystems.drive.imu.GyroIOPigeon2;
@@ -46,7 +49,7 @@ public class RobotContainer {
4649
public Vision vision;
4750
public Elevator elevator;
4851
// public Intake intake;
49-
// public Climber climber;
52+
public Climber climber;
5053
public EndEffector endEffector;
5154
// public AutoFactory autoFactory;
5255

@@ -80,7 +83,7 @@ public RobotContainer() {
8083
);
8184
elevator = new Elevator(new ElevatorIOKraken());
8285
// intake = new Intake(new IntakeIOReal());
83-
// climber = new Climber(new ClimberIOReal());
86+
climber = new Climber(new ClimberIOReal());
8487
endEffector = new EndEffector(new EndEffectorIOReal());
8588
// autoFactory = new AutoFactory(drive::getPose, drive::setPose, drive::followTrajectory,
8689
// false, drive);
@@ -120,6 +123,10 @@ public RobotContainer() {
120123

121124
break;
122125
}
126+
SmartDashboard.putNumber("Elevator Voltage", 0.5);
127+
SmartDashboard.putNumber("Elevator P", 0.35);
128+
SmartDashboard.putNumber("Elevator I", 0.0);
129+
SmartDashboard.putNumber("Elevator D", 0.0);
123130

124131
configureButtonBindings();
125132
}
@@ -150,6 +157,19 @@ private void configureButtonBindings() {
150157
.onFalse(endEffector.setShooter(0).andThen(endEffector.RotateCoralPickup()));
151158

152159
controller2.rightTrigger().onTrue(elevator.goToMaxL1()).onFalse(elevator.goToGroundLevel());
160+
161+
// Climber
162+
163+
controller2.povLeft().whileTrue(climber.setSpeed(0.75)).onFalse(climber.setSpeed(0));
164+
165+
controller2.povRight().whileTrue(climber.setSpeed(-0.5)).onFalse(climber.setSpeed(0));
166+
167+
controller2.a().onTrue(elevator.goToLevelOne());
168+
controller2.x().onTrue(elevator.goToLevelTwo());
169+
controller2.b().onTrue(elevator.goToLevelThree());
170+
controller2.y().onTrue(elevator.goToLevelFour());
171+
controller2.povDown().onTrue(elevator.goToGroundLevel());
172+
153173
// Elevator
154174
// Ground Intake
155175
/* controller2

‎src/main/java/frc/robot/constants/ElevatorConstants.java‎

Lines changed: 18 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -2,12 +2,13 @@
22

33
public final class ElevatorConstants {
44

5-
public static final double elevatorSpeed = 4; //Out of 12
5+
public static final double elevatorSpeed = 4; // Out of 12
66
public static double currentLimitAmps = 40.0;
77

8-
public static final double KpElevator = 0.1;
9-
public static final double KiElevator = 0;
10-
public static final double KdElevator = 0;
8+
public static final double KpElevator = 0.40;
9+
public static final double KiElevator = 0.02;
10+
public static final double KdElevator = 0.055;
11+
public static final double voltageMultiplier = 0.5;
1112

1213
public static final double GEAR_RATIO = 48.0 / 10.0;
1314
public static final double SPOOL_DIAMETER = 3.5;
@@ -17,25 +18,22 @@ public final class ElevatorConstants {
1718

1819
public static final double levelGroundInches = 0;
1920

20-
public static final double maxL1 = 65.0;
21+
public static final double maxL1 = 65.0; // 8.3
2122

22-
public static final double levelOne = 18.0;
23-
public static final double levelTwo = 31.875;
24-
public static final double levelThree = 47.625;
25-
public static final double levelFour = 72.0;
26-
public static final double levelBarge =
27-
96.0; // Assuming this is the height needed to interact with the Barge 96
28-
public static final double levelPickup =
29-
10.0; // Assuming this is the height for picking up game pieces
23+
public static final double levelOne = 9.0;
24+
public static final double levelTwo = 13.13;
25+
public static final double levelThree = 19.40;
26+
public static final double levelFour = 29.30;
27+
public static final double levelBarge = 29.30;
28+
public static final double levelPickup = 8.3;
3029

31-
public static final double firstStageStroke = 21.0;
32-
public static final double secondStageStroke = 27.0 + firstStageStroke;
33-
public static final double thirdStageStroke = 28.0 + secondStageStroke;
34-
35-
public static final double firstStageVoltage = 0.02;
36-
public static final double secondStageVoltage = 0.04;
37-
public static final double thirdStageVoltage = 0.06;
30+
public static final double firstStageStroke = 9.138;
31+
public static final double secondStageStroke = 19.350;
32+
public static final double thirdStageStroke = 30.0; // 29.35
3833

34+
public static final double firstStageVoltage = 0.020000 + 0.03;
35+
public static final double secondStageVoltage = 0.038000 + 0.03;
36+
public static final double thirdStageVoltage = 0.10000 + 0.03;
3937
}
4038

4139
// First stage stroke = 21 inches

‎src/main/java/frc/robot/constants/EndEffectorConstants.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@ public class EndEffectorConstants {
1717
Rotation2d.fromDegrees(180 - 52.993);
1818
public static final Rotation2d PointingUpArticulation = Rotation2d.fromDegrees(180 - 100.405);*/
1919

20-
public static final double ArticulationToleranceRad = Math.PI/12;
20+
public static final double ArticulationToleranceRad = Math.PI / 12;
2121

2222
public static final double ArticulationVelocity = 0;
2323
public static final double ArticulationAcceleration = 0;

‎src/main/java/frc/robot/constants/HangConstants.java‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
package frc.robot.constants;
2+
3+
public class HangConstants {
4+
public static final double hanginSpeed = 0.5;
5+
6+
public static final double hangVoltage = 12;
7+
}

‎src/main/java/frc/robot/constants/SwerveConstants.java‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -147,7 +147,8 @@ public class SwerveConstants {
147147
private static final int kFrontRightDriveMotorId = 5;
148148
private static final int kFrontRightSteerMotorId = 4;
149149
private static final int kFrontRightEncoderId = 6;
150-
private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.12646484375);
150+
private static final Angle kFrontRightEncoderOffset =
151+
Rotations.of(-0.118408203125); // -0.12646484375
151152
private static final boolean kFrontRightSteerMotorInverted = true;
152153
private static final boolean kFrontRightEncoderInverted = false;
153154

@@ -158,7 +159,7 @@ public class SwerveConstants {
158159
private static final int kBackLeftDriveMotorId = 8;
159160
private static final int kBackLeftSteerMotorId = 7;
160161
private static final int kBackLeftEncoderId = 9;
161-
private static final Angle kBackLeftEncoderOffset = Rotations.of(0.47705078125);
162+
private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.41796875); // 0.47705078125
162163
private static final boolean kBackLeftSteerMotorInverted = true;
163164
private static final boolean kBackLeftEncoderInverted = false;
164165

‎src/main/java/frc/robot/constants/TunableConstants.java‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -7,8 +7,6 @@ public final class TunableConstants {
77
public static final double KpTheta = 5.0;
88
public static final double KdTheta = 0;
99

10-
11-
1210
public static final double KpIntakeArticulation = 3;
1311
public static final double KiIntakeArticulation = 0;
1412
public static final double KdIntakeArticulation = 0;

‎src/main/java/frc/robot/subsystems/climber/Climber.java‎

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

3+
import edu.wpi.first.wpilibj2.command.Command;
4+
import edu.wpi.first.wpilibj2.command.InstantCommand;
35
import edu.wpi.first.wpilibj2.command.SubsystemBase;
46
import org.littletonrobotics.junction.Logger;
57

68
public class Climber extends SubsystemBase {
79
private final ClimberIO io;
810
private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged();
11+
private double Speed;
912

1013
public Climber(ClimberIO io) {
1114
this.io = io;
@@ -15,10 +18,17 @@ public Climber(ClimberIO io) {
1518
public void periodic() {
1619

1720
io.updateInputs(inputs);
21+
1822
Logger.processInputs("Climber", inputs);
23+
Logger.recordOutput("Climber/Speed", Speed);
24+
}
25+
26+
public Command setSpeed(double speed) {
27+
Speed = speed;
28+
return new InstantCommand(() -> setClimb(speed));
1929
}
2030

21-
public void setVoltage(double voltage) {
22-
setVoltage(voltage);
31+
public void setClimb(double speed) {
32+
io.setClimbVoltage(12.0 * speed);
2333
}
2434
}

‎src/main/java/frc/robot/subsystems/climber/ClimberIO.java‎

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -21,7 +21,5 @@ class ClimberIOInputs {
2121

2222
default void updateInputs(ClimberIOInputs inputs) {}
2323

24-
default void leftHangRunVoltage(double voltage) {}
25-
26-
default void rightHangRunVoltage(double voltage) {}
24+
default void setClimbVoltage(double voltage) {}
2725
}

‎src/main/java/frc/robot/subsystems/climber/ClimberIOReal.java‎

Lines changed: 8 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,20 @@ public class ClimberIOReal implements ClimberIO {
1212

1313
public ClimberIOReal() {}
1414

15-
public void setVoltage(double volts) {
15+
@Override
16+
public void setClimbVoltage(double volts) {
1617
leftHangMotor.setVoltage(volts);
1718
rightHangMotor.setVoltage(-volts);
1819
}
1920

2021
public void updateInputs(ClimberIOInputs inputs) {
2122
inputs.leftCurrentAmps = leftHangMotor.getOutputCurrent();
2223
inputs.rightCurrentAmps = rightHangMotor.getOutputCurrent();
24+
25+
inputs.leftAppliedVolts = leftHangMotor.getAppliedOutput();
26+
inputs.rightAppliedVolts = rightHangMotor.getAppliedOutput();
27+
28+
inputs.encoderRight = rightHangMotor.getAppliedOutput();
29+
inputs.encoderLeft = leftHangMotor.getAppliedOutput();
2330
}
2431
}

0 commit comments

Comments
 (0)