mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Rename Gyro to AnalogGyro and make Gyro an interface.
Refactor common implementation parts of AnalogGyro into GyroBase. This will make it possible to add digital gyros in a similar way to how digital accelerometers were added. Change-Id: I437ef259e9ecb81f18a91a95c5a58b6607db5e15
This commit is contained in:
@@ -4,7 +4,7 @@
|
||||
DriveTrain::DriveTrain()
|
||||
: Subsystem("DriveTrain"), left_encoder(new Encoder(1, 2)),
|
||||
right_encoder(new Encoder(3, 4)), rangefinder(new AnalogInput(6)),
|
||||
gyro(new Gyro(1)) {
|
||||
gyro(new AnalogGyro(1)) {
|
||||
drive = new RobotDrive(new Talon(1), new Talon(2),
|
||||
new Talon(3), new Talon(4));
|
||||
|
||||
|
||||
@@ -13,7 +13,7 @@ private:
|
||||
RobotDrive* drive;
|
||||
std::shared_ptr<Encoder> left_encoder, right_encoder;
|
||||
std::shared_ptr<AnalogInput> rangefinder;
|
||||
std::shared_ptr<Gyro> gyro;
|
||||
std::shared_ptr<AnalogGyro> gyro;
|
||||
|
||||
public:
|
||||
DriveTrain();
|
||||
|
||||
@@ -27,7 +27,7 @@ class Robot: public SampleRobot {
|
||||
const double voltsPerDegreePerSecond = .0128;
|
||||
|
||||
RobotDrive myRobot;
|
||||
Gyro gyro;
|
||||
AnalogGyro gyro;
|
||||
Joystick joystick;
|
||||
|
||||
public:
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
class Robot: public SampleRobot {
|
||||
Joystick joystick;
|
||||
RobotDrive myRobot;
|
||||
Gyro gyro;
|
||||
AnalogGyro gyro;
|
||||
|
||||
//channels for motors
|
||||
const int leftMotorChannel = 1;
|
||||
|
||||
@@ -13,7 +13,7 @@ DriveTrain::DriveTrain()
|
||||
drive(frontRightCIM, backLeftCIM, frontRightCIM, backRightCIM),
|
||||
rightEncoder(new Encoder(1, 2, true, Encoder::k4X)),
|
||||
leftEncoder(new Encoder(3, 4, false, Encoder::k4X)),
|
||||
gyro(new Gyro(0)) {
|
||||
gyro(new AnalogGyro(0)) {
|
||||
// XXX: LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Left CIM", (Victor) frontLeftCIM);
|
||||
// XXX: LiveWindow::GetInstance()->AddActuator("DriveTrain", "Front Right CIM", (Victor) frontRightCIM);
|
||||
// XXX: LiveWindow::GetInstance()->AddActuator("DriveTrain", "Back Left CIM", (Victor) backLeftCIM);
|
||||
|
||||
@@ -16,7 +16,7 @@ private:
|
||||
std::shared_ptr<SpeedController> backLeftCIM, backRightCIM;
|
||||
RobotDrive drive;
|
||||
std::shared_ptr<Encoder> rightEncoder, leftEncoder;
|
||||
std::shared_ptr<Gyro> gyro;
|
||||
std::shared_ptr<AnalogGyro> gyro;
|
||||
|
||||
public:
|
||||
DriveTrain();
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
package $package.subsystems;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.AnalogInput;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Joystick.AxisType;
|
||||
import edu.wpi.first.wpilibj.Gyro;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.RobotDrive;
|
||||
import edu.wpi.first.wpilibj.SpeedController;
|
||||
@@ -25,7 +25,7 @@ public class DriveTrain extends Subsystem {
|
||||
private RobotDrive drive;
|
||||
private Encoder left_encoder, right_encoder;
|
||||
private AnalogInput rangefinder;
|
||||
private Gyro gyro;
|
||||
private AnalogGyro gyro;
|
||||
|
||||
public DriveTrain() {
|
||||
super();
|
||||
@@ -53,7 +53,7 @@ public class DriveTrain extends Subsystem {
|
||||
}
|
||||
|
||||
rangefinder = new AnalogInput(6);
|
||||
gyro = new Gyro(1);
|
||||
gyro = new AnalogGyro(1);
|
||||
|
||||
// Let's show everything on the LiveWindow
|
||||
LiveWindow.addActuator("Drive Train", "Front_Left Motor", (Talon) front_left_motor);
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
|
||||
package $package;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
import edu.wpi.first.wpilibj.Gyro;
|
||||
import edu.wpi.first.wpilibj.SampleRobot;
|
||||
import edu.wpi.first.wpilibj.RobotDrive;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
@@ -35,7 +35,7 @@ public class Robot extends SampleRobot {
|
||||
final double voltsPerDegreePerSecond = .0128;
|
||||
|
||||
RobotDrive myRobot;
|
||||
Gyro gyro;
|
||||
AnalogGyro gyro;
|
||||
Joystick joystick;
|
||||
|
||||
public Robot()
|
||||
@@ -44,7 +44,7 @@ public class Robot extends SampleRobot {
|
||||
myRobot = new RobotDrive(new CANTalon(leftMotorChannel), new CANTalon(
|
||||
leftRearMotorChannel), new CANTalon(rightMotorChannel),
|
||||
new CANTalon(rightRearMotorChannel));
|
||||
gyro = new Gyro(gyroChannel);
|
||||
gyro = new AnalogGyro(gyroChannel);
|
||||
joystick = new Joystick(joystickChannel);
|
||||
}
|
||||
|
||||
|
||||
@@ -1,8 +1,8 @@
|
||||
|
||||
package $package;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.CANTalon;
|
||||
import edu.wpi.first.wpilibj.Gyro;
|
||||
import edu.wpi.first.wpilibj.SampleRobot;
|
||||
import edu.wpi.first.wpilibj.RobotDrive;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
@@ -20,7 +20,7 @@ import edu.wpi.first.wpilibj.RobotDrive.MotorType;
|
||||
public class Robot extends SampleRobot {
|
||||
RobotDrive myRobot;
|
||||
Joystick joystick;
|
||||
Gyro gyro;
|
||||
AnalogGyro gyro;
|
||||
|
||||
//channels for motors
|
||||
final int leftMotorChannel = 1;
|
||||
@@ -42,7 +42,7 @@ public class Robot extends SampleRobot {
|
||||
myRobot.setInvertedMotor(MotorType.kRearLeft, true); // you may need to change or remove this to match your robot
|
||||
|
||||
joystick = new Joystick(0);
|
||||
gyro = new Gyro(gyroChannel);
|
||||
gyro = new AnalogGyro(gyroChannel);
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@@ -3,9 +3,9 @@ package $package.subsystems;
|
||||
import $package.Robot;
|
||||
import $package.commands.DriveWithJoystick;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
|
||||
import edu.wpi.first.wpilibj.Encoder;
|
||||
import edu.wpi.first.wpilibj.Gyro;
|
||||
import edu.wpi.first.wpilibj.Joystick;
|
||||
import edu.wpi.first.wpilibj.PIDSource.PIDSourceParameter;
|
||||
import edu.wpi.first.wpilibj.RobotDrive;
|
||||
@@ -24,7 +24,7 @@ public class DriveTrain extends Subsystem {
|
||||
private SpeedController backLeftCIM, backRightCIM;
|
||||
private RobotDrive drive;
|
||||
private Encoder rightEncoder, leftEncoder;
|
||||
private Gyro gyro;
|
||||
private AnalogGyro gyro;
|
||||
|
||||
public DriveTrain() {
|
||||
// Configure drive motors
|
||||
@@ -67,7 +67,7 @@ public class DriveTrain extends Subsystem {
|
||||
LiveWindow.addSensor("DriveTrain", "Left Encoder", leftEncoder);
|
||||
|
||||
// Configure gyro
|
||||
gyro = new Gyro(2);
|
||||
gyro = new AnalogGyro(2);
|
||||
if (Robot.isReal()) {
|
||||
gyro.setSensitivity(0.007); // TODO: Handle more gracefully?
|
||||
}
|
||||
@@ -124,4 +124,4 @@ public class DriveTrain extends Subsystem {
|
||||
public double getAngle() {
|
||||
return gyro.getAngle();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
54
wpilibc/wpilibC++/include/interfaces/Gyro.h
Normal file
54
wpilibc/wpilibC++/include/interfaces/Gyro.h
Normal file
@@ -0,0 +1,54 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2014. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
#pragma once
|
||||
|
||||
/**
|
||||
* Interface for yaw rate gyros
|
||||
*/
|
||||
class Gyro {
|
||||
public:
|
||||
virtual ~Gyro() = default;
|
||||
|
||||
/**
|
||||
* Initialize the gyro. Calibrate the gyro by running for a number of samples
|
||||
* and computing the center value. Then use the center value as the
|
||||
* Accumulator center value for subsequent measurements. It's important to
|
||||
* make sure that the robot is not moving while the centering calculations are
|
||||
* in progress, this is typically done when the robot is first turned on while
|
||||
* it's sitting at rest before the competition starts.
|
||||
*/
|
||||
virtual void InitGyro() = 0;
|
||||
|
||||
/**
|
||||
* Reset the gyro. Resets the gyro to a heading of zero. This can be used if
|
||||
* there is significant drift in the gyro and it needs to be recalibrated
|
||||
* after it has been running.
|
||||
*/
|
||||
virtual void Reset() = 0;
|
||||
|
||||
/**
|
||||
* Return the actual angle in degrees that the robot is currently facing.
|
||||
*
|
||||
* The angle is based on the current accumulator value corrected by the
|
||||
* oversampling rate, the gyro type and the A/D calibration values. The angle
|
||||
* is continuous, that is it will continue from 360 to 361 degrees. This
|
||||
* allows algorithms that wouldn't want to see a discontinuity in the gyro
|
||||
* output as it sweeps past from 360 to 0 on the second time around.
|
||||
*
|
||||
* @return the current heading of the robot in degrees. This heading is based
|
||||
* on integration of the returned rate from the gyro.
|
||||
*/
|
||||
virtual float GetAngle() const = 0;
|
||||
|
||||
/**
|
||||
* Return the rate of rotation of the gyro
|
||||
*
|
||||
* The rate is based on the most recent reading of the gyro analog value
|
||||
*
|
||||
* @return the current rate in degrees per second
|
||||
*/
|
||||
virtual double GetRate() const = 0;
|
||||
};
|
||||
@@ -6,11 +6,7 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
#pragma once
|
||||
|
||||
#include "SensorBase.h"
|
||||
#include "PIDSource.h"
|
||||
#include "LiveWindow/LiveWindowSendable.h"
|
||||
|
||||
#include <memory>
|
||||
#include "GyroBase.h"
|
||||
|
||||
class AnalogInput;
|
||||
|
||||
@@ -29,8 +25,10 @@ class AnalogInput;
|
||||
* with a channel that is assigned one of the Analog accumulators from the FPGA.
|
||||
* See
|
||||
* AnalogInput for the current accumulator assignments.
|
||||
*
|
||||
* This class is for gyro sensors that connect to an analog input.
|
||||
*/
|
||||
class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
|
||||
class AnalogGyro : public GyroBase {
|
||||
public:
|
||||
static const uint32_t kOversampleBits = 10;
|
||||
static const uint32_t kAverageBits = 0;
|
||||
@@ -38,29 +36,22 @@ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
|
||||
static constexpr float kCalibrationSampleTime = 5.0;
|
||||
static constexpr float kDefaultVoltsPerDegreePerSecond = 0.007;
|
||||
|
||||
explicit Gyro(int32_t channel);
|
||||
explicit AnalogGyro(int32_t channel);
|
||||
DEPRECATED(
|
||||
"Raw pointers are deprecated; consider calling the Gyro constructor with "
|
||||
"a channel number or passing a shared_ptr instead.")
|
||||
explicit Gyro(AnalogInput *channel);
|
||||
explicit Gyro(std::shared_ptr<AnalogInput> channel);
|
||||
virtual ~Gyro() = default;
|
||||
virtual float GetAngle() const;
|
||||
virtual double GetRate() const;
|
||||
"Raw pointers are deprecated; consider calling the AnalogGyro constructor"
|
||||
" with a channel number or passing a shared_ptr instead.")
|
||||
explicit AnalogGyro(AnalogInput *channel);
|
||||
explicit AnalogGyro(std::shared_ptr<AnalogInput> channel);
|
||||
virtual ~AnalogGyro() = default;
|
||||
|
||||
float GetAngle() const override;
|
||||
double GetRate() const override;
|
||||
void SetSensitivity(float voltsPerDegreePerSecond);
|
||||
void SetDeadband(float volts);
|
||||
virtual void Reset();
|
||||
void InitGyro();
|
||||
void Reset() override;
|
||||
void InitGyro() override;
|
||||
|
||||
// PIDSource interface
|
||||
double PIDGet() override;
|
||||
|
||||
void UpdateTable() override;
|
||||
void StartLiveWindowMode() override;
|
||||
void StopLiveWindowMode() override;
|
||||
std::string GetSmartDashboardType() const override;
|
||||
void InitTable(std::shared_ptr<ITable> subTable) override;
|
||||
std::shared_ptr<ITable> GetTable() const override;
|
||||
|
||||
protected:
|
||||
std::shared_ptr<AnalogInput> m_analog;
|
||||
@@ -69,6 +60,4 @@ class Gyro : public SensorBase, public PIDSource, public LiveWindowSendable {
|
||||
float m_voltsPerDegreePerSecond;
|
||||
float m_offset;
|
||||
uint32_t m_center;
|
||||
|
||||
std::shared_ptr<ITable> m_table = nullptr;
|
||||
};
|
||||
36
wpilibc/wpilibC++Devices/include/GyroBase.h
Normal file
36
wpilibc/wpilibC++Devices/include/GyroBase.h
Normal file
@@ -0,0 +1,36 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2008. All Rights Reserved.
|
||||
*/
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
#pragma once
|
||||
|
||||
#include "SensorBase.h"
|
||||
#include "PIDSource.h"
|
||||
#include "interfaces/Gyro.h"
|
||||
#include "LiveWindow/LiveWindowSendable.h"
|
||||
|
||||
#include <memory>
|
||||
|
||||
/**
|
||||
* GyroBase is the common base class for Gyro implementations such as
|
||||
* AnalogGyro.
|
||||
*/
|
||||
class GyroBase : public Gyro, public SensorBase, public PIDSource, public LiveWindowSendable {
|
||||
public:
|
||||
virtual ~GyroBase() = default;
|
||||
|
||||
// PIDSource interface
|
||||
double PIDGet() override;
|
||||
|
||||
void UpdateTable() override;
|
||||
void StartLiveWindowMode() override;
|
||||
void StopLiveWindowMode() override;
|
||||
std::string GetSmartDashboardType() const override;
|
||||
void InitTable(std::shared_ptr<ITable> subTable) override;
|
||||
std::shared_ptr<ITable> GetTable() const override;
|
||||
|
||||
private:
|
||||
std::shared_ptr<ITable> m_table;
|
||||
};
|
||||
@@ -14,6 +14,7 @@
|
||||
#include "ADXL345_I2C.h"
|
||||
#include "ADXL345_SPI.h"
|
||||
#include "AnalogAccelerometer.h"
|
||||
#include "AnalogGyro.h"
|
||||
#include "AnalogInput.h"
|
||||
#include "AnalogOutput.h"
|
||||
#include "AnalogPotentiometer.h"
|
||||
@@ -49,8 +50,8 @@
|
||||
#include "ErrorBase.h"
|
||||
#include "GearTooth.h"
|
||||
#include "GenericHID.h"
|
||||
#include "Gyro.h"
|
||||
#include "interfaces/Accelerometer.h"
|
||||
#include "interfaces/Gyro.h"
|
||||
#include "interfaces/Potentiometer.h"
|
||||
#include "I2C.h"
|
||||
#include "IterativeRobot.h"
|
||||
|
||||
@@ -5,18 +5,18 @@
|
||||
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "Gyro.h"
|
||||
#include "AnalogGyro.h"
|
||||
#include "AnalogInput.h"
|
||||
//#include "NetworkCommunication/UsageReporting.h"
|
||||
#include "Timer.h"
|
||||
#include "WPIErrors.h"
|
||||
#include "LiveWindow/LiveWindow.h"
|
||||
#include <climits>
|
||||
const uint32_t Gyro::kOversampleBits;
|
||||
const uint32_t Gyro::kAverageBits;
|
||||
constexpr float Gyro::kSamplesPerSecond;
|
||||
constexpr float Gyro::kCalibrationSampleTime;
|
||||
constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;
|
||||
const uint32_t AnalogGyro::kOversampleBits;
|
||||
const uint32_t AnalogGyro::kAverageBits;
|
||||
constexpr float AnalogGyro::kSamplesPerSecond;
|
||||
constexpr float AnalogGyro::kCalibrationSampleTime;
|
||||
constexpr float AnalogGyro::kDefaultVoltsPerDegreePerSecond;
|
||||
|
||||
/**
|
||||
* Initialize the gyro.
|
||||
@@ -30,7 +30,7 @@ constexpr float Gyro::kDefaultVoltsPerDegreePerSecond;
|
||||
* it's sitting at
|
||||
* rest before the competition starts.
|
||||
*/
|
||||
void Gyro::InitGyro() {
|
||||
void AnalogGyro::InitGyro() {
|
||||
if (!m_analog->IsAccumulatorChannel()) {
|
||||
wpi_setWPIErrorWithContext(ParameterOutOfRange,
|
||||
" channel (must be accumulator channel)");
|
||||
@@ -74,7 +74,7 @@ void Gyro::InitGyro() {
|
||||
* @param channel The analog channel the gyro is connected to. Gyros
|
||||
can only be used on on-board Analog Inputs 0-1.
|
||||
*/
|
||||
Gyro::Gyro(int32_t channel) {
|
||||
AnalogGyro::AnalogGyro(int32_t channel) {
|
||||
m_analog = std::make_shared<AnalogInput>(channel);
|
||||
InitGyro();
|
||||
}
|
||||
@@ -91,9 +91,9 @@ Gyro::Gyro(int32_t channel) {
|
||||
DEPRECATED(
|
||||
"Raw pointers are deprecated; consider calling the Gyro constructor with "
|
||||
"a channel number or passing a shared_ptr instead.")
|
||||
Gyro::Gyro(AnalogInput *channel)
|
||||
: Gyro(std::shared_ptr<AnalogInput>(channel,
|
||||
NullDeleter<AnalogInput>())) {}
|
||||
AnalogGyro::AnalogGyro(AnalogInput *channel)
|
||||
: AnalogGyro(
|
||||
std::shared_ptr<AnalogInput>(channel, NullDeleter<AnalogInput>())) {}
|
||||
|
||||
/**
|
||||
* Gyro constructor with a precreated AnalogInput object.
|
||||
@@ -103,7 +103,8 @@ Gyro::Gyro(AnalogInput *channel)
|
||||
* @param channel A pointer to the AnalogInput object that the gyro is
|
||||
* connected to.
|
||||
*/
|
||||
Gyro::Gyro(std::shared_ptr<AnalogInput> channel) : m_analog(channel) {
|
||||
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
|
||||
: m_analog(channel) {
|
||||
if (channel == nullptr) {
|
||||
wpi_setWPIError(NullParameter);
|
||||
} else {
|
||||
@@ -117,7 +118,7 @@ Gyro::Gyro(std::shared_ptr<AnalogInput> channel) : m_analog(channel) {
|
||||
* significant
|
||||
* drift in the gyro and it needs to be recalibrated after it has been running.
|
||||
*/
|
||||
void Gyro::Reset() { m_analog->ResetAccumulator(); }
|
||||
void AnalogGyro::Reset() { m_analog->ResetAccumulator(); }
|
||||
|
||||
/**
|
||||
* Return the actual angle in degrees that the robot is currently facing.
|
||||
@@ -134,7 +135,7 @@ void Gyro::Reset() { m_analog->ResetAccumulator(); }
|
||||
* integration
|
||||
* of the returned rate from the gyro.
|
||||
*/
|
||||
float Gyro::GetAngle() const {
|
||||
float AnalogGyro::GetAngle() const {
|
||||
int64_t rawValue;
|
||||
uint32_t count;
|
||||
m_analog->GetAccumulatorOutput(rawValue, count);
|
||||
@@ -155,7 +156,7 @@ float Gyro::GetAngle() const {
|
||||
*
|
||||
* @return the current rate in degrees per second
|
||||
*/
|
||||
double Gyro::GetRate() const {
|
||||
double AnalogGyro::GetRate() const {
|
||||
return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 *
|
||||
m_analog->GetLSBWeight() /
|
||||
((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
|
||||
@@ -171,7 +172,7 @@ double Gyro::GetRate() const {
|
||||
*
|
||||
* @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second
|
||||
*/
|
||||
void Gyro::SetSensitivity(float voltsPerDegreePerSecond) {
|
||||
void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) {
|
||||
m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
|
||||
}
|
||||
|
||||
@@ -183,44 +184,10 @@ void Gyro::SetSensitivity(float voltsPerDegreePerSecond) {
|
||||
*
|
||||
* @param volts The size of the deadband in volts
|
||||
*/
|
||||
void Gyro::SetDeadband(float volts) {
|
||||
void AnalogGyro::SetDeadband(float volts) {
|
||||
int32_t deadband = volts * 1e9 / m_analog->GetLSBWeight() *
|
||||
(1 << m_analog->GetOversampleBits());
|
||||
m_analog->SetAccumulatorDeadband(deadband);
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the PIDOutput for the PIDSource base object. Can be set to return
|
||||
* angle or rate using SetPIDSourceType(). Defaults to angle.
|
||||
*
|
||||
* @return The PIDOutput (angle or rate, defaults to angle)
|
||||
*/
|
||||
double Gyro::PIDGet() {
|
||||
switch (GetPIDSourceType()) {
|
||||
case PIDSourceType::kRate:
|
||||
return GetRate();
|
||||
case PIDSourceType::kDisplacement:
|
||||
return GetAngle();
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void Gyro::UpdateTable() {
|
||||
if (m_table != nullptr) {
|
||||
m_table->PutNumber("Value", GetAngle());
|
||||
}
|
||||
}
|
||||
|
||||
void Gyro::StartLiveWindowMode() {}
|
||||
|
||||
void Gyro::StopLiveWindowMode() {}
|
||||
|
||||
std::string Gyro::GetSmartDashboardType() const { return "Gyro"; }
|
||||
|
||||
void Gyro::InitTable(std::shared_ptr<ITable> subTable) {
|
||||
m_table = subTable;
|
||||
UpdateTable();
|
||||
}
|
||||
|
||||
std::shared_ptr<ITable> Gyro::GetTable() const { return m_table; }
|
||||
std::string AnalogGyro::GetSmartDashboardType() const { return "AnalogGyro"; }
|
||||
46
wpilibc/wpilibC++Devices/src/GyroBase.cpp
Normal file
46
wpilibc/wpilibC++Devices/src/GyroBase.cpp
Normal file
@@ -0,0 +1,46 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2008. All Rights Reserved.
|
||||
*/
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include "GyroBase.h"
|
||||
#include "WPIErrors.h"
|
||||
#include "LiveWindow/LiveWindow.h"
|
||||
|
||||
/**
|
||||
* Get the PIDOutput for the PIDSource base object. Can be set to return
|
||||
* angle or rate using SetPIDSourceType(). Defaults to angle.
|
||||
*
|
||||
* @return The PIDOutput (angle or rate, defaults to angle)
|
||||
*/
|
||||
double GyroBase::PIDGet() {
|
||||
switch (GetPIDSourceType()) {
|
||||
case PIDSourceType::kRate:
|
||||
return GetRate();
|
||||
case PIDSourceType::kDisplacement:
|
||||
return GetAngle();
|
||||
default:
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
void GyroBase::UpdateTable() {
|
||||
if (m_table != nullptr) {
|
||||
m_table->PutNumber("Value", GetAngle());
|
||||
}
|
||||
}
|
||||
|
||||
void GyroBase::StartLiveWindowMode() {}
|
||||
|
||||
void GyroBase::StopLiveWindowMode() {}
|
||||
|
||||
std::string GyroBase::GetSmartDashboardType() const { return "Gyro"; }
|
||||
|
||||
void GyroBase::InitTable(std::shared_ptr<ITable> subTable) {
|
||||
m_table = subTable;
|
||||
UpdateTable();
|
||||
}
|
||||
|
||||
std::shared_ptr<ITable> GyroBase::GetTable() const { return m_table; }
|
||||
@@ -6,7 +6,7 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
|
||||
#include <ADXL345_SPI.h>
|
||||
#include <Gyro.h>
|
||||
#include <AnalogGyro.h>
|
||||
#include <Servo.h>
|
||||
#include <Timer.h>
|
||||
#include "gtest/gtest.h"
|
||||
@@ -28,14 +28,14 @@ static constexpr double kAccelerometerTolerance = 0.2;
|
||||
*/
|
||||
class TiltPanCameraTest : public testing::Test {
|
||||
protected:
|
||||
static Gyro *m_gyro;
|
||||
static AnalogGyro *m_gyro;
|
||||
Servo *m_tilt, *m_pan;
|
||||
Accelerometer *m_spiAccel;
|
||||
|
||||
static void SetUpTestCase() {
|
||||
// The gyro object blocks for 5 seconds in the constructor, so only
|
||||
// construct it once for the whole test case
|
||||
m_gyro = new Gyro(TestBench::kCameraGyroChannel);
|
||||
m_gyro = new AnalogGyro(TestBench::kCameraGyroChannel);
|
||||
m_gyro->SetSensitivity(0.013);
|
||||
}
|
||||
|
||||
@@ -61,7 +61,7 @@ class TiltPanCameraTest : public testing::Test {
|
||||
}
|
||||
};
|
||||
|
||||
Gyro *TiltPanCameraTest::m_gyro = nullptr;
|
||||
AnalogGyro *TiltPanCameraTest::m_gyro = nullptr;
|
||||
|
||||
/**
|
||||
* Test if the gyro angle defaults to 0 immediately after being reset.
|
||||
|
||||
@@ -0,0 +1,56 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2014. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
package edu.wpi.first.wpilibj.interfaces;
|
||||
|
||||
/**
|
||||
* Interface for yaw rate gyros
|
||||
*/
|
||||
public interface Gyro {
|
||||
/**
|
||||
* Initialize the gyro. Calibrate the gyro by running for a number of samples
|
||||
* and computing the center value. Then use the center value as the
|
||||
* Accumulator center value for subsequent measurements. It's important to
|
||||
* make sure that the robot is not moving while the centering calculations are
|
||||
* in progress, this is typically done when the robot is first turned on while
|
||||
* it's sitting at rest before the competition starts.
|
||||
*/
|
||||
public void initGyro();
|
||||
|
||||
/**
|
||||
* Reset the gyro. Resets the gyro to a heading of zero. This can be used if
|
||||
* there is significant drift in the gyro and it needs to be recalibrated
|
||||
* after it has been running.
|
||||
*/
|
||||
public void reset();
|
||||
|
||||
/**
|
||||
* Return the actual angle in degrees that the robot is currently facing.
|
||||
*
|
||||
* The angle is based on the current accumulator value corrected by the
|
||||
* oversampling rate, the gyro type and the A/D calibration values. The angle
|
||||
* is continuous, that is it will continue from 360 to 361 degrees. This
|
||||
* allows algorithms that wouldn't want to see a discontinuity in the gyro
|
||||
* output as it sweeps past from 360 to 0 on the second time around.
|
||||
*
|
||||
* @return the current heading of the robot in degrees. This heading is based
|
||||
* on integration of the returned rate from the gyro.
|
||||
*/
|
||||
public double getAngle();
|
||||
|
||||
/**
|
||||
* Return the rate of rotation of the gyro
|
||||
*
|
||||
* The rate is based on the most recent reading of the gyro analog value
|
||||
*
|
||||
* @return the current rate in degrees per second
|
||||
*/
|
||||
public double getRate();
|
||||
|
||||
/**
|
||||
* Free the resources used by the gyro
|
||||
*/
|
||||
public void free();
|
||||
}
|
||||
@@ -8,6 +8,7 @@ package edu.wpi.first.wpilibj;
|
||||
|
||||
import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
|
||||
import edu.wpi.first.wpilibj.communication.UsageReporting;
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
||||
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
|
||||
import edu.wpi.first.wpilibj.tables.ITable;
|
||||
@@ -20,8 +21,10 @@ import edu.wpi.first.wpilibj.tables.ITable;
|
||||
* short calibration routine where it samples the gyro while at rest to
|
||||
* determine the default offset. This is subtracted from each sample to
|
||||
* determine the heading.
|
||||
*
|
||||
* This class is for gyro sensors that connect to an analog input.
|
||||
*/
|
||||
public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable {
|
||||
|
||||
static final int kOversampleBits = 10;
|
||||
static final int kAverageBits = 0;
|
||||
@@ -37,12 +40,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
private PIDSourceType m_pidSource;
|
||||
|
||||
/**
|
||||
* Initialize the gyro. Calibrate the gyro by running for a number of samples
|
||||
* and computing the center value. Then use the center value as the
|
||||
* Accumulator center value for subsequent measurements. It's important to
|
||||
* make sure that the robot is not moving while the centering calculations are
|
||||
* in progress, this is typically done when the robot is first turned on while
|
||||
* it's sitting at rest before the competition starts.
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public void initGyro() {
|
||||
result = new AccumulatorResult();
|
||||
@@ -75,7 +73,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
setPIDSourceType(PIDSourceType.kDisplacement);
|
||||
|
||||
UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
|
||||
LiveWindow.addSensor("Gyro", m_analog.getChannel(), this);
|
||||
LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
|
||||
}
|
||||
|
||||
/**
|
||||
@@ -84,7 +82,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
* @param channel The analog channel the gyro is connected to. Gyros can only
|
||||
* be used on on-board channels 0-1.
|
||||
*/
|
||||
public Gyro(int channel) {
|
||||
public AnalogGyro(int channel) {
|
||||
this(new AnalogInput(channel));
|
||||
m_channelAllocated = true;
|
||||
}
|
||||
@@ -96,7 +94,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
* @param channel The AnalogInput object that the gyro is connected to. Gyros
|
||||
* can only be used on on-board channels 0-1.
|
||||
*/
|
||||
public Gyro(AnalogInput channel) {
|
||||
public AnalogGyro(AnalogInput channel) {
|
||||
m_analog = channel;
|
||||
if (m_analog == null) {
|
||||
throw new NullPointerException("AnalogInput supplied to Gyro constructor is null");
|
||||
@@ -105,9 +103,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
}
|
||||
|
||||
/**
|
||||
* Reset the gyro. Resets the gyro to a heading of zero. This can be used if
|
||||
* there is significant drift in the gyro and it needs to be recalibrated
|
||||
* after it has been running.
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public void reset() {
|
||||
if (m_analog != null) {
|
||||
@@ -127,16 +123,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the actual angle in degrees that the robot is currently facing.
|
||||
*
|
||||
* The angle is based on the current accumulator value corrected by the
|
||||
* oversampling rate, the gyro type and the A/D calibration values. The angle
|
||||
* is continuous, that is it will continue from 360 to 361 degrees. This
|
||||
* allows algorithms that wouldn't want to see a discontinuity in the gyro
|
||||
* output as it sweeps past from 360 to 0 on the second time around.
|
||||
*
|
||||
* @return the current heading of the robot in degrees. This heading is based
|
||||
* on integration of the returned rate from the gyro.
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public double getAngle() {
|
||||
if (m_analog == null) {
|
||||
@@ -155,11 +142,7 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
}
|
||||
|
||||
/**
|
||||
* Return the rate of rotation of the gyro
|
||||
*
|
||||
* The rate is based on the most recent reading of the gyro analog value
|
||||
*
|
||||
* @return the current rate in degrees per second
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public double getRate() {
|
||||
if (m_analog == null) {
|
||||
@@ -196,87 +179,11 @@ public class Gyro extends SensorBase implements PIDSource, LiveWindowSendable {
|
||||
m_analog.setAccumulatorDeadband(deadband);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set which parameter of the gyro you are using as a process control
|
||||
* variable. The Gyro class supports the rate and displacement parameters
|
||||
*
|
||||
* @param pidSource An enum to select the parameter.
|
||||
*/
|
||||
public void setPIDSourceType(PIDSourceType pidSource) {
|
||||
m_pidSource = pidSource;
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public PIDSourceType getPIDSourceType() {
|
||||
return m_pidSource;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the output of the gyro for use with PIDControllers. May be the angle or
|
||||
* rate depending on the set PIDSourceType
|
||||
*
|
||||
* @return the output according to the gyro
|
||||
*/
|
||||
@Override
|
||||
public double pidGet() {
|
||||
switch (m_pidSource) {
|
||||
case kRate:
|
||||
return getRate();
|
||||
case kDisplacement:
|
||||
return getAngle();
|
||||
default:
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Live Window code, only does anything if live window is activated.
|
||||
*/
|
||||
@Override
|
||||
public String getSmartDashboardType() {
|
||||
return "Gyro";
|
||||
return "AnalogGyro";
|
||||
}
|
||||
|
||||
private ITable m_table;
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void initTable(ITable subtable) {
|
||||
m_table = subtable;
|
||||
updateTable();
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public ITable getTable() {
|
||||
return m_table;
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void updateTable() {
|
||||
if (m_table != null) {
|
||||
m_table.putNumber("Value", getAngle());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void startLiveWindowMode() {}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void stopLiveWindowMode() {}
|
||||
}
|
||||
@@ -0,0 +1,124 @@
|
||||
/*----------------------------------------------------------------------------*/
|
||||
/* Copyright (c) FIRST 2008-2012. All Rights Reserved. */
|
||||
/* Open Source Software - may be modified and shared by FRC teams. The code */
|
||||
/* must be accompanied by the FIRST BSD license file in the root directory of */
|
||||
/* the project. */
|
||||
/*----------------------------------------------------------------------------*/
|
||||
package edu.wpi.first.wpilibj;
|
||||
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
|
||||
import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
|
||||
import edu.wpi.first.wpilibj.tables.ITable;
|
||||
|
||||
/**
|
||||
* GyroBase is the common base class for Gyro implementations such as
|
||||
* AnalogGyro.
|
||||
*/
|
||||
public abstract class GyroBase extends SensorBase implements Gyro, PIDSource, LiveWindowSendable {
|
||||
private PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public abstract void initGyro();
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public abstract void reset();
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public abstract double getAngle();
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public abstract double getRate();
|
||||
|
||||
/**
|
||||
* Set which parameter of the gyro you are using as a process control
|
||||
* variable. The Gyro class supports the rate and displacement parameters
|
||||
*
|
||||
* @param pidSource An enum to select the parameter.
|
||||
*/
|
||||
public void setPIDSourceType(PIDSourceType pidSource) {
|
||||
m_pidSource = pidSource;
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
public PIDSourceType getPIDSourceType() {
|
||||
return m_pidSource;
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the output of the gyro for use with PIDControllers. May be the angle or
|
||||
* rate depending on the set PIDSourceType
|
||||
*
|
||||
* @return the output according to the gyro
|
||||
*/
|
||||
@Override
|
||||
public double pidGet() {
|
||||
switch (m_pidSource) {
|
||||
case kRate:
|
||||
return getRate();
|
||||
case kDisplacement:
|
||||
return getAngle();
|
||||
default:
|
||||
return 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* Live Window code, only does anything if live window is activated.
|
||||
*/
|
||||
@Override
|
||||
public String getSmartDashboardType() {
|
||||
return "Gyro";
|
||||
}
|
||||
|
||||
private ITable m_table;
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void initTable(ITable subtable) {
|
||||
m_table = subtable;
|
||||
updateTable();
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public ITable getTable() {
|
||||
return m_table;
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void updateTable() {
|
||||
if (m_table != null) {
|
||||
m_table.putNumber("Value", getAngle());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void startLiveWindowMode() {}
|
||||
|
||||
/**
|
||||
* {@inheritDoc}
|
||||
*/
|
||||
@Override
|
||||
public void stopLiveWindowMode() {}
|
||||
}
|
||||
@@ -8,7 +8,7 @@ package edu.wpi.first.wpilibj.fixtures;
|
||||
|
||||
import java.util.logging.Logger;
|
||||
|
||||
import edu.wpi.first.wpilibj.Gyro;
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
import edu.wpi.first.wpilibj.Timer;
|
||||
|
||||
|
||||
@@ -12,12 +12,12 @@ import java.util.Arrays;
|
||||
import java.util.Collection;
|
||||
import java.util.List;
|
||||
|
||||
import edu.wpi.first.wpilibj.AnalogGyro;
|
||||
import edu.wpi.first.wpilibj.AnalogInput;
|
||||
import edu.wpi.first.wpilibj.AnalogOutput;
|
||||
import edu.wpi.first.wpilibj.CANJaguar;
|
||||
import edu.wpi.first.wpilibj.DigitalInput;
|
||||
import edu.wpi.first.wpilibj.DigitalOutput;
|
||||
import edu.wpi.first.wpilibj.Gyro;
|
||||
import edu.wpi.first.wpilibj.Jaguar;
|
||||
import edu.wpi.first.wpilibj.Relay;
|
||||
import edu.wpi.first.wpilibj.Servo;
|
||||
@@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj.fixtures.DIOCrossConnectFixture;
|
||||
import edu.wpi.first.wpilibj.fixtures.MotorEncoderFixture;
|
||||
import edu.wpi.first.wpilibj.fixtures.RelayCrossConnectFixture;
|
||||
import edu.wpi.first.wpilibj.fixtures.TiltPanCameraFixture;
|
||||
import edu.wpi.first.wpilibj.interfaces.Gyro;
|
||||
import edu.wpi.first.wpilibj.mockhardware.FakePotentiometerSource;
|
||||
|
||||
/**
|
||||
@@ -253,7 +254,7 @@ public final class TestBench {
|
||||
TiltPanCameraFixture tpcam = new TiltPanCameraFixture() {
|
||||
@Override
|
||||
protected Gyro giveGyro() {
|
||||
Gyro gyro = new Gyro(kGyroChannel);
|
||||
AnalogGyro gyro = new AnalogGyro(kGyroChannel);
|
||||
gyro.setSensitivity(kGyroSensitivity);
|
||||
return gyro;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user