changes have been made. do not ask what these changes accomplish.

This commit is contained in:
aarav18
2021-05-20 17:56:17 -06:00
parent 5883c67d44
commit 53d2776658
2 changed files with 80 additions and 0 deletions
@@ -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);
@@ -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;
}
}