Skip to content

Commit

Permalink
autos?
Browse files Browse the repository at this point in the history
  • Loading branch information
dannynotsmart committed Jan 22, 2024
2 parents aec536b + d5b2d98 commit 6c6dbeb
Show file tree
Hide file tree
Showing 16 changed files with 478 additions and 142 deletions.
9 changes: 9 additions & 0 deletions .DataLogTool/datalogtool.json
Original file line number Diff line number Diff line change
@@ -0,0 +1,9 @@
{
"download": {
"localDir": "C:\\Users\\Ivan\\Code\\2023-2024 Repos\\Hydrogen\\logs",
"serverTeam": "1155"
},
"output": {
"outputFolder": "C:\\Users\\Ivan\\Code\\2023-2024 Repos\\Hydrogen\\logs"
}
}
13 changes: 10 additions & 3 deletions .Glass/glass.json
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,6 @@
"open": true
}
},
"open": true,
"rearRight": {
"SwerveModulePosition##v_/Robot/drive/rearRight/position": {
"Rotation2d##v_angle": {
Expand Down Expand Up @@ -46,13 +45,21 @@
"/Robot/drive/rearLeft/turnFeedback": "PIDController",
"/Robot/drive/rearRight/driveFeedback": "PIDController",
"/Robot/drive/rearRight/turnFeedback": "PIDController",
"/SmartDashboard/drive quasistatic forward": "Command"
"/SmartDashboard/drive dynamic backward": "Command",
"/SmartDashboard/drive dynamic forward": "Command",
"/SmartDashboard/drive quasistatic backward": "Command",
"/SmartDashboard/drive quasistatic forward": "Command",
"/SmartDashboard/turn dynamic backward": "Command",
"/SmartDashboard/turn dynamic forward": "Command",
"/SmartDashboard/turn quasistatic backward": "Command",
"/SmartDashboard/turn quasistatic forward": "Command"
}
},
"NetworkTables Info": {
"visible": true
},
"NetworkTables Settings": {
"mode": "Client (NT4)"
"mode": "Client (NT3)",
"serverTeam": "1155"
}
}
41 changes: 37 additions & 4 deletions simgui.json
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,34 @@
"/Robot/drive/rearLeft/turnFeedback": "PIDController",
"/Robot/drive/rearRight/driveFeedback": "PIDController",
"/Robot/drive/rearRight/turnFeedback": "PIDController",
"/SmartDashboard/drive quasistatic forward": "Command"
"/SmartDashboard/drive dynamic backward": "Command",
"/SmartDashboard/drive dynamic forward": "Command",
"/SmartDashboard/drive quasistatic backward": "Command",
"/SmartDashboard/drive quasistatic forward": "Command",
"/SmartDashboard/turn dynamic backward": "Command",
"/SmartDashboard/turn dynamic forward": "Command",
"/SmartDashboard/turn quasistatic backward": "Command",
"/SmartDashboard/turn quasistatic forward": "Command"
},
"windows": {
"/Robot/drive/field2d": {
"module- FL": {
"style": "Line"
},
"module- RR": {
"style": "Line"
},
"module-FR": {
"style": "Line"
},
"module-RL": {
"style": "Line"
},
"window": {
"visible": true
}
},
"/SmartDashboard/drive dynamic forward": {
"window": {
"visible": true
}
Expand All @@ -38,6 +62,12 @@
"transitory": {
"Robot": {
"drive": {
"Pose2d##v_/Robot/drive/getPose": {
"Rotation2d##v_rotation": {
"open": true
},
"open": true
},
"frontRight": {
"SwerveModulePosition##v_/Robot/drive/frontRight/position": {
"Rotation2d##v_angle": {
Expand All @@ -51,15 +81,18 @@
},
"open": true
},
"driveFeedback": {
"open": true
},
"open": true
},
"open": true
},
"open": true
},
"Shuffleboard": {
"open": true
}
}
},
"NetworkTables Info": {
"visible": true
}
}
125 changes: 125 additions & 0 deletions src/main/java/org/sciborgs1155/lib/InputStream.java
Original file line number Diff line number Diff line change
@@ -0,0 +1,125 @@
package org.sciborgs1155.lib;

import edu.wpi.first.math.MathUtil;
import edu.wpi.first.math.filter.SlewRateLimiter;
import java.util.function.DoubleSupplier;
import java.util.function.DoubleUnaryOperator;

