From 839871ba03cc6d9865154216174183ff012fe113 Mon Sep 17 00:00:00 2001 From: kartofen Date: Mon, 4 Mar 2024 21:35:41 +0200 Subject: sysid and trajectory things --- src/main/java/frc/robot/Constants.java | 25 +++++++++++++++---------- 1 file changed, 15 insertions(+), 10 deletions(-) (limited to 'src/main/java/frc/robot/Constants.java') 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); } -- cgit v1.2.3