diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index d378e65..abcdc20 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -462,8 +462,11 @@ public class RobotContainer { // traj.add(new State(0.45710678118654746, 1.0, -4.0, new Pose2d(3.575, -2.3, new Rotation2d(0)), 0.0)); // traj.add(new State(0.7071067811865475, 0.0, -4.0, new Pose2d(3.7, -2.3, new Rotation2d(0)), 0.0)); Trajectory traj = TrajectoryGenerator.generateTrajectory( + // start at origin facing +X direction new Pose2d(0, 0, new Rotation2d(0)), + // pass through two interior waypoints List.of(new Translation2d(1, 1), new Translation2d(2, -1)), + // end 3 meters straight of initial position, facing forward new Pose2d(3, 0, new Rotation2d(0)), config); diff --git a/src/main/java/frc4388/utility/AutoRecording.java b/src/main/java/frc4388/utility/AutoRecording.java new file mode 100644 index 0000000..24ccb0d --- /dev/null +++ b/src/main/java/frc4388/utility/AutoRecording.java @@ -0,0 +1,77 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2019 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package frc4388.utility; + +import edu.wpi.first.wpilibj.geometry.Pose2d; +import edu.wpi.first.wpilibj.geometry.Transform2d; +import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc4388.robot.subsystems.Drive; + +public class AutoRecording extends CommandBase { + /** + * Creates a new AutoRecording. + */ + Drive m_drive; + + Pose2d start = new Pose2d(); + Pose2d prev = new Pose2d(); + Pose2d curr = new Pose2d(); + + boolean isFirst; + + double currentVel = 0; + double currentAcc = 0; + + double maxVel = Double.MIN_VALUE; + double maxAcc = Double.MIN_VALUE; + + double EPSILON = 1e-5; + + int ctr = 0; + + public AutoRecording(Drive subsystem) { + // Use addRequirements() here to declare subsystem dependencies. + m_drive = subsystem; + addRequirements(m_drive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + start = m_drive.getPose(); + isFirst = true; + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + prev = isFirst ? start : curr; + isFirst = false; + + curr = m_drive.getPose(); + + Transform2d diff = new Transform2d(prev, curr); + + if ((Math.abs(diff.getTranslation().getX()) > EPSILON) || (Math.abs(diff.getTranslation().getY()) > EPSILON)) { + + } + + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +}