Skip to content

Commit 4b6e7e8

Browse files
kevinfreikevinfrei
authored
IMU/IGyro interface change (#41)
* Adafruit External IMU support * Cleaned up/added to IGyro, implemented on AdafruitIMU and normal IMU * Got the angle mapping correct. External IMU works on ColinBot.
1 parent 4edacc6 commit 4b6e7e8

File tree

7 files changed

+237
-80
lines changed

7 files changed

+237
-80
lines changed

‎Path/src/main/java/com/technototes/path/subsystem/DistanceSensorLocalizer.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -106,6 +106,6 @@ public void setGyroOffset(double v) {
106106
}
107107

108108
public double getHeading() {
109-
return Angle.norm(gyro.gyroHeadingInRadians() - gyroOffset);
109+
return Angle.norm(gyro.getHeadingInRadians() - gyroOffset);
110110
}
111111
}

‎Path/src/main/java/com/technototes/path/subsystem/PathingMecanumDrivebaseSubsystem.java

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
2626
import com.technototes.library.hardware.HardwareDevice;
2727
import com.technototes.library.hardware.motor.EncodedMotor;
28+
import com.technototes.library.hardware.sensor.IGyro;
2829
import com.technototes.library.hardware.sensor.IMU;
2930
import com.technototes.library.subsystem.Subsystem;
3031
import com.technototes.path.subsystem.MecanumConstants.GearRatio;
@@ -91,7 +92,7 @@ public enum Mode {
9192

9293
protected DcMotorEx leftFront, leftRear, rightRear, rightFront;
9394
protected List<DcMotorEx> motors;
94-
protected IMU imu;
95+
protected IGyro gyro;
9596

9697
protected VoltageSensor batteryVoltageSensor;
9798

@@ -100,18 +101,18 @@ public PathingMecanumDrivebaseSubsystem(
100101
EncodedMotor<DcMotorEx> fr,
101102
EncodedMotor<DcMotorEx> rl,
102103
EncodedMotor<DcMotorEx> rr,
103-
IMU i,
104+
IGyro g,
104105
MecanumConstants c
105106
) {
106-
this(fl, fr, rl, rr, i, c, null);
107+
this(fl, fr, rl, rr, g, c, null);
107108
}
108109

109110
public PathingMecanumDrivebaseSubsystem(
110111
EncodedMotor<DcMotorEx> fl,
111112
EncodedMotor<DcMotorEx> fr,
112113
EncodedMotor<DcMotorEx> rl,
113114
EncodedMotor<DcMotorEx> rr,
114-
IMU i,
115+
IGyro g,
115116
MecanumConstants c,
116117
Localizer localizer
117118
) {
@@ -179,7 +180,7 @@ public PathingMecanumDrivebaseSubsystem(
179180
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
180181
}
181182

182-
imu = i;
183+
gyro = g;
183184

184185
for (DcMotorEx motor : motors) {
185186
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
@@ -367,7 +368,8 @@ public void setMotorPowers(double v, double v1, double v2, double v3) {
367368

368369
@Override
369370
public double getRawExternalHeading() {
370-
return imu.getAngularOrientation(AngleUnit.RADIANS).firstAngle;
371+
return gyro.getHeadingInRadians();
372+
// return gyro.getAngularOrientation(AngleUnit.RADIANS).firstAngle;
371373
}
372374

373375
@Override
@@ -393,7 +395,8 @@ public Double getExternalHeadingVelocity() {
393395
// Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
394396
// flat on a surface
395397

396-
return (double) imu.getAngularVelocity(AngleUnit.RADIANS).zRotationRate;
398+
// return (double) gyro.getAngularVelocity(AngleUnit.RADIANS).zRotationRate;
399+
return gyro.getVelocityInRadians();
397400
}
398401

399402
public static TrajectoryVelocityConstraint getVelocityConstraint(

‎Path/src/main/java/com/technototes/path/subsystem/TankDrivebaseSubsystem.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -326,7 +326,7 @@ public Double getExternalHeadingVelocity() {
326326
// Rotate about the z axis is the default assuming your REV Hub/Control Hub is laying
327327
// flat on a surface
328328

329-
return (double) imu.getAngularVelocity(AngleUnit.RADIANS).zRotationRate;
329+
return (double) imu.getVelocity(AngleUnit.RADIANS);
330330
}
331331

332332
public static TrajectoryVelocityConstraint getVelocityConstraint(

‎Path/src/main/java/com/technototes/path/subsystem/TwoDeadWheelLocalizer.java

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ public TwoDeadWheelLocalizer(IMU imu, MotorEncoder lr, MotorEncoder fb, DeadWhee
4949
);
5050
leftrightEncoder = lr;
5151
frontbackEncoder = fb;
52-
headingSupplier = () -> imu.gyroHeading();
52+
headingSupplier = () -> imu.getHeading();
5353

5454
lateralDistance = constants.getDouble(LateralDistance.class);
5555
forwardOffset = constants.getDouble(ForwardOffset.class);

‎RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/AdafruitIMU.java

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
package com.technototes.library.hardware.sensor;
2+
3+
import com.qualcomm.hardware.adafruit.AdafruitBNO055IMU;
4+
import com.qualcomm.hardware.adafruit.AdafruitBNO055IMUNew;
5+
import com.qualcomm.hardware.bosch.BNO055IMU;
6+
import java.util.function.DoubleSupplier;
7+
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
8+
import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
9+
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
10+
import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
11+
12+
public class AdafruitIMU extends Sensor<AdafruitBNO055IMU> implements IGyro, DoubleSupplier {
13+
14+
public enum Orientation {
15+
Yaw,
16+
Pitch,
17+
Roll,
18+
ZYaw,
19+
ZPitch,
20+
ZRoll,
21+
}
22+
23+
private final Orientation imuDirection;
24+
private double resetRadians;
25+
private AngleUnit units;
26+
27+
public AdafruitIMU(String deviceName, Orientation o) {
28+
super(deviceName);
29+
imuDirection = o;
30+
units = AngleUnit.DEGREES;
31+
this.getRawDevice().initialize(new BNO055IMU.Parameters());
32+
resetRadians = getHeadingInRadians();
33+
}
34+
35+
public AdafruitIMU(AdafruitBNO055IMU device, String deviceName, Orientation o) {
36+
super(device, deviceName);
37+
imuDirection = o;
38+
units = AngleUnit.DEGREES;
39+
this.getRawDevice().initialize(new BNO055IMU.Parameters());
40+
resetRadians = getHeadingInRadians();
41+
}
42+
43+
public AngleUnit getUnits() {
44+
return units;
45+
}
46+
47+
public void setUnits(AngleUnit u) {
48+
units = u;
49+
}
50+
51+
public double getHeading(AngleUnit u) {
52+
return u.fromUnit(AngleUnit.RADIANS, getRawValue(AngleUnit.RADIANS) - resetRadians);
53+
}
54+
55+
public void setHeading(double newHeading, AngleUnit u) {
56+
resetRadians = getHeadingInRadians() - AngleUnit.RADIANS.fromUnit(u, newHeading);
57+
}
58+
59+
public double getVelocity(AngleUnit u) {
60+
AngularVelocity av = this.getRawDevice().getAngularVelocity(u);
61+
switch (imuDirection) {
62+
case Yaw:
63+
return av.yRotationRate;
64+
case ZYaw:
65+
return -av.yRotationRate;
66+
case Pitch:
67+
return av.xRotationRate;
68+
case ZPitch:
69+
return -av.xRotationRate;
70+
case Roll:
71+
return av.zRotationRate;
72+
case ZRoll:
73+
return -av.zRotationRate;
74+
}
75+
return 0.0;
76+
}
77+
78+
private double getRawValue(AngleUnit u) {
79+
org.firstinspires.ftc.robotcore.external.navigation.Orientation ypr =
80+
this.getRawDevice().getAngularOrientation();
81+
switch (imuDirection) {
82+
case Yaw:
83+
return u.fromUnit(AngleUnit.RADIANS, ypr.secondAngle);
84+
case ZYaw:
85+
return u.fromUnit(AngleUnit.RADIANS, -ypr.secondAngle);
86+
case Pitch:
87+
return u.fromUnit(AngleUnit.RADIANS, ypr.firstAngle);
88+
case ZPitch:
89+
return u.fromUnit(AngleUnit.RADIANS, -ypr.firstAngle);
90+
case Roll:
91+
return u.fromUnit(AngleUnit.RADIANS, ypr.thirdAngle);
92+
case ZRoll:
93+
return u.fromUnit(AngleUnit.RADIANS, -ypr.thirdAngle);
94+
}
95+
return 0.0;
96+
}
97+
98+
@Override
99+
public double getAsDouble() {
100+
return getHeading();
101+
}
102+
103+
@Override
104+
public String LogLine() {
105+
return String.valueOf(getHeading());
106+
}
107+
}

‎RobotLibrary/src/main/java/com/technototes/library/hardware/sensor/IGyro.java

Lines changed: 93 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -1,41 +1,120 @@
11
package com.technototes.library.hardware.sensor;
22

3+
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
4+
35
/**
46
* An interface for a single-angle gyroscope
57
*/
68
public interface IGyro {
79
/**
8-
* The heading (in default units)
9-
*
10-
* @return the head (in default units)
10+
* Set the units for the various values
1111
*/
12-
double gyroHeading();
12+
void setUnits(AngleUnit u);
1313

1414
/**
15-
* The heading (in degrees)
16-
*
17-
* @return the head (in degrees)
15+
* Get the units the gyroscope is set to
1816
*/
19-
double gyroHeadingInDegrees();
17+
AngleUnit getUnits();
2018

2119
/**
22-
* The heading (in radians)
23-
*
24-
* @return the head (in radians units)
20+
* Get the heading (in specified units)
2521
*/
26-
double gyroHeadingInRadians();
22+
double getHeading(AngleUnit u);
2723

2824
/**
2925
* Sets the current heading (in default units)
3026
*
3127
* @param newHeading The new heading (in default units)
28+
* @param u The angle units of the new heading
29+
*/
30+
void setHeading(double newHeading, AngleUnit u);
31+
32+
/**
33+
* Gets the current angular velocity (in specified units)
3234
*/
33-
void setHeading(double newHeading);
35+
double getVelocity(AngleUnit u);
36+
37+
default void degrees() {
38+
setUnits(AngleUnit.DEGREES);
39+
}
40+
41+
default void radians() {
42+
setUnits(AngleUnit.RADIANS);
43+
}
3444

3545
/**
3646
* Zeroes the current heading (in default units)
3747
*/
3848
default void zero() {
39-
setHeading(0);
49+
setHeading(0, getUnits());
50+
}
51+
52+
/**
53+
* Get the heading (in default units)
54+
*/
55+
default double getHeading() {
56+
return getHeading(getUnits());
57+
}
58+
59+
/**
60+
* Get the heading (in degrees)
61+
*/
62+
default double getHeadingInDegrees() {
63+
return getHeading(AngleUnit.DEGREES);
64+
}
65+
66+
/**
67+
* Get the heading (in radians)
68+
*/
69+
default double getHeadingInRadians() {
70+
return getHeading(AngleUnit.RADIANS);
71+
}
72+
73+
/**
74+
* Gets the current angular velocity (in default units)
75+
*/
76+
default double getVelocity() {
77+
return getVelocity(getUnits());
78+
}
79+
80+
/**
81+
* Gets the current angular velocity (in radians)
82+
*/
83+
default double getVelocityInDegrees() {
84+
return getVelocity(AngleUnit.DEGREES);
85+
}
86+
87+
/**
88+
* Gets the current angular velocity (in degrees)
89+
*/
90+
default double getVelocityInRadians() {
91+
return getVelocity(AngleUnit.RADIANS);
92+
}
93+
94+
/**
95+
* Sets the current heading (in default units)
96+
*
97+
* @param newHeading The new heading (in default units)
98+
*/
99+
default void setHeading(double newHeading) {
100+
setHeading(newHeading, getUnits());
101+
}
102+
103+
/**
104+
* Sets the current heading (in default units)
105+
*
106+
* @param newHeading The new heading (in default units)
107+
*/
108+
default void setHeadingInDegrees(double newHeading) {
109+
setHeading(newHeading, AngleUnit.DEGREES);
110+
}
111+
112+
/**
113+
* Sets the current heading (in default units)
114+
*
115+
* @param newHeading The new heading (in default units)
116+
*/
117+
default void setHeadingInRadians(double newHeading) {
118+
setHeading(newHeading, AngleUnit.RADIANS);
40119
}
41120
}

0 commit comments

Comments
 (0)