Skip to content

Commit

Permalink
Migrate to IMU interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@rbrott
rbrott committed Jan 19, 2023
1 parent b38e9e6 commit 970625f
Show file tree
Hide file tree
Showing 6 changed files with 27 additions and 173 deletions.
16 changes: 8 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,14 +27,14 @@
import com.acmerobotics.roadrunner.Twist2dIncrDual;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.VelConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;

import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
Expand Down Expand Up @@ -95,7 +95,7 @@ kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),

public final VoltageSensor voltageSensor;

public final BNO055Wrapper imu;
public final IMU imu;

public final Localizer localizer;
public Pose2d pose;
Expand Down Expand Up @@ -176,11 +176,11 @@ public MecanumDrive(HardwareMap hardwareMap, Pose2d pose) {
rightBack.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);

BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
baseImu.initialize(parameters);
imu = new BNO055Wrapper(baseImu);
imu = hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
imu.initialize(parameters);

voltageSensor = hardwareMap.voltageSensor.iterator().next();

Expand Down
16 changes: 8 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TankDrive.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,14 +29,14 @@
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.Vector2dDual;
import com.acmerobotics.roadrunner.VelConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;

import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.LynxFirmwareVersion;
Expand Down Expand Up @@ -93,7 +93,7 @@ kinematics.new WheelVelConstraint(MAX_WHEEL_VEL),

public final List<DcMotorEx> leftMotors, rightMotors;

public final BNO055Wrapper imu;
public final IMU imu;

public final VoltageSensor voltageSensor;

Expand Down Expand Up @@ -191,11 +191,11 @@ public TankDrive(HardwareMap hardwareMap, Pose2d pose) {
m.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}

BNO055IMU baseImu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
baseImu.initialize(parameters);
imu = new BNO055Wrapper(baseImu);
imu = hardwareMap.get(IMU.class, "imu");
IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
RevHubOrientationOnRobot.LogoFacingDirection.UP,
RevHubOrientationOnRobot.UsbFacingDirection.FORWARD));
imu.initialize(parameters);

voltageSensor = hardwareMap.voltageSensor.iterator().next();

Expand Down
13 changes: 7 additions & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/TwoDeadWheelLocalizer.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,9 @@
import com.acmerobotics.roadrunner.Vector2dDual;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;

import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.RawEncoder;
Expand All @@ -20,35 +21,35 @@ public final class TwoDeadWheelLocalizer implements Localizer {
public static double PERP_X_TICKS = 0.0;

public final Encoder par, perp;
public final BNO055Wrapper imu;
public final IMU imu;

private int lastParPos, lastPerpPos;
private Rotation2d lastHeading;

private final double inPerTick;

public TwoDeadWheelLocalizer(HardwareMap hardwareMap, BNO055Wrapper imu, double inPerTick) {
public TwoDeadWheelLocalizer(HardwareMap hardwareMap, IMU imu, double inPerTick) {
par = new RawEncoder(hardwareMap.get(DcMotorEx.class, "par"));
perp = new RawEncoder(hardwareMap.get(DcMotorEx.class, "perp"));
this.imu = imu;

lastParPos = par.getPositionAndVelocity().position;
lastPerpPos = perp.getPositionAndVelocity().position;
lastHeading = imu.getHeading();
lastHeading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));

this.inPerTick = inPerTick;
}

public Twist2dIncrDual<Time> updateAndGetIncr() {
Encoder.PositionVelocityPair parPosVel = par.getPositionAndVelocity();
Encoder.PositionVelocityPair perpPosVel = perp.getPositionAndVelocity();
Rotation2d heading = imu.getHeading();
Rotation2d heading = Rotation2d.exp(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));

int parPosDelta = parPosVel.position - lastParPos;
int perpPosDelta = perpPosVel.position - lastPerpPos;
double headingDelta = heading.minus(lastHeading);

double headingVel = imu.getHeadingVelocity();
double headingVel = imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;

Twist2dIncrDual<Time> twistIncr = new Twist2dIncrDual<>(
new Vector2dDual<>(
Expand Down
3 changes: 2 additions & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/AngularRampLogger.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotorEx;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.MidpointTimer;
Expand Down Expand Up @@ -119,7 +120,7 @@ class Data {
}
data.encTimes.add(t.addSplit());

AngularVelocity av = view.imu.getAngularVelocity();
AngularVelocity av = view.imu.getRobotAngularVelocity(AngleUnit.RADIANS);
data.angVels.get(0).add((double) av.xRotationRate);
data.angVels.get(1).add((double) av.yRotationRate);
data.angVels.get(2).add((double) av.zRotationRate);
Expand Down
4 changes: 2 additions & 2 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/DriveView.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,13 @@
import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.VoltageSensor;

import org.firstinspires.ftc.teamcode.MecanumDrive;
import org.firstinspires.ftc.teamcode.TankDrive;
import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer;
import org.firstinspires.ftc.teamcode.util.BNO055Wrapper;
import org.firstinspires.ftc.teamcode.util.Encoder;
import org.firstinspires.ftc.teamcode.util.Localizer;
import org.firstinspires.ftc.teamcode.util.OverflowEncoder;
Expand All @@ -37,7 +37,7 @@ final class DriveView {
public final List<RawEncoder> leftEncs, rightEncs, parEncs, perpEncs;
public final List<RawEncoder> forwardEncs;

public final BNO055Wrapper imu;
public final IMU imu;

public final VoltageSensor voltageSensor;

Expand Down
148 changes: 0 additions & 148 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/BNO055Wrapper.java
View file

This file was deleted.

0 comments on commit 970625f

Please sign in to comment.