diff options
| -rw-r--r-- | src/main/java/frc/robot/Constants.java | 35 | ||||
| -rw-r--r-- | src/main/java/frc/robot/RobotContainer.java | 15 | ||||
| -rw-r--r-- | src/main/java/frc/robot/subsystems/Drivetrain.java | 33 | ||||
| -rw-r--r-- | src/main/java/frc/robot/util/IMUGyro.java | 192 | ||||
| -rw-r--r-- | vendordeps/REVLib.json | 74 |
5 files changed, 306 insertions, 43 deletions
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index f517f08..41c9689 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -1,37 +1,32 @@ package frc.robot; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.wpilibj.AnalogGyro; +import com.revrobotics.CANSparkMax; +import com.revrobotics.CANSparkLowLevel.MotorType; + import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.drive.DifferentialDrive; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.I2C.Port; +import frc.robot.util.IMUGyro; public class Constants { - // other constants - public static final int PortDriverController = 0; - public static final double DriveMaxSpeed = 0; - public static final double DriveMaxAngular = 0; + public static final double DriveMaxSpeed = 20; // 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 EncoderResolution = 4096; - - // public static final DifferentialDrive drive = new DifferentialDrive(...); - public static final AnalogGyro gyro = new AnalogGyro(...); - - public static final PWMSparkMax motorL_leader = new PWMSparkMax(...); - public static final PWMSparkMax motorL_follower = new PWMSparkMax(...); - public static final PWMSparkMax motorR_leader = new PWMSparkMax(...); - public static final PWMSparkMax motorR_follower = new PWMSparkMax(...); - + public static final IMUGyro gyro = new IMUGyro(new I2C(Port.kOnboard, 0x68)); - public static final Encoder encoderL = new Encoder(...); - public static final Encoder encoderR = new Encoder(...); + 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 SimpleMotorFeedforward feedforward = new SimpleMotorFeedforward(...); + 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 e08bf8d..d91ff78 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -5,9 +5,9 @@ package frc.robot; import edu.wpi.first.math.filter.SlewRateLimiter; +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.CommandXboxController; import frc.robot.subsystems.Drivetrain; public class RobotContainer @@ -17,8 +17,8 @@ public class RobotContainer private final SlewRateLimiter m_speedlimiter = new SlewRateLimiter(3); private final SlewRateLimiter m_rotlimiter = new SlewRateLimiter(3); - private final CommandXboxController m_dcontroller = - new CommandXboxController(Constants.PortDriverController); + private final GenericHID m_controller = + new GenericHID(Constants.PortDriverController); public RobotContainer() { @@ -27,12 +27,11 @@ public class RobotContainer private void configureBindings() { - // drivetrain controls m_drive.setDefaultCommand(m_drive.run(() -> - m_drive.drive( - m_speedlimiter.calculate(m_dcontroller.getLeftY()) * Constants.DriveMaxSpeed, - m_rotlimiter.calculate(m_dcontroller.getRightX()) * Constants.DriveMaxAngular - ))); + m_drive.drive( + m_speedlimiter.calculate(m_controller.getRawAxis(1)) * Constants.DriveMaxSpeed, + m_rotlimiter.calculate(m_controller.getRawAxis(2)) * Constants.DriveMaxAngular + ))); } public Command getAutonomousCommand() diff --git a/src/main/java/frc/robot/subsystems/Drivetrain.java b/src/main/java/frc/robot/subsystems/Drivetrain.java index 14c9d50..8f1b975 100644 --- a/src/main/java/frc/robot/subsystems/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/Drivetrain.java @@ -1,30 +1,31 @@ package frc.robot.subsystems; +import com.revrobotics.CANSparkMax; + import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.kinematics.ChassisSpeeds; 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.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Encoder; -import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; +import frc.robot.util.IMUGyro; public class Drivetrain extends SubsystemBase { - private AnalogGyro m_gyro = Constants.gyro; + private IMUGyro m_gyro = Constants.gyro; - private PWMSparkMax m_motorL_leader = Constants.motorL_leader; - private PWMSparkMax m_motorL_follower = Constants.motorL_follower; - private PWMSparkMax m_motorR_leader = Constants.motorR_leader; - private PWMSparkMax m_motorR_follower = Constants.motorR_follower; + private CANSparkMax m_motorL_leader = Constants.motorL_leader; + private CANSparkMax m_motorL_follower = Constants.motorL_follower; + private CANSparkMax m_motorR_leader = Constants.motorR_leader; + private CANSparkMax m_motorR_follower = Constants.motorR_follower; private Encoder m_encoderL = Constants.encoderL; private Encoder m_encoderR = Constants.encoderR; - private SimpleMotorFeedforward m_feedforward = Constants.feedforward; + private SimpleMotorFeedforward m_feedforward = new SimpleMotorFeedforward(1, 1); private final PIDController m_pidL = new PIDController(1, 0, 0); private final PIDController m_pidR = new PIDController(1, 0, 0); @@ -36,10 +37,10 @@ public class Drivetrain extends SubsystemBase public Drivetrain() { - m_gyro.reset(); + m_gyro.reset(); - m_motorL_leader.addFollower(m_motorL_follower); - m_motorR_leader.addFollower(m_motorR_follower); + m_motorR_follower.follow(m_motorR_leader); + m_motorL_follower.follow(m_motorL_leader); // perhaps invert // m_motorR_leader.setInverted(true); @@ -51,7 +52,7 @@ public class Drivetrain extends SubsystemBase m_encoderR.reset(); m_odometry = new DifferentialDriveOdometry( - m_gyro.getRotation2d(), + m_gyro.toRotation2d(), Constants.encoderL.getDistance(), Constants.encoderR.getDistance()); } @@ -79,10 +80,12 @@ public class Drivetrain extends SubsystemBase @Override public void periodic() { + m_gyro.update(); + m_odometry.update( - Constants.gyro.getRotation2d(), - Constants.encoderL.getDistance(), - Constants.encoderR.getDistance()); + m_gyro.toRotation2d(), + m_encoderL.getDistance(), + m_encoderR.getDistance()); } @Override diff --git a/src/main/java/frc/robot/util/IMUGyro.java b/src/main/java/frc/robot/util/IMUGyro.java new file mode 100644 index 0000000..1ea1983 --- /dev/null +++ b/src/main/java/frc/robot/util/IMUGyro.java @@ -0,0 +1,192 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.Timer; + +public class IMUGyro +{ + public I2C mpu; + + public double pitch; + public double roll; + public double yaw; + + private double gyroOffsets[] = new double[3]; + + // for deltatime + private double prev_time = Timer.getFPGATimestamp(); + + private final int PWR_MGMT_1 = 0x6B; + private final int CONFIG_ADDR = 0x1A; + private final int SMPLRT_DIV = 0x19; + + 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)); + private final double SCALE_ACCL = (1/16.384); + + public IMUGyro(I2C gyro) + { + mpu = gyro; + reset(); + + mpu.write(CONFIG_ADDR, 0b00000110); + mpu.write(SMPLRT_DIV, 0b00010011); + mpu.write(GYRO_CONFIG, 0b00001000); + mpu.write(ACCL_CONFIG, 0b00000000); + } + + public void reset() + { + mpu.write(PWR_MGMT_1, 0b10001000); + + pitch = 0; + roll = 0; + yaw = 0; + } + + public void update() + { + double cur_time = Timer.getFPGATimestamp(); + 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]; + } + + public Rotation2d toRotation2d() + { + return new Rotation2d(yaw % 360); + } + + public void calibrateGyro(int iter) + { + gyroOffsets = new double[3]; + + double x = 0, y = 0, z = 0; + for(int i = 0; i < iter; i++) + { + double[] rates = gyroRates(); + x += rates[0]; + y += rates[1]; + z += rates[2]; + } + + gyroOffsets[0] = x/iter; + gyroOffsets[1] = y/iter; + gyroOffsets[2] = z/iter; + } + + private double[] gyroRates() + { + 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]; + mpu.read(addr, 6, buffer); + double dx = decode(buffer[0], buffer[1]) * scale - gyroOffsets[0]; + double dy = decode(buffer[2], buffer[3]) * scale - gyroOffsets[1]; + double dz = decode(buffer[4], buffer[5]) * scale - gyroOffsets[2]; + return new double[] {dx, dy, dz}; + } + + private double decode(byte first, byte second) + { + 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; + } +} + diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json new file mode 100644 index 0000000..a829581 --- /dev/null +++ b/vendordeps/REVLib.json @@ -0,0 +1,74 @@ +{ + "fileName": "REVLib.json", + "name": "REVLib", + "version": "2024.2.1", + "frcYear": "2024", + "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb", + "mavenUrls": [ + "https://maven.revrobotics.com/" + ], + "jsonUrl": "https://software-metadata.revrobotics.com/REVLib-2024.json", + "javaDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-java", + "version": "2024.2.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-cpp", + "version": "2024.2.1", + "libName": "REVLib", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "com.revrobotics.frc", + "artifactId": "REVLib-driver", + "version": "2024.2.1", + "libName": "REVLibDriver", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +}
\ No newline at end of file |