/**
* A functional interface to aid in modifying double suppliers, such as from a joystick.
*
* <p>Inspired by 694's StuyLib.
*/
@FunctionalInterface
public interface InputStream extends DoubleSupplier {

/**
* Creates an input stream from another.
*
* @param base The base stream.
* @return A new input stream.
*/
public static InputStream of(DoubleSupplier base) {
return base::getAsDouble;
}

/**
* Shorthand to return a double value.
*
* @return The value from {@link #getAsDouble()}.
*/
public default double get() {
return getAsDouble();
}

/**
* Maps the stream outputs by an operator.
*
* @param operator A function that takes in a double input and returns a double output.
* @return A mapped stream.
*/
public default InputStream map(DoubleUnaryOperator operator) {
return () -> operator.applyAsDouble(getAsDouble());
}

/**
* Scales the stream outputs by a factor.
*
* @param factor A supplier of scaling factors.
* @return A scaled stream.
*/
public default InputStream scale(DoubleSupplier factor) {
return map(x -> x * factor.getAsDouble());
}

/**
* Scales the stream outputs by a factor.
*
* @param factor A scaling factor.
* @return A scaled stream.
*/
public default InputStream scale(double factor) {
return scale(() -> factor);
}

/**
* Negates the stream outputs.
*
* @return A stream scaled by -1.
*/
public default InputStream negate() {
return scale(-1);
}

/**
* Raises the stream outputs to an exponent.
*
* @param exponent The exponent to raise them to.
* @return An exponentiated stream.
*/
public default InputStream pow(double exponent) {
return map(x -> Math.pow(x, exponent));
}

/**
* Raises the stream outputs to an exponent and keeps their original sign.
*
* @param exponent The exponent to raise them to.
* @return An exponentiated stream.
*/
public default InputStream signedPow(double exponent) {
return map(x -> Math.copySign(Math.pow(x, exponent), x));
}

/**
* Deadbands the stream outputs by a minimum bound and scales them from 0 to a maximum bound.
*
* @param bound The lower bound to deadband with.
* @param max The maximum value to scale with.
* @return A deadbanded stream.
*/
public default InputStream deadband(double deadband, double max) {
return map(x -> MathUtil.applyDeadband(x, deadband, Double.MAX_VALUE));
}

/**
* Clamps the stream outputs by a maximum bound.
*
* @param magnitude The upper bound to clamp with.
* @return A clamped stream.
*/
public default InputStream clamp(double magnitude) {
return map(x -> MathUtil.clamp(x, -magnitude, magnitude));
}

/**
* Rate limits the stream outputs by a specified rate.
*
* @param rate The rate in units / s.
* @return A rate limited stream.
*/
public default InputStream rateLimit(double rate) {
var limiter = new SlewRateLimiter(rate);
return map(x -> limiter.calculate(x));
}
}
6 changes: 6 additions & 0 deletions src/main/java/org/sciborgs1155/robot/Constants.java
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,7 @@

import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Time;
import org.sciborgs1155.robot.drive.DriveConstants;

/**
* Constants is a globally accessible class for storing immutable values. Every value should be
Expand All @@ -17,4 +18,9 @@
public class Constants {
public static final Measure<Time> PERIOD = Seconds.of(0.02); // roborio tickrate (s)
public static final double DEADBAND = 0.1;
public static final double MAX_RATE =
DriveConstants.MAX_ACCEL.baseUnitMagnitude()
/ DriveConstants.MAX_ANGULAR_SPEED.baseUnitMagnitude();
public static final double SLOW_SPEED = 0.33;
public static final double FULL_SPEED = 1.0;
}
39 changes: 38 additions & 1 deletion src/main/java/org/sciborgs1155/robot/Robot.java
Original file line number Diff line number Diff line change
@@ -1,5 +1,9 @@
package org.sciborgs1155.robot;

import static edu.wpi.first.units.Units.MetersPerSecond;
import static edu.wpi.first.units.Units.MetersPerSecondPerSecond;
import static edu.wpi.first.units.Units.RadiansPerSecond;
import static edu.wpi.first.units.Units.Second;
import static edu.wpi.first.wpilibj2.command.button.RobotModeTriggers.*;

import com.pathplanner.lib.auto.AutoBuilder;
Expand All @@ -16,8 +20,10 @@
import org.littletonrobotics.urcl.URCL;
import org.sciborgs1155.lib.CommandRobot;
import org.sciborgs1155.lib.FaultLogger;
import org.sciborgs1155.lib.InputStream;
import org.sciborgs1155.robot.Ports.OI;
import org.sciborgs1155.robot.drive.Drive;
import org.sciborgs1155.robot.drive.DriveConstants;

/**
* This class is where the bulk of the robot should be declared. Since Command-based is a
Expand All @@ -37,6 +43,8 @@ public class Robot extends CommandRobot implements Logged {
// COMMANDS
@Log.NT private final SendableChooser<Command> autos = AutoBuilder.buildAutoChooser();

@Log.NT private double speedMultiplier = Constants.FULL_SPEED;

/** The robot contains subsystems, OI devices, and commands. */
public Robot() {
registerCommands();
Expand All @@ -61,13 +69,36 @@ private void configureGameBehavior() {
}
}

/** Creates an input stream for a joystick. */
private InputStream createJoystickStream(InputStream input, double maxSpeed, double maxRate) {
return input
.deadband(Constants.DEADBAND, 1)
.negate()
.scale(maxSpeed)
.scale(() -> speedMultiplier)
.signedPow(2)
.rateLimit(maxRate);
}

/**
* Configures subsystem default commands. Default commands are scheduled when no other command is
* running on a subsystem.
*/
private void configureSubsystemDefaults() {
drive.setDefaultCommand(
drive.drive(() -> -driver.getLeftX(), () -> -driver.getLeftY(), () -> -driver.getRightX()));
drive.drive(
createJoystickStream(
driver::getLeftX,
DriveConstants.MAX_SPEED.in(MetersPerSecond),
DriveConstants.MAX_ACCEL.in(MetersPerSecondPerSecond)),
createJoystickStream(
driver::getLeftY,
DriveConstants.MAX_SPEED.in(MetersPerSecond),
DriveConstants.MAX_ACCEL.in(MetersPerSecondPerSecond)),
createJoystickStream(
driver::getRightX,
DriveConstants.MAX_ANGULAR_SPEED.in(RadiansPerSecond),
DriveConstants.MAX_ANGULAR_ACCEL.in(RadiansPerSecond.per(Second)))));
}

/** Registers all named commands, which will be used by pathplanner */
Expand All @@ -79,5 +110,11 @@ private void registerCommands() {
private void configureBindings() {
autonomous().whileTrue(new ProxyCommand(autos::getSelected));
FaultLogger.onFailing(f -> Commands.print(f.toString()));

driver
.leftBumper()
.or(driver.rightBumper())
.onTrue(Commands.runOnce(() -> speedMultiplier = Constants.FULL_SPEED))
.onFalse(Commands.run(() -> speedMultiplier = Constants.SLOW_SPEED));
}
}
Loading

0 comments on commit 6c6dbeb

Please sign in to comment.