Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

Add manual CSV tuning

Browse Source 
Remove free storage and RAM commands
This commit is contained in:
nathanrsxtn
2022-03-24 19:08:24 -06:00
parent 7168e7b618
commit 042a03a58f
4 changed files with 95 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+11 -51
View File
@@ -5,30 +5,20 @@
package frc4388.robot;
import java.io.File;
import java.nio.file.Files;
import java.nio.file.Path;
import java.util.HashMap;
import java.util.Map;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.diffplug.common.base.Errors;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.drive.Vector2d;
import edu.wpi.first.wpilibj.DriverStation.Alliance;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
import edu.wpi.first.wpilibj2.command.RunCommand;
import frc4388.utility.RobotTime;
import frc4388.utility.VelocityCorrection;
import frc4388.utility.desmos.DesmosServer;
import frc4388.utility.Vector2D;
/**
* The VM is configured to automatically run this class, and to call the
@@ -83,45 +73,6 @@ public class Robot extends TimedRobot {
m_robotContainer = new RobotContainer();
// addPeriodic(m_robotContainer::recordPeriodic, kDefaultPeriod);
SmartDashboard.putData(CommandScheduler.getInstance());
SmartDashboard.putData("JVM Memory", new RunCommand(() -> {
}) {
@Override
public boolean runsWhenDisabled() {
return true;
}
@Override
public String getName() {
if (isScheduled()) {
Runtime runtime = Runtime.getRuntime();
long totalMemory = runtime.totalMemory() / 1_000_000;
long freeMemory = runtime.freeMemory() / 1_000_000;
long maxMemory = runtime.maxMemory() / 1_000_000;
return totalMemory - freeMemory + " MB / " + totalMemory + " MB / " + maxMemory + " MB";
}
return "Not Running";
}
});
SmartDashboard.putData("Usable Deploy Space", new RunCommand(() -> {
}) {
@Override
public boolean runsWhenDisabled() {
return true;
}
@Override
public String getName() {
if (isScheduled()) {
File deploy = Filesystem.getDeployDirectory();
long usedSpace = Errors.suppress().getWithDefault(
() -> Files.walk(deploy.toPath()).map(Path::toFile).filter(File::isFile).mapToLong(File::length).sum(),
0l) / 1_000_000;
long usableSpace = deploy.getUsableSpace() / 1_000_000;
return usedSpace + " MB / " + usableSpace + " MB";
}
return "Not Running";
}
});
// desmosServer.start();
m_robotContainer.m_robotVisionOdometry.setLEDs(true);
@@ -140,6 +91,15 @@ public class Robot extends TimedRobot {
@Override
public void robotPeriodic() {
m_robotTime.updateTimes();
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00);
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition);
System.out.println("First Ball (FEET): " + Vector2D.divide(firstBallPosition, 12).toString());
System.out.println("Second Ball (FEET): " + Vector2D.divide(secondBallPosition, 12).toString());
System.out.println("First To Second (FEET): " + Vector2D.divide(firstToSecond, 12).toString());
// current =
// m_robotContainer.m_robotBoomBoom.getCurrent() +
// m_robotContainer.m_robotClimber.getCurrent(); //+
src/main/java/frc4388/robot/RobotContainer.java
+61 -26
View File
@@ -4,12 +4,15 @@
package frc4388.robot;
import java.io.File;
import java.nio.file.Files;
import java.nio.file.StandardOpenOption;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Map;
import java.util.Arrays;
import java.util.Objects;
import java.util.logging.Logger;
import com.diffplug.common.base.Errors;
import com.pathplanner.lib.PathPlanner;
import com.pathplanner.lib.PathPlannerTrajectory;
import com.pathplanner.lib.PathPlannerTrajectory.PathPlannerState;
@@ -19,14 +22,19 @@ import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
import edu.wpi.first.wpilibj2.command.PrintCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -37,7 +45,6 @@ import frc4388.robot.commands.PathRecorder;
import frc4388.robot.commands.RunCommandForTime;
import frc4388.robot.commands.DriveCommands.DriveWithInputForTime;
import frc4388.robot.commands.ExtenderIntakeCommands.ExtenderIntakeGroup;
import frc4388.robot.commands.ShooterCommands.Shoot;
import frc4388.robot.commands.ShooterCommands.TrackTarget;
import frc4388.robot.subsystems.BoomBoom;
import frc4388.robot.subsystems.Claws;
@@ -129,8 +136,6 @@ public class RobotContainer {
*/
public RobotContainer() {
// double turretDistanceFromFront = 10.0; // * distance of turret from the front of the robot in inches. might need to be somewhat accurate.
// double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
@@ -269,6 +274,39 @@ public class RobotContainer {
new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.45), m_robotBoomBoom)
);
if (m_robotBoomBoom.readManualData) {
var tab = Shuffleboard.getTab("Manual Shooter Data");
var manual = tab.getLayout("Manual Shooter Data", BuiltInLayouts.kList).withSize(2, 3);
manual.add("Manual Shooter Data", new Sendable() {
private double manualHoodExt;
private double manualDrumVel;
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("RobotPreferences");
builder.addBooleanProperty("Disable CSV", () -> m_robotBoomBoom.readManualData, b -> m_robotBoomBoom.readManualData = b);
builder.addDoubleProperty("Drum Velocity", () -> manualDrumVel, d -> manualDrumVel = d);
builder.addDoubleProperty("Hood Extension", () -> manualHoodExt, d -> manualHoodExt = d);
builder.addDoubleProperty("Measured Distance", () -> SmartDashboard.getNumber("Distance to Target", -1), System.out::println);
}
});
manual.add("Shooter Table Appender", new InstantCommand(() -> Errors.log().run(() -> Files.write(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath(), String.format("%s,%s,%s%n", SmartDashboard.getNumber("Manual Shooter Data/Distance to Target", -1), SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", -1), SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", -1)).getBytes(), StandardOpenOption.WRITE, StandardOpenOption.APPEND))) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Append"));
var csv = tab.getLayout("Shooter Data", BuiltInLayouts.kList).withPosition(2,0).withSize(4, 3);
csv.add("Initial Shooter Data", builder -> Arrays.stream(m_robotBoomBoom.m_shooterTable).forEach(e -> builder.addDoubleArrayProperty(Double.toString(e.distance), () -> new double[] {e.hoodExt, e.drumVelocity}, a -> {})));
csv.add("Reload Data", new InstantCommand(m_robotBoomBoom::updateShooterTable) {
@Override
public boolean runsWhenDisabled() {
return true;
}
}.withName("Reload"));
}
}
/**
@@ -460,6 +498,7 @@ public class RobotContainer {
// * 2. try opposite joystick input: new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, false), m_robotSwerveDrive);
// * 3a. try permanently setting drive motors to brake, not coast, in RobotMap.java, and ask the driver how it feels.
// * 3b. try to only set the drive motors to brake if in auto mode.
// * 4. try new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive);
// ! 1.0 input, 1 second: 134 inches
// ! 0.75 input, 1 second: 48 inches
@@ -471,33 +510,29 @@ public class RobotContainer {
double distancePerSecond = 134.0; // * assuming emulated joystick input magnitude is 1.0
double offset = 10.0; // * distance (in inches) from ball that we actually want to stop
// ! ball positions are unit tested
Vector2D firstBallPosition = new Vector2D(15.56 - (82.83 / 2.00), 11.21 - 162.00); // * position of first ball, relative to hub.
Vector2D secondBallPosition = new Vector2D(-(40.44 * (Math.sqrt(2.00) / 2.00)) - ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (82.83 / 2.00), -(40.44 * (Math.sqrt(2.00) / 2.00)) + ((82.83 - 7.58) * (Math.sqrt(2.00) / 2.00)) - (219.25 / 2.00)); // * position of second ball, relative to hub.
Vector2D firstToSecond = Vector2D.subtract(secondBallPosition, firstBallPosition); // * vector from first ball to second ball, used to calculate emulated joystick inputs.
// SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new ParallelCommandGroup(
// new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
// new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
// )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new ParallelCommandGroup(
new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, true),
new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), 2.0)
)); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead.
// return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); // * stop running storage
// ! SHOOT FIRST BALL, THEN DRIVE OFF LINE (HOPEFULLY)
return new SequentialCommandGroup( //new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
// ! DRIVE OFF LINE, THEN SHOOT BALL (HOPEFULLY)
return new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.5, 0.0, 0.0}, 0.5), // * drive out of tarmac
new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), // * brake
new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving
new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.6, 0.0, 0.0}, 0.5))); // * drive off line
new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
weirdAutoShootingGroup, // * shoot
new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage) // * stop running storage
);
// ! TWO BALL AUTO (HOPEFULLY)
// return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage
// new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
// return new SequentialCommandGroup( new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), // * extend out, in preparation of running intake
// new ParallelCommandGroup( new RunCommand(() -> m_robotIntake.runAtOutput(1.0), m_robotIntake), // * run intake all throughout path
@@ -506,7 +541,7 @@ public class RobotContainer {
// // new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 376),
// new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive),
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target
// new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true), // * aim with turret to target
// weirdAutoShootingGroup, // * shoot
// new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)))); // * stop running storage
src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java
+14 -14
View File
@@ -35,7 +35,7 @@ public class TrackTarget extends CommandBase {
BoomBoom m_boomBoom;
Hood m_hood;
// boolean isAuto;
boolean isAuto;
static double velocity;
static double hoodPosition;
@@ -55,22 +55,22 @@ public class TrackTarget extends CommandBase {
long startTime;
private double timeTolerance;
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry/*, boolean isAuto*/) {
public TrackTarget (Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry, boolean isAuto) {
m_turret = turret;
m_boomBoom = boomBoom;
m_hood = hood;
m_visionOdometry = visionOdometry;
// this.isAuto = isAuto;
this.isAuto = isAuto;
this.timeTolerance = 1000;
addRequirements(m_turret, m_boomBoom, m_hood, m_visionOdometry);
SmartDashboard.putNumber("Distance Adjust", -35);
}
// public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
// this(turret, boomBoom, hood, visionOdometry, false);
// }
public TrackTarget(Turret turret, BoomBoom boomBoom, Hood hood, VisionOdometry visionOdometry) {
this(turret, boomBoom, hood, visionOdometry, false);
}
// Called when the command is initially scheduled.
@Override
@@ -195,14 +195,14 @@ public class TrackTarget extends CommandBase {
// Returns true when the command should end.
@Override
public boolean isFinished() {
// if (this.isAuto) {
// if (targetLocked && !timerStarted) {
// timerStarted = true;
// startTime = System.currentTimeMillis();
// }
// return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
// } else {
if (this.isAuto) {
if (targetLocked && !timerStarted) {
timerStarted = true;
startTime = System.currentTimeMillis();
}
return (targetLocked && timerStarted && ((System.currentTimeMillis() - startTime) > timeTolerance));
} else {
return false;
// }
}
}
}
src/main/java/frc4388/robot/subsystems/BoomBoom.java
+9 -7
View File
@@ -31,6 +31,7 @@ import frc4388.utility.Gains;
import frc4388.utility.NumericData;
public class BoomBoom extends SubsystemBase {
public boolean readManualData = true;
private static final Logger LOGGER = Logger.getLogger(BoomBoom.class.getSimpleName());
public WPI_TalonFX m_shooterFalconLeft;
public WPI_TalonFX m_shooterFalconRight;
@@ -54,7 +55,7 @@ public class BoomBoom extends SubsystemBase {
public Double distance, hoodExt, drumVelocity;
}
private ShooterTableEntry[] m_shooterTable;
public ShooterTableEntry[] m_shooterTable;
/** Creates a new BoomBoom, which has a drum shooter and angle adjuster. */
public BoomBoom(WPI_TalonFX shooterFalconLeft, WPI_TalonFX shooterFalconRight) {
@@ -63,7 +64,7 @@ public class BoomBoom extends SubsystemBase {
setShooterGains();
m_shooterTable = readShooterTable();
updateShooterTable();
// Run a helper method that logs the contents of the table on a new thread.
new Thread(() -> LOGGER.fine(() -> CSV.ReflectionTable.create(m_shooterTable, RobotBase.isSimulation()))).start();
}
@@ -76,7 +77,7 @@ public class BoomBoom extends SubsystemBase {
* @return Drum Velocity in units per 100 ms
*/
public Double getVelocity(final Double distance) {
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Drum Velocity", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.drumVelocity).doubleValue();
}
/**
@@ -87,7 +88,7 @@ public class BoomBoom extends SubsystemBase {
* @return Hood extension in units
*/
public Double getHood(final Double distance) {
return NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
return readManualData ? SmartDashboard.getNumber("Manual Shooter Data/Hood Extension", 0) : NumericData.linearInterpolate(m_shooterTable, distance, e -> e.distance, e -> e.hoodExt).doubleValue();
}
@Override
@@ -139,7 +140,8 @@ public class BoomBoom extends SubsystemBase {
// m_shooterFalconLeft.set(m_controller.calculate(m_shooterFalconLeft.get(), targetVel));
}
private static ShooterTableEntry[] readShooterTable() {
public void updateShooterTable() {
try {
// This is a helper class that allows us to read a CSV file into a Java array.
CSV<ShooterTableEntry> csv = new CSV<>(ShooterTableEntry::new) {
@@ -162,14 +164,14 @@ public class BoomBoom extends SubsystemBase {
}
};
// This is reading the CSV file into a Java array.
return csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
m_shooterTable = csv.read(new File(Filesystem.getDeployDirectory(), "ShooterData.csv").toPath());
} catch (IOException exception) {
ShooterTableEntry dummyEntry = new ShooterTableEntry();
dummyEntry.distance = 0.0;
dummyEntry.hoodExt = 0.0;
dummyEntry.drumVelocity = 0.0;
LOGGER.log(Level.SEVERE, "Exception while reading shooter CSV table.", exception);
return new ShooterTableEntry[] { dummyEntry };
m_shooterTable = new ShooterTableEntry[] { dummyEntry };
}
}