Skip to content

Commit

Permalink
manual elevator
Browse files Browse the repository at this point in the history
  • Loading branch information
ethan jones committed Feb 1, 2025
1 parent 096158f commit 10d0b5d
Show file tree
Hide file tree
Showing 5 changed files with 39 additions and 169 deletions.
5 changes: 5 additions & 0 deletions simgui-ds.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,5 +88,10 @@
"buttonCount": 0,
"povCount": 0
}
],
"robotJoysticks": [
{
"guid": "Keyboard1"
}
]
}
171 changes: 8 additions & 163 deletions simgui.json
View file
Original file line number Diff line number Diff line change
@@ -1,29 +1,9 @@
{
"HALProvider": {
"Addressable LEDs": {
"0": {
"columns": 64
},
"window": {
"visible": true
}
},
"Other Devices": {
"window": {
"visible": false
}
}
},
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/Faults/Active Faults": "Alerts",
"/Faults/Total Faults": "Alerts",
<<<<<<< HEAD
"/Robot/arm/armCanvas": "Mechanism2d",
"/Robot/arm/fb": "ProfiledPIDController",
=======
>>>>>>> main
"/Robot/autos": "String Chooser",
"/Robot/drive/field2d": "Field2d",
"/Robot/drive/rotationController": "PIDController",
Expand All @@ -34,52 +14,24 @@
"/SmartDashboard/Alerts": "Alerts",
"/SmartDashboard/Scheduler": "Scheduler",
"/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d",
<<<<<<< HEAD
=======
"/SmartDashboard/drive dynamic backward": "Command",
"/SmartDashboard/drive dynamic forward": "Command",
"/SmartDashboard/drive quasistatic backward": "Command",
"/SmartDashboard/drive quasistatic forward": "Command",
"/SmartDashboard/pivot dynamic backward": "Command",
"/SmartDashboard/pivot dynamic forward": "Command",
"/SmartDashboard/pivot quasistatic backward": "Command",
"/SmartDashboard/pivot quasistatic forward": "Command",
>>>>>>> main
"/SmartDashboard/elevator dynamic backward": "Command",
"/SmartDashboard/elevator dynamic forward": "Command",
"/SmartDashboard/elevator quasistatic backward": "Command",
"/SmartDashboard/elevator quasistatic forward": "Command",
"/SmartDashboard/rotation dynamic backward": "Command",
"/SmartDashboard/rotation dynamic forward": "Command",
"/SmartDashboard/rotation quasistatic backward": "Command",
"/SmartDashboard/rotation quasistatic forward": "Command",
"/SmartDashboard/translation dynamic backward": "Command",
"/SmartDashboard/translation dynamic forward": "Command",
"/SmartDashboard/translation quasistatic backward": "Command",
"/SmartDashboard/translation quasistatic forward": "Command",
"/SmartDashboard/turn dynamic backward": "Command",
"/SmartDashboard/turn dynamic forward": "Command",
"/SmartDashboard/turn quasistatic backward": "Command",
"/SmartDashboard/turn quasistatic forward": "Command"
"/SmartDashboard/translation quasistatic forward": "Command"
},
"windows": {
"/Robot/autos": {
"window": {
"visible": true
}
},
"/Robot/drive/field2d": {
"bottom": 1476,
"height": 8.210550308227539,
"left": 150,
"module- FL": {
"style": "Line"
},
"module- RR": {
"style": "Line"
},
"module-FR": {
"style": "Line"
},
"module-RL": {
"style": "Line"
},
"right": 2961,
"top": 79,
"width": 16.541748046875,
Expand All @@ -100,91 +52,11 @@
}
},
"NetworkTables": {
"Retained Values": {
"open": false
},
"transitory": {
"Robot": {
"drive": {
"Pose2d##v_/Robot/drive/getPose": {
"Rotation2d##v_rotation": {
"open": true
},
"open": true
},
"Pose2d##v_/Robot/drive/pose": {
"Rotation2d##v_rotation": {
"open": true
},
"Translation2d##v_translation": {
"open": true
},
"open": true
},
"SwerveModulePosition[]##v_/Robot/drive/getModulePositions": {
"SwerveModulePosition##v_[1]": {
"Rotation2d##v_angle": {
"open": true
}
}
},
"SwerveModuleState[]##v_/Robot/drive/getModuleSetpoints": {
"SwerveModuleState##v_[0]": {
"Rotation2d##v_angle": {
"open": true
}
},
"SwerveModuleState##v_[1]": {
"open": true
},
"SwerveModuleState##v_[2]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"SwerveModuleState##v_[3]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"SwerveModuleState[]##v_/Robot/drive/getModuleStates": {
"SwerveModuleState##v_[0]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"frontRight": {
"SwerveModulePosition##v_/Robot/drive/frontRight/position": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"SwerveModuleState##v_/Robot/drive/frontRight/state": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"elevator": {
"open": true
},
"vision": {
"open": true
}
},
"Shuffleboard": {
"open": true
},
"SmartDashboard": {
"open": true
}
}
Expand All @@ -202,36 +74,9 @@
0.0,
0.8500000238418579
],
<<<<<<< HEAD
"height": 0,
=======
"height": 405,
>>>>>>> main
"series": [
{
"color": [
0.2980392277240753,
0.44705885648727417,
0.6901960968971252,
1.0
],
"id": "NT:/Robot/elevator/positionSetpoint"
},
{
"color": [
0.8666667342185974,
0.5176470875740051,
0.32156863808631897,
1.0
],
"id": "NT:/Robot/elevator/position"
}
]
"height": 219
}
],
"window": {
"name": "Elevator"
}
]
}
}
}
9 changes: 5 additions & 4 deletions src/main/java/org/sciborgs1155/robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -103,10 +103,11 @@ private void configureGameBehavior() {

/** Configures trigger -> command bindings. */
private void configureBindings() {
operator.a().onTrue(elevator.scoreLevel(Level.L1));
operator.b().onTrue(elevator.scoreLevel(Level.L2));
operator.x().onTrue(elevator.scoreLevel(Level.L3));
operator.y().onTrue(elevator.scoreLevel(Level.L4));
operator.a().whileTrue(elevator.manualElevator(operator::getLeftY));
// operator.a().onTrue(elevator.scoreLevel(Level.L1));
// operator.b().onTrue(elevator.scoreLevel(Level.L2));
// operator.x().onTrue(elevator.scoreLevel(Level.L3));
// operator.y().onTrue(elevator.scoreLevel(Level.L4));

// x and y are switched: we use joystick Y axis to control field x motion
InputStream x = InputStream.of(driver::getLeftY).negate();
Expand Down
22 changes: 21 additions & 1 deletion src/main/java/org/sciborgs1155/robot/elevator/Elevator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
import static edu.wpi.first.units.Units.Seconds;
import static edu.wpi.first.units.Units.Volts;
import static org.sciborgs1155.lib.Assertion.eAssert;
import static org.sciborgs1155.robot.elevator.ElevatorConstants.*;
Expand All @@ -19,11 +20,15 @@
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
import java.util.Optional;
import java.util.Set;
import java.util.function.DoubleSupplier;
import monologue.Annotations.Log;
import monologue.Logged;
import org.sciborgs1155.lib.Assertion;
import org.sciborgs1155.lib.InputStream;
import org.sciborgs1155.lib.Test;
import org.sciborgs1155.robot.Constants;
import org.sciborgs1155.robot.Constants.Field.Level;
import org.sciborgs1155.robot.Robot;

Expand Down Expand Up @@ -102,14 +107,27 @@ public Command scoreLevel(Level level) {
return goTo(level.height.in(Meters));
}

public Command manualElevator(InputStream input) {
return goTo(input
.scale(MAX_VELOCITY.in(MetersPerSecond))
.scale(Constants.PERIOD.in(Seconds))
.rateLimit(MAX_ACCEL.in(MetersPerSecondPerSecond))
.add(this::position))
.withName("manual elevator");
}

/**
* Drives elevator to the desired height, within its physical boundaries.
*
* @param height Desired height in meters.
* @return A command which drives the elevator to the desired height.
*/
public Command goTo(DoubleSupplier height) {
return run(() -> update(height.getAsDouble()));
}

public Command goTo(double height) {
return run(() -> update(height));
return goTo(() -> height);
}

/**
Expand Down Expand Up @@ -171,6 +189,8 @@ private void update(double position) {
public void periodic() {
setpoint.setLength(positionSetpoint());
measurement.setLength(position());

log("command", Optional.ofNullable(getCurrentCommand()).map(Command::getName).orElse("none"));
}

/**
Expand Down
1 change: 0 additions & 1 deletion src/main/java/org/sciborgs1155/robot/elevator/ElevatorConstants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import static edu.wpi.first.units.Units.Amps;
import static edu.wpi.first.units.Units.Inches;
import static edu.wpi.first.units.Units.Kilograms;
import static edu.wpi.first.units.Units.Meters;
import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
Expand Down

0 comments on commit 10d0b5d

Please sign in to comment.