summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--.DataLogTool/datalogtool.json6
-rw-r--r--.SysId/sysid.json1
-rw-r--r--.vscode/settings.json3
-rw-r--r--src/main/java/frc/robot/Constants.java25
-rw-r--r--src/main/java/frc/robot/RobotContainer.java86
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java78
-rw-r--r--src/main/java/frc/robot/util/IMUGyro.java2
7 files changed, 123 insertions, 78 deletions
diff --git a/.DataLogTool/datalogtool.json b/.DataLogTool/datalogtool.json
new file mode 100644
index 0000000..8644b73
--- /dev/null
+++ b/.DataLogTool/datalogtool.json
@@ -0,0 +1,6 @@
+{
+ "download": {
+ "localDir": "/home/atanas",
+ "serverTeam": "10.90.89.2"
+ }
+}
diff --git a/.SysId/sysid.json b/.SysId/sysid.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 8be11f2..8743373 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -25,5 +25,6 @@
}
},
],
- "java.test.defaultConfig": "WPIlibUnitTests"
+ "java.test.defaultConfig": "WPIlibUnitTests",
+ "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 1ff70bb..c4c2329 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -6,6 +6,7 @@ import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.math.controller.ArmFeedforward;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.I2C;
@@ -15,21 +16,25 @@ import frc.robot.util.IMUGyro;
public class Constants
{
public static final GenericHID controller = new GenericHID(0);
+ public static final double InputSmoothing = 0.1;
public static final double DriveMaxSpeed = 5; // m/s
+ public static final double DriveMaxAcceleration = 5; // m/s^2
public static final double DriveMaxAngular = Math.PI; // rad/s?
+ public static final double AutonomousMaxSpeed = 0.5; // m/s
+ public static final double AutonomousMaxAcceleration = 0.25; // m/s^2
+
// for Drivetrain
- public static final double WheelDiameter = 0.1524; // in meters
- public static final double TrackWidth = 0.685; // in meters
- public static final double EncoderResolution = 4096 / 24;
+ public static final double WheelDiameter = Units.inchesToMeters(6); // in meters
+ public static final double TrackWidth = 0.58; // in meters
+ public static final double EncoderResolution = 16;
public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
public static final TrapezoidProfile.Constraints motor_constraints =
- new TrapezoidProfile.Constraints(DriveMaxSpeed, 4.5);
- public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(1.1, 3.5);
- // public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0, 0, 0);
+ new TrapezoidProfile.Constraints(DriveMaxSpeed, DriveMaxAcceleration);
+ public static final SimpleMotorFeedforward motor_feedforward = new SimpleMotorFeedforward(0.54082, 2.4705);
public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
@@ -64,12 +69,12 @@ public class Constants
public static final double ChainEncoderScale = 2600;
public static final TrapezoidProfile.Constraints chain_constraints =
- new TrapezoidProfile.Constraints(0, 0);
+ new TrapezoidProfile.Constraints(0.5, 0.5);
- public static final double ChainkP = 0.0;
+ public static final double ChainkP = 45.0;
public static final double ChainkI = 0.0;
public static final double ChainkD = 0.0;
- public static final CANSparkMax chain = null;//new CANSparkMax(70, MotorType.kBrushed);
- public static final Encoder chain_encoder = null;//new Encoder(3, 4);
+ public static final CANSparkMax chain = new CANSparkMax(50, MotorType.kBrushed);
+ public static final Encoder chain_encoder = new Encoder(8, 9);
}
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index cbaceea..87c4e60 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,16 +4,25 @@
package frc.robot;
-import org.littletonrobotics.junction.Logger;
+import java.util.List;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.button.Trigger;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine.Direction;
+
import frc.robot.subsystems.Drivetrain;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
+ // private final Chain m_chain = new Chain();
private final GenericHID m_controller = Constants.controller;
@@ -22,6 +31,20 @@ public class RobotContainer
configureBindings();
}
+ private double[] correctInputs(double in1, double in2)
+ {
+ double p1 = processInput(in1);
+ double p2 = processInput(in2);
+
+ if(Math.signum(p1) != Math.signum(p2))
+ return new double[] {p1, p2};
+
+ double diff = Math.abs(p1 - p2) * Constants.InputSmoothing;
+
+ return (p1 > p2) ? new double[] {p1 - diff, p2 + diff}
+ : new double[] {p1 + diff, p2 - diff};
+ }
+
private double processInput(double val)
{
double input = deadzone(-0.06, 0.06, val);
@@ -35,37 +58,36 @@ public class RobotContainer
}
private void configureBindings()
- {
- m_drive.setDefaultCommand(m_drive.run(() ->
- m_drive.tank(
- processInput(m_controller.getRawAxis(2)) * Constants.DriveMaxSpeed,
- processInput(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed
- )));
-
- // m_drive.setDefaultCommand(m_drive.run(() -> {
- // Logger.recordOutput("inputL", processInput(m_controller.getRawAxis(2)));
- // Logger.recordOutput("inputR", processInput(m_controller.getRawAxis(1)));
- // }));
- // m_controller.b().onTrue(Commands.runOnce(() -> {
- // m_drive.reset();
- // }));
-
- // m_controller.x().and(m_controller.leftBumper())
- // .onTrue(Commands.sequence(Commands.runOnce(() ->
- // System.out.println("Quasistatic Forward")),
- // m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
- // m_controller.x().and(m_controller.rightBumper())
- // .onTrue(Commands.sequence(Commands.runOnce(() ->
- // System.out.println("Quasistatic Reverse")),
- // m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
- // m_controller.y().and(m_controller.leftBumper())
- // .onTrue(Commands.sequence(Commands.runOnce(() ->
- // System.out.println("Dynamic Forward")),
- // m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
- // m_controller.y().and(m_controller.rightBumper())
- // .onTrue(Commands.sequence(Commands.runOnce(() ->
- // System.out.println("Dynamic Reverse")),
- // m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
+ {
+ m_drive.setDefaultCommand(m_drive.run(() -> {
+ double[] inputs = correctInputs(
+ m_controller.getRawAxis(2),
+ m_controller.getRawAxis(1));
+ m_drive.tank(
+ inputs[0] * Constants.DriveMaxSpeed,
+ inputs[1] * Constants.DriveMaxSpeed);
+ }));
+
+ Trajectory trajectory = TrajectoryGenerator.generateTrajectory(
+ new Pose2d(0, 0, new Rotation2d(0)),
+ // List.of(),
+ List.of(new Translation2d(1, 1), new Translation2d(2, -1)),
+ new Pose2d(3, 0, new Rotation2d(0)),
+ m_drive.trajectoyConfig);
+
+ new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.followTrajectory(trajectory));
+
+ // m_chain.setDefaultCommand(m_chain.run(() ->
+ // m_chain.atPercentage(
+ // processInput(m_controller.getRawAxis(2))
+ // )));
+
+
+ // new Trigger(() -> m_controller.getRawAxis(6) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kForward));
+ // new Trigger(() -> m_controller.getRawAxis(4) > 0).whileTrue(m_drive.sysIdQausistatic(Direction.kReverse));
+ // new Trigger(() -> m_controller.getRawAxis(7) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kForward));
+ // new Trigger(() -> m_controller.getRawAxis(5) > 0).whileTrue(m_drive.sysIdDynamic(Direction.kReverse));
+
}
public Command getAutonomousCommand()
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 5756650..5f34119 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -2,7 +2,9 @@ package frc.robot.subsystems;
import com.revrobotics.CANSparkMax;
+import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.controller.RamseteController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
@@ -10,15 +12,17 @@ import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.math.trajectory.TrajectoryConfig;
+import edu.wpi.first.math.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.units.Distance;
import edu.wpi.first.units.Measure;
import edu.wpi.first.units.Velocity;
import edu.wpi.first.units.Voltage;
import edu.wpi.first.units.MutableMeasure;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
@@ -47,8 +51,10 @@ public class Drivetrain extends SubsystemBase
private SimpleMotorFeedforward m_feedforward = Constants.motor_feedforward;
- private ProfiledPIDController m_pidL = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
- private ProfiledPIDController m_pidR = new ProfiledPIDController(0.2, 0, 0, Constants.motor_constraints);
+ private final double kP = 0.363;
+
+ private ProfiledPIDController m_pidL = new ProfiledPIDController(kP, 0, 0, Constants.motor_constraints);
+ private ProfiledPIDController m_pidR = new ProfiledPIDController(kP, 0, 0, Constants.motor_constraints);
private DifferentialDriveKinematics m_kinematics =
new DifferentialDriveKinematics(Constants.TrackWidth);
@@ -59,30 +65,39 @@ public class Drivetrain extends SubsystemBase
private final MutableMeasure<Voltage> m_voltage = mutable(Volts.of(0));
private final MutableMeasure<Distance> m_distance = mutable(Meters.of(0));
private final MutableMeasure<Velocity<Distance>> m_velocity = mutable(MetersPerSecond.of(0));
+
private final SysIdRoutine m_sysIdRoutine = new SysIdRoutine(
new SysIdRoutine.Config(),
new SysIdRoutine.Mechanism(
(Measure<Voltage> volts) -> {
- m_motorL_leader.set(-volts.in(Volts));
- m_motorR_leader.set(-volts.in(Volts));
+ m_motorL_leader.setVoltage(volts.in(Volts));
+ m_motorR_leader.setVoltage(volts.in(Volts));
},
log -> {
log.motor("drive-left")
.voltage(
m_voltage.mut_replace(
- m_motorL_leader.get() * RobotController.getBatteryVoltage(), Volts)
+ m_motorL_leader.getAppliedOutput() * m_motorL_leader.getBusVoltage(), Volts)
)
.linearPosition(m_distance.mut_replace(m_encoderL.getDistance(), Meters))
.linearVelocity(m_velocity.mut_replace(m_encoderL.getRate(), MetersPerSecond));
log.motor("driver-right")
- .voltage(
+ .voltage(
m_voltage.mut_replace(
- m_motorR_leader.get() * RobotController.getBatteryVoltage(), Volts)
+ m_motorR_leader.getAppliedOutput() * m_motorR_follower.getBusVoltage(), Volts)
)
.linearPosition(m_distance.mut_replace(m_encoderR.getDistance(), Meters))
.linearVelocity(m_velocity.mut_replace(m_encoderR.getRate(), MetersPerSecond));
+
}, this));
+ public final TrajectoryConfig trajectoyConfig =
+ new TrajectoryConfig(
+ Constants.AutonomousMaxSpeed,
+ Constants.AutonomousMaxAcceleration)
+ .setKinematics(m_kinematics);
+
+
public Drivetrain()
{
m_gyro.reset();
@@ -94,8 +109,8 @@ public class Drivetrain extends SubsystemBase
m_motorL_leader.setInverted(true);
m_encoderL.setReverseDirection(true);
- m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
- m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / Constants.EncoderResolution);
+ m_encoderL.setDistancePerPulse(Math.PI * Constants.WheelDiameter / 10.71 / Constants.EncoderResolution);
+ m_encoderR.setDistancePerPulse(Math.PI * Constants.WheelDiameter / 10.71 / Constants.EncoderResolution);
m_encoderL.reset();
m_encoderR.reset();
@@ -170,41 +185,36 @@ public class Drivetrain extends SubsystemBase
Logger.recordOutput(getName() + "/gyro", m_gyro.toRotation2d().getDegrees());
Logger.recordOutput(getName() + "/pose", pose2d());
-
- Logger.recordOutput(getName() + "/encoderL", m_encoderL.getRate());
- Logger.recordOutput(getName() + "/encoderR", m_encoderR.getRate());
+
+ Logger.recordOutput(getName() + "/encoderRateL", m_encoderL.getRate());
+ Logger.recordOutput(getName() + "/encoderRateR", m_encoderR.getRate());
+ Logger.recordOutput(getName() + "/encoderDL", m_encoderL.getDistance());
+ Logger.recordOutput(getName() + "/encoderDR", m_encoderR.getDistance());
}
public Command followTrajectory(Trajectory trajectory)
{
- // RamseteCommand ramsete = new RamseteCommand(
- // trajectory, this::pose2d,
- // new RamseteController(0, 0),
- // m_feedforward, m_kinematics,
- // () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()),
- // m_pidR, m_pidL, this::voltage, this);
-
- // return Commands.runOnce(() -> this.reset())
- // .andThen(ramsete)
- // .andThen(Commands.runOnce(() -> voltage(0, 0)));
- return Commands.none();
+ RamseteCommand ramsete = new RamseteCommand(
+ trajectory, this::pose2d,
+ new RamseteController(2.0, 0.7),
+ m_feedforward, m_kinematics,
+ () -> new DifferentialDriveWheelSpeeds(m_encoderL.getRate(), m_encoderR.getRate()),
+ new PIDController(kP, 0, 0),
+ new PIDController(kP, 0, 0),
+ this::voltage, this);
+
+ return Commands.runOnce(() -> this.reset())
+ .andThen(ramsete)
+ .andThen(Commands.runOnce(() -> voltage(0, 0)));
}
public Command sysIdQausistatic(SysIdRoutine.Direction direction)
{
- return Commands.race(m_sysIdRoutine.quasistatic(direction),
- this.runOnce(() -> {
- while(m_encoderL.getDistance() < 6 &&
- m_encoderR.getDistance() < 6);
- }));
+ return m_sysIdRoutine.quasistatic(direction);
}
public Command sysIdDynamic(SysIdRoutine.Direction direction)
{
- return Commands.race(m_sysIdRoutine.dynamic(direction),
- this.runOnce(() -> {
- while(m_encoderL.getDistance() < 6 &&
- m_encoderR.getDistance() < 6);
- }));
+ return m_sysIdRoutine.dynamic(direction);
}
} \ No newline at end of file
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index aa4563b..580af8b 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -65,7 +65,7 @@ public class IMUGyro
public Rotation2d toRotation2d()
{
- return new Rotation2d(yaw % 360);
+ return new Rotation2d(-yaw);
}
public void calibrateGyro(int iter)
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 06:43:28 +0000