summaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/main/java/frc/robot/Constants.java4
-rw-r--r--src/main/java/frc/robot/Robot.java4
-rw-r--r--src/main/java/frc/robot/RobotContainer.java45
-rw-r--r--src/main/java/frc/robot/subsystems/Drivetrain.java13
-rw-r--r--src/main/java/frc/robot/util/IMUGyro.java83
5 files changed, 26 insertions, 123 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index 98824ae..017fa74 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -4,14 +4,14 @@ 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 edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import frc.robot.util.IMUGyro;
public class Constants
{
- public static final GenericHID controller = new GenericHID(1);
+ public static final CommandXboxController controller = new CommandXboxController(0);
public static final double DriveMaxSpeed = 5; // m/s
public static final double DriveMaxAngular = Math.PI; // rad/s?
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 7afe5f8..bdaae21 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,10 +4,6 @@
package frc.robot;
-import com.revrobotics.CANSparkMax;
-import com.revrobotics.CANSparkLowLevel.MotorType;
-
-import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.CommandScheduler;
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 3580633..ed6afe9 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,13 +4,10 @@
package frc.robot;
-import javax.smartcardio.CommandAPDU;
-
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 edu.wpi.first.wpilibj2.command.button.CommandXboxController;
import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
import frc.robot.subsystems.Drivetrain;
@@ -18,7 +15,7 @@ public class RobotContainer
{
private final Drivetrain m_drive = new Drivetrain();
- private final GenericHID m_controller = Constants.controller;
+ private final CommandXboxController m_controller = Constants.controller;
public RobotContainer()
{
@@ -34,59 +31,37 @@ public class RobotContainer
{
m_drive.setDefaultCommand(m_drive.run(() ->
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
+ deadzone(-0.09, 0.09, m_controller.getLeftY()) * Constants.DriveMaxSpeed,
+ deadzone(-0.09, 0.09, m_controller.getRightY()) * Constants.DriveMaxSpeed
)));
// debug controls
- new Trigger(() -> m_controller.getRawButton(1)).onTrue(Commands.runOnce(() -> {
+ m_controller.a().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(() -> {
+ m_controller.b().onTrue(Commands.runOnce(() -> {
System.out.println("Reset");
m_drive.reset();
}));
-
- new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(8))
+ m_controller.x().and(m_controller.leftBumper())
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Quasistatic Forward")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
- new Trigger(() -> m_controller.getRawButton(3) && m_controller.getRawButton(7))
+ m_controller.x().and(m_controller.rightBumper())
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Quasistatic Reverse")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
- new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(8))
+ m_controller.y().and(m_controller.leftBumper())
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Dynamic Forward")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward)));
- new Trigger(() -> m_controller.getRawButton(4) && m_controller.getRawButton(7))
+ m_controller.y().and(m_controller.rightBumper())
.onTrue(Commands.sequence(Commands.runOnce(() ->
System.out.println("Dynamic Reverse")),
m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse)));
-
- // new Trigger(() -> m_controller.getRawButton(3)).onTrue(Commands.runOnce(() -> {
- // System.out.println("Running Quasistatis routine");
- // if(m_controller.getRawButton(8)) {
- // System.out.println("Reverse");
- // Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kReverse));
- // } else {
- // System.out.println("Forward");
- // Commands.sequence(m_drive.sysIdQausistatic(SysIdRoutine.Direction.kForward));
- // }
- // }));
- // new Trigger(() -> m_controller.getRawButton(4) && m_controller()).onTrue(() -> {
- // System.out.println("Running Dynamic routine");
- // if(m_controller.getRawButton(8)) {
- // System.out.println("Reverse");
- // return m_drive.sysIdDynamic(SysIdRoutine.Direction.kReverse);
- // } else {
- // System.out.println("Forward");
- // return m_drive.sysIdDynamic(SysIdRoutine.Direction.kForward);
- // }
- // }());
}
public Command getAutonomousCommand()
diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java
index c149477..557c7e3 100644
--- a/src/main/java/frc/robot/subsystems/Drivetrain.java
+++ b/src/main/java/frc/robot/subsystems/Drivetrain.java
@@ -175,11 +175,20 @@ public class Drivetrain extends SubsystemBase
public Command sysIdQausistatic(SysIdRoutine.Direction direction)
{
- return m_sysIdRoutine.quasistatic(direction);
+ return Commands.race(m_sysIdRoutine.quasistatic(direction),
+ Commands.runOnce(() -> {
+ while(m_encoderL.getDistance() < 6 &&
+ m_encoderR.getDistance() < 6);
+ }));
+
}
public Command sysIdDynamic(SysIdRoutine.Direction direction)
{
- return m_sysIdRoutine.dynamic(direction);
+ return Commands.race(m_sysIdRoutine.dynamic(direction),
+ Commands.runOnce(() -> {
+ while(m_encoderL.getDistance() < 6 &&
+ m_encoderR.getDistance() < 6);
+ }));
}
} \ 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 8d41c63..aa4563b 100644
--- a/src/main/java/frc/robot/util/IMUGyro.java
+++ b/src/main/java/frc/robot/util/IMUGyro.java
@@ -28,21 +28,17 @@ public class IMUGyro
private final int GYRO_ADDR = 0x43;
private final int GYRO_CONFIG = 0x1B;
- private final int ACCL_ADDR = 0x3B;
- private final int ACCL_CONFIG = 0x1C;
-
private final double SCALE_GYRO = (Math.PI/(180*16.384) * 2 /*??*/);
- private final double SCALE_ACCL = (1/16.384);
public IMUGyro(I2C gyro)
{
mpu = gyro;
reset();
- mpu.write(CONFIG_ADDR, 0b00000100);
- mpu.write(SMPLRT_DIV, 0b00010011);
+ mpu.write(CONFIG_ADDR, 0b00000101);
+ // mpu.write(SMPLRT_DIV, 0b00010011);
mpu.write(GYRO_CONFIG, 0b00011000);
- mpu.write(ACCL_CONFIG, 0b00000000);
+ // mpu.write(ACCL_CONFIG, 0b00000000);
}
public void reset()
@@ -95,11 +91,6 @@ public class IMUGyro
return scaledRates(GYRO_ADDR, SCALE_GYRO);
}
- private double[] acclRates()
- {
- return scaledRates(ACCL_ADDR, SCALE_ACCL);
- }
-
private double[] scaledRates(int addr, double scale)
{
byte[] buffer = new byte[6];
@@ -114,73 +105,5 @@ public class IMUGyro
{
return ((first << 8) | second);
}
-
- // mahony quaternion
- private double[] q = {1.0, 0.0, 0.0, 0.0};
- // mahony integral term
- private double[] i = new double[3];
-
- private double[] fromQuaternion(double[] q)
- {
- return new double[] {
- Math.atan2((q[0] * q[1] + q[2] * q[3]), 0.5 - (q[1] * q[1] + q[2] * q[2])),
- Math.asin(2.0 * (q[0] * q[2] - q[1] * q[3])),
- -Math.atan2((q[1] * q[2] + q[0] * q[3]), 0.5 - (q[2] * q[2] + q[3] * q[3]))
- };
- }
-
- private void mahony(double Ki, double Kp, double[] gRates, double[] aRates, double dt)
- {
- double gx = gRates[0], gy = gRates[1], gz = gRates[2];
- double ax = aRates[0], ay = aRates[1], az = aRates[2];
-
- double tmp = ax * ax + ay * ay + az * az;
- if(tmp > 0.0)
- {
- tmp = Math.sqrt(tmp);
- ax /= tmp; ay /= tmp; az /= tmp;
-
- // estimated direction of gravity in the body frame
- double vx = q[1] * q[3] - q[0] * q[2];
- double vy = q[0] * q[1] + q[2] * q[3];
- double vz = q[0] * q[0] - 0.5f + q[3] * q[3];
-
- // error is cross product
- double ex = (ay * vz - az * vy);
- double ey = (az * vx - ax * vz);
- double ez = (ax * vy - ay * vx);
-
- // integral feedback
- if(Ki > 0.0) {
- i[0] += Ki * ex * dt;
- i[1] += Ki * ey * dt;
- i[2] += Ki * ez * dt;
- gx += i[0];
- gy += i[1];
- gz += i[2];
- }
-
- gx += Kp * ex;
- gy += Kp * ey;
- gz += Kp * ez;
- }
-
- // integrate quaternion
- dt /= 2; // ??
- gx *= dt; gy *= dt; gz *= dt;
-
- double qa = q[0], qb = q[1], qc = q[2], qd = q[3];
- q[0] += (-qb * gx - qc * gy - qd * gz);
- q[1] += (qa * gx + qc * gz - qd * gy);
- q[2] += (qa * gy - qb * gz + qd * gx);
- q[3] += (qa * gz + qb * gy - qc * gx);
-
- // renormalize
- double qnorm_factor = Math.sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
- q[0] /= qnorm_factor;
- q[1] /= qnorm_factor;
- q[2] /= qnorm_factor;
- q[3] /= qnorm_factor;
- }
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-10-31 01:29:06 +0000