From 375a19563e3681413a999140f8b27d0f190760f5 Mon Sep 17 00:00:00 2001 From: Joseph Date: Tue, 28 Jul 2015 11:57:22 -0400 Subject: [PATCH] Gyro: Add support for fixed calibration (artf4124). Testing added for Gyro constructors, getters, and setters. Change-Id: Id3ba2656bfdb286e01fbd95dff95115a3446c92e --- wpilibc/Athena/include/AnalogGyro.h | 5 + wpilibc/Athena/src/AnalogGyro.cpp | 110 ++++++++++++++---- .../src/TiltPanCameraTest.cpp | 58 ++++++++- .../edu/wpi/first/wpilibj/AnalogGyro.java | 93 ++++++++++++--- .../edu/wpi/first/wpilibj/AnalogInput.java | 2 +- .../java/edu/wpi/first/wpilibj/GyroTest.java | 52 ++++++++- .../fixtures/TiltPanCameraFixture.java | 29 ++++- .../edu/wpi/first/wpilibj/test/TestBench.java | 9 +- 8 files changed, 304 insertions(+), 54 deletions(-) diff --git a/wpilibc/Athena/include/AnalogGyro.h b/wpilibc/Athena/include/AnalogGyro.h index 817997f5c6..a6b38fbd0f 100644 --- a/wpilibc/Athena/include/AnalogGyro.h +++ b/wpilibc/Athena/include/AnalogGyro.h @@ -42,13 +42,18 @@ class AnalogGyro : public GyroBase { " with a channel number or passing a shared_ptr instead.") explicit AnalogGyro(AnalogInput *channel); explicit AnalogGyro(std::shared_ptr channel); + AnalogGyro(int32_t channel, uint32_t center, float offset); + AnalogGyro(std::shared_ptr channel, uint32_t center, float offset); virtual ~AnalogGyro() = default; float GetAngle() const override; double GetRate() const override; + virtual uint32_t GetCenter() const; + virtual float GetOffset() const; void SetSensitivity(float voltsPerDegreePerSecond); void SetDeadband(float volts); void Reset() override; + virtual void InitGyro(); void Calibrate() override; std::string GetSmartDashboardType() const override; diff --git a/wpilibc/Athena/src/AnalogGyro.cpp b/wpilibc/Athena/src/AnalogGyro.cpp index 7b5dbc115c..1029aeb928 100644 --- a/wpilibc/Athena/src/AnalogGyro.cpp +++ b/wpilibc/Athena/src/AnalogGyro.cpp @@ -56,32 +56,49 @@ AnalogGyro::AnalogGyro(std::shared_ptr channel) if (channel == nullptr) { wpi_setWPIError(NullParameter); } else { - if (!m_analog->IsAccumulatorChannel()) { - wpi_setWPIErrorWithContext(ParameterOutOfRange, - " channel (must be accumulator channel)"); - m_analog = nullptr; - return; - } - - m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; - m_analog->SetAverageBits(kAverageBits); - m_analog->SetOversampleBits(kOversampleBits); - float sampleRate = - kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); - m_analog->SetSampleRate(sampleRate); - Wait(0.1); - - SetDeadband(0.0f); - - SetPIDSourceType(PIDSourceType::kDisplacement); - - HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel()); - LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this); - + InitGyro(); Calibrate(); } } +/** + * Gyro constructor using the Analog Input channel number with parameters for + * presetting the center and offset values. Bypasses calibration. + * + * @param channel The analog channel the gyro is connected to. Gyros + * can only be used on on-board Analog Inputs 0-1. + * @param center Preset uncalibrated value to use as the accumulator center value. + * @param offset Preset uncalibrated value to use as the gyro offset. + */ +AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) { + m_analog = std::make_shared(channel); + InitGyro(); + m_center = center; + m_offset = offset; + m_analog->SetAccumulatorCenter(m_center); + m_analog->ResetAccumulator(); +} + +/** + * Gyro constructor with a precreated AnalogInput object and calibrated parameters. + * Use this constructor when the analog channel needs to be shared. + * This object will not clean up the AnalogInput object when using this + * constructor + * @param channel A pointer to the AnalogInput object that the gyro is + * connected to. + */ +AnalogGyro::AnalogGyro(std::shared_ptr channel, uint32_t center, float offset) : m_analog(channel) { + if (channel == nullptr) { + wpi_setWPIError(NullParameter); + } else { + InitGyro(); + m_center = center; + m_offset = offset; + m_analog->SetAccumulatorCenter(m_center); + m_analog->ResetAccumulator(); + } +} + /** * Reset the gyro. * Resets the gyro to a heading of zero. This can be used if there is @@ -93,6 +110,35 @@ void AnalogGyro::Reset() { m_analog->ResetAccumulator(); } +/** + * Initialize the gyro. Calibration is handled by Calibrate(). + */ +void AnalogGyro::InitGyro() { + if (StatusIsFatal()) return; + + if (!m_analog->IsAccumulatorChannel()) { + wpi_setWPIErrorWithContext(ParameterOutOfRange, + " channel (must be accumulator channel)"); + m_analog = nullptr; + return; + } + + m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; + m_analog->SetAverageBits(kAverageBits); + m_analog->SetOversampleBits(kOversampleBits); + float sampleRate = + kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); + m_analog->SetSampleRate(sampleRate); + Wait(0.1); + + SetDeadband(0.0f); + + SetPIDSourceType(PIDSourceType::kDisplacement); + + HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel()); + LiveWindow::GetInstance()->AddSensor("Gyro", m_analog->GetChannel(), this); +} + /** * {@inheritDoc} */ @@ -160,6 +206,26 @@ double AnalogGyro::GetRate() const { ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond); } +/** + * Return the gyro offset value. If run after calibration, + * the offset value can be used as a preset later. + * + * @return the current offset value + */ +float AnalogGyro::GetOffset() const { + return m_offset; +} + +/** + * Return the gyro center value. If run after calibration, + * the center value can be used as a preset later. + * + * @return the current center value + */ +uint32_t AnalogGyro::GetCenter() const { + return m_center; +} + /** * Set the gyro sensitivity. * This takes the number of volts/degree/second sensitivity of the gyro and uses diff --git a/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp b/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp index c95216aa59..35b8d29712 100644 --- a/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp +++ b/wpilibcIntegrationTests/src/TiltPanCameraTest.cpp @@ -21,6 +21,7 @@ static constexpr double kTiltSetpoint45 = 0.45; static constexpr double kTiltSetpoint90 = 0.68; static constexpr double kTiltTime = 1.0; static constexpr double kAccelerometerTolerance = 0.2; +static constexpr double kSensitivity = 0.013; /** * A fixture for the camera with two servos and a gyro @@ -36,7 +37,7 @@ class TiltPanCameraTest : public testing::Test { // The gyro object blocks for 5 seconds in the constructor, so only // construct it once for the whole test case m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel); - m_gyro->SetSensitivity(0.013); + m_gyro->SetSensitivity(kSensitivity); } static void TearDownTestCase() { delete m_gyro; } @@ -54,6 +55,10 @@ class TiltPanCameraTest : public testing::Test { m_gyro->Reset(); } + void DefaultGyroAngle(); + void GyroAngle(); + void GyroCalibratedParameters(); + virtual void TearDown() override { delete m_tilt; delete m_pan; @@ -66,7 +71,7 @@ AnalogGyro *TiltPanCameraTest::m_gyro = nullptr; /** * Test if the gyro angle defaults to 0 immediately after being reset. */ -TEST_F(TiltPanCameraTest, DefaultGyroAngle) { +void TiltPanCameraTest::DefaultGyroAngle() { EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f); } @@ -76,10 +81,10 @@ TEST_F(TiltPanCameraTest, DefaultGyroAngle) { * was designed for so setAngle is significantly off. This has been calibrated * for the servo on the rig. */ -TEST_F(TiltPanCameraTest, GyroAngle) { +void TiltPanCameraTest::GyroAngle() { // Make sure that the gyro doesn't get jerked when the servo goes to zero. m_pan->SetAngle(0.0); - Wait(0.25); + Wait(0.5); m_gyro->Reset(); for (int i = 0; i < 600; i++) { @@ -94,6 +99,51 @@ TEST_F(TiltPanCameraTest, GyroAngle) { << kTestAngle << " degrees"; } +/** + * Gets calibrated parameters from previously calibrated gyro, allocates a new + * gyro with the given parameters for center and offset, and re-runs tests on + * the new gyro. + */ +void TiltPanCameraTest::GyroCalibratedParameters() { + uint32_t cCenter = m_gyro->GetCenter(); + float cOffset = m_gyro->GetOffset(); + delete m_gyro; + m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel, cCenter, cOffset); + m_gyro->SetSensitivity(kSensitivity); + + // Default gyro angle test + // Accumulator needs a small amount of time to reset before being tested + m_gyro->Reset(); + Wait(.001); + EXPECT_NEAR(0.0f, m_gyro->GetAngle(), 1.0f); + + // Gyro angle test + // Make sure that the gyro doesn't get jerked when the servo goes to zero. + m_pan->SetAngle(0.0); + Wait(0.5); + m_gyro->Reset(); + + for (int i = 0; i < 600; i++) { + m_pan->Set(i / 1000.0); + Wait(0.001); + } + + double gyroAngle = m_gyro->GetAngle(); + + EXPECT_NEAR(gyroAngle, kTestAngle, 10.0) + << "Gyro measured " << gyroAngle << " degrees, servo should have turned " + << kTestAngle << " degrees"; +} + +/** + * Run all gyro tests in one function to make sure they are run in order. + */ +TEST_F(TiltPanCameraTest, TestAllGyroTests) { + DefaultGyroAngle(); + GyroAngle(); + GyroCalibratedParameters(); +} + /** * Test if the accelerometer measures gravity along the correct axes when the * camera rotates diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java index 2a22e373b0..e09fa1266c 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java @@ -39,6 +39,27 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS AccumulatorResult result; private PIDSourceType m_pidSource; + /** + * Initialize the gyro. Calibration is handled by calibrate(). + */ + public void initGyro() { + result = new AccumulatorResult(); + + m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; + m_analog.setAverageBits(kAverageBits); + m_analog.setOversampleBits(kOversampleBits); + double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); + AnalogInput.setGlobalSampleRate(sampleRate); + Timer.delay(0.1); + + setDeadband(0.0); + + setPIDSourceType(PIDSourceType.kDisplacement); + + UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel()); + LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this); + } + /** * {@inheritDoc} */ @@ -81,25 +102,45 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS if (m_analog == null) { throw new NullPointerException("AnalogInput supplied to Gyro constructor is null"); } - result = new AccumulatorResult(); - - m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond; - m_analog.setAverageBits(kAverageBits); - m_analog.setOversampleBits(kOversampleBits); - double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits)); - AnalogInput.setGlobalSampleRate(sampleRate); - Timer.delay(0.1); - - setDeadband(0.0); - - setPIDSourceType(PIDSourceType.kDisplacement); - - UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel()); - LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this); - + initGyro(); calibrate(); } + /** + * Gyro constructor using the channel number along with parameters for + * presetting the center and offset values. Bypasses calibration. + * @param channel The analog channel the gyro is connected to. Gyros can only + * be used on on-board channels 0-1. + * @param center Preset uncalibrated value to use as the accumulator center value. + * @param offset Preset uncalibrated value to use as the gyro offset. + */ + public AnalogGyro(int channel, int center, double offset) { + this(new AnalogInput(channel), center, offset); + m_channelAllocated = true; + } + + /** + * Gyro constructor with a precreated analog channel object along with + * parameters for presetting the center and offset values. Bypasses + * calibration. + * @param channel The analog channel the gyro is connected to. Gyros can only + * be used on on-board channels 0-1. + * @param center Preset uncalibrated value to use as the accumulator center value. + * @param offset Preset uncalibrated value to use as the gyro offset. + */ + public AnalogGyro(AnalogInput channel, int center, double offset) { + m_analog = channel; + if (m_analog == null) { + throw new NullPointerException("AnalogInput supplied to Gyro constructor is null"); + } + initGyro(); + m_offset = offset; + m_center = center; + + m_analog.setAccumulatorCenter(m_center); + m_analog.resetAccumulator(); + } + /** * {@inheritDoc} */ @@ -151,6 +192,26 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS } } + /** + * Return the gyro offset value set during calibration to + * use as a future preset + * + * @return the current offset value + */ + public double getOffset() { + return m_offset; + } + + /** + * Return the gyro center value set during calibration to + * use as a future preset + * + * @return the current center value + */ + public int getCenter() { + return m_center; + } + /** * Set the gyro sensitivity. This takes the number of volts/degree/second * sensitivity of the gyro and uses it in subsequent calculations to allow the diff --git a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogInput.java b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogInput.java index 008790d186..cead01a8cd 100644 --- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogInput.java +++ b/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogInput.java @@ -221,7 +221,7 @@ public class AnalogInput extends SensorBase implements PIDSource, LiveWindowSend public void initAccumulator() { if (!isAccumulatorChannel()) { throw new AllocationException("Accumulators are only available on slot " + kAccumulatorSlot - + " on channels " + kAccumulatorChannels[0] + "," + kAccumulatorChannels[1]); + + " on channels " + kAccumulatorChannels[0] + ", " + kAccumulatorChannels[1]); } m_accumulatorOffset = 0; AnalogJNI.initAccumulator(m_port); diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java index 7dd41228b4..ee84545013 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/GyroTest.java @@ -40,11 +40,17 @@ public class GyroTest extends AbstractComsSetup { @After public void tearDown() throws Exception { - tpcam.reset(); tpcam.teardown(); } @Test + public void testAllGyroTests() { + testInitial(); + testGyroAngle(); + testDeviationOverTime(); + testGyroAngleCalibratedParameters(); + } + public void testInitial() { double angle = tpcam.getGyro().getAngle(); assertEquals(errorMessage(angle, 0), 0, angle, .5); @@ -56,7 +62,6 @@ public class GyroTest extends AbstractComsSetup { * for so setAngle is significantly off. This has been calibrated for the * servo on the rig. */ - @Test public void testGyroAngle() { // Set angle for (int i = 0; i < 5; i++) { @@ -85,10 +90,9 @@ public class GyroTest extends AbstractComsSetup { assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10); } - @Test public void testDeviationOverTime() { // Make sure that the test isn't influenced by any previous motions. - Timer.delay(0.25); + Timer.delay(0.5); tpcam.getGyro().reset(); Timer.delay(0.25); double angle = tpcam.getGyro().getAngle(); @@ -98,6 +102,46 @@ public class GyroTest extends AbstractComsSetup { assertEquals("After 5 seconds " + errorMessage(angle, 0), 0, angle, 1); } + /** + * Gets calibrated parameters from already calibrated gyro, allocates a + * new gyro with the center and offset parameters, and re-runs the test. + */ + public void testGyroAngleCalibratedParameters() { + // Get calibrated parameters to make new Gyro with parameters + double cOffset = tpcam.getGyro().getOffset(); + int cCenter = tpcam.getGyro().getCenter(); + tpcam.freeGyro(); + tpcam.setupGyroParam(cCenter, cOffset); + + // Repeat tests + // Set angle + for (int i = 0; i < 5; i++) { + tpcam.getPan().set(0); + Timer.delay(.1); + } + + Timer.delay(0.5); + + // Reset for setup + tpcam.getGyroParam().reset(); + Timer.delay(0.5); + + // Perform test + for (int i = 0; i < 53; i++) { + tpcam.getPan().set(i / 100.0); + Timer.delay(0.05); + } + Timer.delay(1.2); + + double angle = tpcam.getGyroParam().getAngle(); + + double difference = TEST_ANGLE - angle; + + double diff = Math.abs(difference); + + assertEquals(errorMessage(diff, TEST_ANGLE), TEST_ANGLE, angle, 10); + } + private String errorMessage(double difference, double target) { return "Gyro angle skewed " + difference + " deg away from target " + target; } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java index ac1c1d2df4..06bda65acc 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/fixtures/TiltPanCameraFixture.java @@ -8,7 +8,7 @@ package edu.wpi.first.wpilibj.fixtures; import java.util.logging.Logger; -import edu.wpi.first.wpilibj.interfaces.Gyro; +import edu.wpi.first.wpilibj.AnalogGyro; import edu.wpi.first.wpilibj.Servo; import edu.wpi.first.wpilibj.Timer; @@ -23,13 +23,16 @@ public abstract class TiltPanCameraFixture implements ITestFixture { public static final Logger logger = Logger.getLogger(TiltPanCameraFixture.class.getName()); public static final double RESET_TIME = 2.0; - private Gyro gyro; + private AnalogGyro gyro; + private AnalogGyro gyroParam; private Servo tilt; private Servo pan; private boolean initialized = false; - protected abstract Gyro giveGyro(); + protected abstract AnalogGyro giveGyro(); + + protected abstract AnalogGyro giveGyroParam(int center, double offset); protected abstract Servo giveTilt(); @@ -73,18 +76,32 @@ public abstract class TiltPanCameraFixture implements ITestFixture { return pan; } - public Gyro getGyro() { + public AnalogGyro getGyro() { return gyro; } + public AnalogGyro getGyroParam() { + return gyroParam; + } + + // Do not call unless all other instances of Gyro have been deallocated + public void setupGyroParam(int center, double offset) { + gyroParam = giveGyroParam(center, offset); + gyroParam.reset(); + } + public void freeGyro() { + gyro.free(); + gyro = null; + } + @Override public boolean teardown() { tilt.free(); tilt = null; pan.free(); pan = null; - gyro.free(); - gyro = null; + gyroParam.free(); + gyroParam = null; return true; } diff --git a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java index 31f8b23ac7..727c61bf5d 100644 --- a/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java +++ b/wpilibjIntegrationTests/src/main/java/edu/wpi/first/wpilibj/test/TestBench.java @@ -253,12 +253,19 @@ public final class TestBench { public TiltPanCameraFixture getTiltPanCam() { TiltPanCameraFixture tpcam = new TiltPanCameraFixture() { @Override - protected Gyro giveGyro() { + protected AnalogGyro giveGyro() { AnalogGyro gyro = new AnalogGyro(kGyroChannel); gyro.setSensitivity(kGyroSensitivity); return gyro; } + @Override + protected AnalogGyro giveGyroParam(int center, double offset) { + AnalogGyro gyro = new AnalogGyro(kGyroChannel, center, offset); + gyro.setSensitivity(kGyroSensitivity); + return gyro; + } + @Override protected Servo giveTilt() { return new Servo(kTiltServoChannel);