Skip to content

Commit

Permalink
Merge pull request #14 from RAR1741/arm
Browse files Browse the repository at this point in the history 
Arm (many changes, looks good)
  • Loading branch information
@GingerJakeDaBoi
GingerJakeDaBoi authored Feb 12, 2025
2 parents 3e21060 + fb56c2e commit 503b250
Show file tree
Hide file tree
Showing 13 changed files with 597 additions and 340 deletions.
2 changes: 2 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -189,5 +189,7 @@ compile_commands.json
# Generated file for build constants
src/main/java/frc/robot/BuildConstants.java

gradle.properties

# Generated file for robot sim
simgui-ds.json
106 changes: 97 additions & 9 deletions AdvantageScope.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,15 +26,17 @@
"/RealOutputs/ASPoseHelper/VirtualRobotPose",
"/RealOutputs/ASPoseHelper/LimelightCheckPose/translation",
"/RealOutputs/ASPoseHelper/LimelightCheckPose/rotation",
"/RealOutputs/Elevator",
"/RealOutputs/Elevator/Current",
"/RealOutputs/Elevator/Position",
"/RealOutputs/Elevator/Output",
"/RealOutputs/Elevator/Voltage"
"/RealOutputs/Elevator/Voltage",
"/RealOutputs/Arm",
"/RealOutputs/Arm/Position",
"/RealOutputs/Arm/RelEncoder"
]
},
"tabs": {
"selected": 7,
"selected": 8,
"tabs": [
{
"type": 0,
Expand Down Expand Up @@ -147,17 +149,17 @@
"cameraIndex": -1,
"orbitFov": 50,
"cameraPosition": [
8.526746504512941,
11.284618901802535,
-9.024754907765363
11.393981096545613,
9.253761994925226,
-9.130869185350189
],
"cameraTarget": [
1.9068259599916666,
-1.6065548077719147,
0.5962583657360303
]
},
"controlsHeight": 168
"controlsHeight": 422
},
{
"type": 2,
Expand Down Expand Up @@ -455,7 +457,7 @@
"type": "stepped",
"logKey": "/RealOutputs/Elevator/Voltage/Left",
"logType": "Number",
"visible": true,
"visible": false,
"options": {
"color": "#858584",
"size": "normal"
Expand Down Expand Up @@ -530,7 +532,93 @@
},
"controllerUUID": "bk1ydc86ort97d4pgo1lia9s8id3yo2o",
"renderer": null,
"controlsHeight": 249
"controlsHeight": 412
},
{
"type": 1,
"title": "Arm",
"controller": {
"leftSources": [
{
"type": "stepped",
"logKey": "/RealOutputs/Arm/Voltage",
"logType": "Number",
"visible": false,
"options": {
"color": "#2b66a2",
"size": "normal"
}
},
{
"type": "stepped",
"logKey": "/RealOutputs/Arm/CurrentAmps",
"logType": "Number",
"visible": true,
"options": {
"color": "#e48b32",
"size": "normal"
}
},
{
"type": "stepped",
"logKey": "/RealOutputs/Arm/RelEncoder/Position",
"logType": "Number",
"visible": false,
"options": {
"color": "#c0b487",
"size": "normal"
}
}
],
"rightSources": [
{
"type": "stepped",
"logKey": "/RealOutputs/Arm/Position/Current",
"logType": "Number",
"visible": true,
"options": {
"color": "#e5b31b",
"size": "normal"
}
},
{
"type": "stepped",
"logKey": "/RealOutputs/Arm/Position/Setpoint",
"logType": "Number",
"visible": true,
"options": {
"color": "#af2437",
"size": "normal"
}
},
{
"type": "stepped",
"logKey": "/RealOutputs/Arm/Position/Target",
"logType": "Number",
"visible": true,
"options": {
"color": "#80588e",
"size": "normal"
}
}
],
"discreteSources": [],
"leftLockedRange": null,
"rightLockedRange": null,
"leftUnitConversion": {
"type": null,
"factor": 1
},
"rightUnitConversion": {
"type": null,
"factor": 1
},
"leftFilter": 0,
"rightFilter": 0
},
"controllerUUID": "jny8io3mszrjqgdabbrd9rmkhqefftnt",
"renderer": null,
"controlsHeight": 240
}
]
}
Expand Down
92 changes: 92 additions & 0 deletions simgui-ds.json
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,92 @@
{
"keyboardJoysticks": [
{
"axisConfig": [
{
"decKey": 65,
"incKey": 68
},
{
"decKey": 87,
"incKey": 83
},
{
"decKey": 69,
"decayRate": 0.0,
"incKey": 82,
"keyRate": 0.009999999776482582
}
],
"axisCount": 3,
"buttonCount": 4,
"buttonKeys": [
90,
88,
67,
86
],
"povConfig": [
{
"key0": 328,
"key135": 323,
"key180": 322,
"key225": 321,
"key270": 324,
"key315": 327,
"key45": 329,
"key90": 326
}
],
"povCount": 1
},
{
"axisConfig": [
{
"decKey": 74,
"incKey": 76
},
{
"decKey": 73,
"incKey": 75
}
],
"axisCount": 2,
"buttonCount": 4,
"buttonKeys": [
77,
44,
46,
47
],
"povCount": 0
},
{
"axisConfig": [
{
"decKey": 263,
"incKey": 262
},
{
"decKey": 265,
"incKey": 264
}
],
"axisCount": 2,
"buttonCount": 6,
"buttonKeys": [
260,
268,
266,
261,
269,
267
],
"povCount": 0
},
{
"axisCount": 0,
"buttonCount": 0,
"povCount": 0
}
]
}
64 changes: 38 additions & 26 deletions src/main/java/frc/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,8 +19,12 @@
import edu.wpi.first.wpilibj.DriverStation;
import frc.robot.controls.controllers.OperatorController;
import frc.robot.controls.controllers.VirtualRobotController;
import frc.robot.subsystems.Arm;
import frc.robot.subsystems.Arm.ArmState;
import frc.robot.subsystems.Elevator;
import frc.robot.subsystems.Elevator.ElevatorState;
import frc.robot.subsystems.EndEffector;
import frc.robot.subsystems.EndEffector.EndEffectorState;
import frc.robot.subsystems.PoseAligner;
import frc.robot.subsystems.SignalManager;
import frc.robot.subsystems.Subsystem;
Expand All @@ -40,14 +44,14 @@ public class Robot extends LoggedRobot {

private final SwerveDrive m_swerve;
private final Elevator m_elevator;
// private final EndEffector m_endAffector;
private final Arm m_arm;
private final EndEffector m_endEffector;
private final RAROdometry m_odometry;
private final PoseAligner m_poseAligner;
private final SignalManager m_signalManager = SignalManager.getInstance();

private final DriverController m_driverController;
private final OperatorController m_operatorController;

private final VirtualRobotController m_virtualRobotController;
private final GenericHID m_sysIdController;

Expand All @@ -63,24 +67,22 @@ public Robot() {
m_subsystems = new ArrayList<>();
m_swerve = SwerveDrive.getInstance();
m_odometry = RAROdometry.getInstance();
m_arm = Arm.getInstance();
m_elevator = Elevator.getInstance();
// m_endAffector = EndEffector.getInstance();
m_endEffector = EndEffector.getInstance();
m_poseAligner = PoseAligner.getInstance();

m_driverController = new DriverController(0, true, true, 0.5);
m_operatorController = new OperatorController(1, true, true, 0.5);
m_virtualRobotController = new VirtualRobotController(2);
m_sysIdController = new GenericHID(3);

// SCARY
DriverStation.silenceJoystickConnectionWarning(true);

m_subsystems.add(m_poseAligner);
m_subsystems.add(m_swerve);
m_subsystems.add(m_odometry);
m_subsystems.add(m_arm);
m_subsystems.add(m_elevator);

// m_subsystems.add(m_endAffector);
m_subsystems.add(m_endEffector);

new RobotTelemetry();

Expand Down Expand Up @@ -138,9 +140,8 @@ public void teleopPeriodic() {
ySpeed *= slowScaler * boostScaler;
rot *= slowScaler * boostScaler;

// Pose2d targetPose =
// m_poseAligner.getAndCalculateTargetPose(m_virtualRobotController.getCurrentPose());
// ASPoseHelper.addPose("VirtualRobot/target", targetPose);
Pose2d targetPose = m_poseAligner.getAndCalculateTargetPose(m_virtualRobotController.getCurrentPose());
ASPoseHelper.addPose("VirtualRobot/target", targetPose);

Pose2d currentPose = m_odometry.getPose();
Pose2d desiredPose = m_poseAligner.getAndCalculateTargetPose(currentPose);
Expand All @@ -157,28 +158,36 @@ public void teleopPeriodic() {
}

if (m_operatorController.getWantsGoToStow()) {
m_elevator.goToElevatorPosition(ElevatorState.STOW);
m_elevator.setState(ElevatorState.STOW);
} else if (m_operatorController.getWantsGoToL1()) {
m_elevator.goToElevatorPosition(ElevatorState.L1);
m_elevator.setState(ElevatorState.L1);
} else if (m_operatorController.getWantsGoToL2()) {
m_elevator.goToElevatorPosition(ElevatorState.L2);
m_elevator.setState(ElevatorState.L2);
} else if (m_operatorController.getWantsGoToL3()) {
m_elevator.goToElevatorPosition(ElevatorState.L3);
m_elevator.setState(ElevatorState.L3);
} else if (m_operatorController.getWantsGoToL4()) {
m_elevator.goToElevatorPosition(ElevatorState.L4);
m_elevator.setState(ElevatorState.L4);
} else if (m_operatorController.getWantsResetElevator()) {
m_elevator.reset();
}

// if (m_operatorController.getWantsScore() > 0) {
// m_endAffector.setState(EndEffectorState.SCORE_BRANCHES);
// } else if (m_operatorController.getWantsScore() < 0) {
// m_endAffector.setState(EndEffectorState.SCORE_TROUGH);
// } else {
// m_endAffector.setState(EndEffectorState.OFF);
// if (m_driverController.getWantsAutoPositionPressed()) {
// m_swerve.resetDriveController();
// }
if (m_operatorController.getWantsScore() > 0) {
m_endEffector.setState(EndEffectorState.SCORE_BRANCHES);
} else if (m_operatorController.getWantsScore() < 0) {
m_endEffector.setState(EndEffectorState.SCORE_TROUGH);
} else {
m_endEffector.setState(EndEffectorState.OFF);
}

if (m_driverController.getWantsAutoPositionPressed()) {
m_swerve.resetDriveController();
}

if (m_operatorController.getWantsArmScore()) {
m_arm.setArmState(ArmState.EXTEND);
} else if (m_operatorController.getWantsArmStow()) {
m_arm.setArmState(ArmState.STOW);
}
}

@Override
Expand All @@ -189,9 +198,12 @@ public void disabledInit() {
public void disabledPeriodic() {
m_odometry.setAllianceGyroAngleAdjustment();

if(m_operatorController.getWantsResetElevator()) {
if (m_operatorController.getWantsResetElevator()) {
m_elevator.reset();
}

// SCARY
DriverStation.silenceJoystickConnectionWarning(DriverStation.getMatchType() == DriverStation.MatchType.None);
}

@Override
Expand Down
Loading

0 comments on commit 503b250

Please sign in to comment.