summaryrefslogtreecommitdiff
path: root/src/main/java/frc
diff options
context:
space:
mode:
Diffstat (limited to 'src/main/java/frc')
-rw-r--r--src/main/java/frc/robot/Constants.java17
-rw-r--r--src/main/java/frc/robot/RobotContainer.java39
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java36
-rw-r--r--src/main/java/frc/robot/util/IMUGyro.java34
4 files changed, 82 insertions, 44 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 41c9689..6d471db 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,28 +4,29 @@ import com.revrobotics.CANSparkMax;
import com.revrobotics.CANSparkLowLevel.MotorType;
import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.I2C.Port;
import frc.robot.util.IMUGyro;
public class Constants
{
- public static final int PortDriverController = 0;
+ public static final GenericHID controller = new GenericHID(1);
- public static final double DriveMaxSpeed = 20; // m/s
+ public static final double DriveMaxSpeed = 5; // m/s
public static final double DriveMaxAngular = Math.PI; // rad/s?
- public static final double WheelRadius = 0; // in meters
- public static final double TrackWidth = 0; // in meters
+ public static final double WheelRadius = 0.0254; // in meters
+ public static final double TrackWidth = 0.74; // in meters
public static final double EncoderResolution = 4096;
public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68));
- public static final CANSparkMax motorL_leader = new CANSparkMax(40, MotorType.kBrushed);
- public static final CANSparkMax motorL_follower = new CANSparkMax(41, MotorType.kBrushed);
- public static final CANSparkMax motorR_leader = new CANSparkMax(42, MotorType.kBrushed);
- public static final CANSparkMax motorR_follower = new CANSparkMax(43, MotorType.kBrushed);
+ public static final CANSparkMax motorL_leader = new CANSparkMax(11, MotorType.kBrushed);
+ public static final CANSparkMax motorL_follower = new CANSparkMax(12, MotorType.kBrushed);
+ public static final CANSparkMax motorR_leader = new CANSparkMax(13, MotorType.kBrushed);
+ public static final CANSparkMax motorR_follower = new CANSparkMax(14, MotorType.kBrushed);
public static final Encoder encoderL = new Encoder(0, 1);
public static final Encoder encoderR = new Encoder(2, 3);
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index d91ff78..d2cede4 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,34 +4,53 @@
package frc.robot;
-import edu.wpi.first.math.filter.SlewRateLimiter;
+import edu.wpi.first.math.geometry.Pose2d;
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 frc.robot.subsystems.Drivetrain;
public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
- private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3);
- private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3);
-
- private final GenericHID m_controller =
- new GenericHID(Constants.PortDriverController);
+ private final GenericHID m_controller = Constants.controller;
public RobotContainer()
{
configureBindings();
}
+ private double deadzone(double low, double high, double val) {
+ if(val > low && val < high) return 0.0;
+ return val;
+ }
+
private void configureBindings()
{
m_drive.setDefaultCommand(m_drive.run(() ->
- m_drive.drive(
- m_speedlimiter.calculate(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
- m_rotlimiter.calculate(m_controller.getRawAxis(2)) * Constants.DriveMaxAngular
- )));
+ // m_drive.arcade(
+ // deadzone(-1, 1, m_controller.getRawAxis(1) * Constants.DriveMaxSpeed),
+ // deadzone(-1, 1, m_controller.getRawAxis(0) * Constants.DriveMaxAngular)
+ // )));
+ m_drive.tank(
+ deadzone(-0.09, 0.09, m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed,
+ deadzone(-0.09, 0.09, m_controller.getRawAxis(3)) * Constants.DriveMaxSpeed
+ )));
+
+ new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
+ Pose2d pose = m_drive.pose2d();
+ System.out.println("x: " + pose.getX() +" y: "+ pose.getY());
+ System.out.println("yaw: " + Constants.gyro.toRotation2d().getDegrees());
+ }));
+ new Trigger(() -> m_controller.getRawButton(2)).onTrue(Commands.runOnce(() -> {
+ System.out.println("Reset");
+ m_drive.reset();
+ }));
+ // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.run(() -> {
+ // m_drive.tank(5, 5);
+ // }));
}
public Command getAutonomousCommand()
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index 8f1b975..80ed06e 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -4,6 +4,7 @@ import com.revrobotics.CANSparkMax;
import edu.wpi.first.math.controller.PIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+import edu.wpi.first.math.geometry.Pose2d;
import edu.wpi.first.math.kinematics.ChassisSpeeds;
import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
@@ -25,7 +26,7 @@ public class Drivetrain extends SubsystemBase
private Encoder m_encoderL = Constants.encoderL;
private Encoder m_encoderR = Constants.encoderR;
- private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1);
+ private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1.5, 2.82, 0.47);
private final PIDController m_pidL = new PIDController(1, 0, 0);
private final PIDController m_pidR = new PIDController(1, 0, 0);
@@ -38,14 +39,13 @@ public class Drivetrain extends SubsystemBase
public Drivetrain()
{
m_gyro.reset();
+ m_gyro.calibrateGyro(10000);
m_motorR_follower.follow(m_motorR_leader);
m_motorL_follower.follow(m_motorL_leader);
+ m_motorR_leader.setInverted(true);
- // perhaps invert
- // m_motorR_leader.setInverted(true);
-
- m_encoderL.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
+ m_encoderL.setDistancePerPulse(-2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
m_encoderR.setDistancePerPulse(2 * Math.PI * Constants.WheelRadius / Constants.EncoderResolution);
m_encoderL.reset();
@@ -59,11 +59,17 @@ public class Drivetrain extends SubsystemBase
// speed - linear velocity in m/s
// rot - angular velocity in rad/s
- public void drive(double speed, double rot)
+ public void arcade(double speed, double rot)
{
speed(m_kinematics.toWheelSpeeds(new ChassisSpeeds(speed, 0.0, rot)));
}
+ // in m/s
+ public void tank(double left, double right)
+ {
+ speed(new DifferentialDriveWheelSpeeds(left, right));
+ }
+
public void speed(DifferentialDriveWheelSpeeds speeds)
{
m_motorL_leader.setVoltage(
@@ -77,6 +83,24 @@ public class Drivetrain extends SubsystemBase
);
}
+ public Pose2d pose2d()
+ {
+ System.out.println("Distance: "+m_encoderL.getDistance());
+ System.out.println("Distance: "+m_encoderR.getDistance());
+ return m_odometry.getPoseMeters();
+ }
+
+ public void reset()
+ {
+ m_gyro.reset();
+ m_encoderL.reset();
+ m_encoderR.reset();
+ m_odometry = new DifferentialDriveOdometry(
+ m_gyro.toRotation2d(),
+ Constants.encoderL.getDistance(),
+ Constants.encoderR.getDistance());
+ }
+
@Override
public void periodic()
{
diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java
index 1ea1983..3860b85 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -12,7 +12,11 @@ public class IMUGyro
public double roll;
public double yaw;
- private double gyroOffsets[] = new double[3];
+ private double gyroOffsets[] = {
+ 0.312449302886265,
+ 0.04883417185589417,
+ -0.12945104079301352
+ };
// for deltatime
private double prev_time = Timer.getFPGATimestamp();
@@ -27,7 +31,7 @@ public class IMUGyro
private final int ACCL_ADDR = 0x3B;
private final int ACCL_CONFIG = 0x1C;
- private final double SCALE_GYRO = (Math.PI/(180*16.384));
+ private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
private final double SCALE_ACCL = (1/16.384);
public IMUGyro(I2C gyro)
@@ -35,15 +39,15 @@ public class IMUGyro
mpu = gyro;
reset();
- mpu.write(CONFIG_ADDR, 0b00000110);
- mpu.write(SMPLRT_DIV, 0b00010011);
- mpu.write(GYRO_CONFIG, 0b00001000);
- mpu.write(ACCL_CONFIG, 0b00000000);
+ // mpu.write(CONFIG_ADDR, 0b000000);
+ mpu.write(SMPLRT_DIV, 0b00000010);
+ // mpu.write(GYRO_CONFIG, 0b00011000);
+ // mpu.write(ACCL_CONFIG, 0b00000000);
}
public void reset()
{
- mpu.write(PWR_MGMT_1, 0b10001000);
+ mpu.write(PWR_MGMT_1, 0b000001000);
pitch = 0;
roll = 0;
@@ -56,21 +60,11 @@ public class IMUGyro
double dt = cur_time - prev_time;
prev_time = cur_time;
- // double[] gRates = gyroRates();
-
- // roll = gRates[0] * dt;
- // pitch = gRates[1] * dt;
- // yaw = gRates[2] * dt;
-
double[] gRates = gyroRates();
- double[] aRates = acclRates();
-
- mahony(30, 0, gRates, aRates, dt);
- double[] angles = fromQuaternion(q);
- roll = angles[0];
- pitch = angles[1];
- yaw = angles[2];
+ roll += gRates[0] * dt;
+ pitch += gRates[1] * dt;
+ yaw += gRates[2] * dt;
}
public Rotation2d toRotation2d()
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 01:05:45 +0000