Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-27 02:01:42 +00:00
Code Issues Packages Projects Releases Wiki Activity

Add gyro and accelerometer simulator wrappers (#1183)

Browse Source
This commit is contained in:
PJ Reiniger
2018-08-02 00:39:20 -04:00
committed by Peter Johnson
parent 0a0d9245e2
commit c2ceebfb9c
19 changed files with 663 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/ADXLThreeAxisAccelerometerSim.cpp Normal file
View File

@@ -0,0 +1,52 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#include "lowfisim/wpisimulators/ADXLThreeAxisAccelerometerSim.h"
namespace frc {
namespace sim {
namespace lowfi {
ADXLThreeAxisAccelerometerSim::ADXLThreeAxisAccelerometerSim(
hal::ThreeAxisAccelerometerData& accelerometerWrapper)
: m_accelerometerWrapper(accelerometerWrapper),
m_xWrapper(std::function<void(double)>(
std::bind(&hal::ThreeAxisAccelerometerData::SetX,
&m_accelerometerWrapper, std::placeholders::_1)),
std::function<double(void)>(
std::bind(&hal::ThreeAxisAccelerometerData::GetX,
&m_accelerometerWrapper))),
m_yWrapper(std::function<void(double)>(
std::bind(&hal::ThreeAxisAccelerometerData::SetY,
&m_accelerometerWrapper, std::placeholders::_1)),
std::function<double(void)>(
std::bind(&hal::ThreeAxisAccelerometerData::GetY,
&m_accelerometerWrapper))),
m_zWrapper(std::function<void(double)>(
std::bind(&hal::ThreeAxisAccelerometerData::SetZ,
&m_accelerometerWrapper, std::placeholders::_1)),
std::function<double(void)>(
std::bind(&hal::ThreeAxisAccelerometerData::GetZ,
&m_accelerometerWrapper))) {}
AccelerometerSim& ADXLThreeAxisAccelerometerSim::GetXWrapper() {
return m_xWrapper;
}
AccelerometerSim& ADXLThreeAxisAccelerometerSim::GetYWrapper() {
return m_yWrapper;
}
AccelerometerSim& ADXLThreeAxisAccelerometerSim::GetZWrapper() {
return m_zWrapper;
}
} // namespace lowfi
} // namespace sim
} // namespace frc

25
simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/ADXRS450_SpiGyroSim.cpp Normal file
View File

@@ -0,0 +1,25 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#include "lowfisim/wpisimulators/ADXRS450_SpiGyroSim.h"
namespace frc {
namespace sim {
namespace lowfi {
ADXRS450_SpiGyroSim::ADXRS450_SpiGyroSim(int spiPort)
: m_gyroWrapper(spiPort) {}
void ADXRS450_SpiGyroSim::SetAngle(double angle) {
m_gyroWrapper.SetAngle(angle);
}
double ADXRS450_SpiGyroSim::GetAngle() { return m_gyroWrapper.GetAngle(); }
} // namespace lowfi
} // namespace sim
} // namespace frc

24
simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/WpiAnalogGyroSim.cpp Normal file
View File

@@ -0,0 +1,24 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#include "lowfisim/wpisimulators/WpiAnalogGyroSim.h"
namespace frc {
namespace sim {
namespace lowfi {
WpiAnalogGyroSim::WpiAnalogGyroSim(int index) : m_gyroSimulator(index) {}
void WpiAnalogGyroSim::SetAngle(double angle) {
m_gyroSimulator.SetAngle(angle);
}
double WpiAnalogGyroSim::GetAngle() { return m_gyroSimulator.GetAngle(); }
} // namespace lowfi
} // namespace sim
} // namespace frc

22
simulation/lowfi_simulation/src/main/native/include/lowfisim/AccelerometerSim.h Normal file
View File

@@ -0,0 +1,22 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
namespace frc {
namespace sim {
namespace lowfi {
class AccelerometerSim {
public:
virtual double GetAcceleration() = 0;
virtual void SetAcceleration(double acceleration) = 0;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

22
simulation/lowfi_simulation/src/main/native/include/lowfisim/GyroSim.h Normal file
View File

@@ -0,0 +1,22 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
namespace frc {
namespace sim {
namespace lowfi {
class GyroSim {
public:
virtual void SetAngle(double angle) = 0;
virtual double GetAngle() = 0;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

37
simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleAccelerometerSim.h Normal file
View File

@@ -0,0 +1,37 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <functional>
#include "lowfisim/AccelerometerSim.h"
namespace frc {
namespace sim {
namespace lowfi {
class SimpleAccelerometerSim : public AccelerometerSim {
public:
SimpleAccelerometerSim(const std::function<void(double)>& setterFunction,
const std::function<double(void)>& getterFunction)
: m_setAccelerationFunction(setterFunction),
m_getAccelerationFunction(getterFunction) {}
double GetAcceleration() override { return m_getAccelerationFunction(); }
void SetAcceleration(double acceleration) override {
m_setAccelerationFunction(acceleration);
}
private:
std::function<void(double)> m_setAccelerationFunction;
std::function<double(void)> m_getAccelerationFunction;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

35
simulation/lowfi_simulation/src/main/native/include/lowfisim/SimpleGyroSim.h Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include <functional>
#include "lowfisim/GyroSim.h"
namespace frc {
namespace sim {
namespace lowfi {
class SimpleGyroSim : public GyroSim {
public:
SimpleGyroSim(std::function<void(double)>& setterFunction,
std::function<double(void)>& getterFunction)
: m_setAngleFunction(setterFunction),
m_getAngleFunction(getterFunction) {}
double GetAngle() override { return m_getAngleFunction(); }
void SetAngle(double angle) override { m_setAngleFunction(angle); }
private:
std::function<void(double)> m_setAngleFunction;
std::function<double(void)> m_getAngleFunction;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

35
simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/ADXLThreeAxisAccelerometerSim.h Normal file
View File

@@ -0,0 +1,35 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "ThreeAxisAccelerometerData.h"
#include "lowfisim/SimpleAccelerometerSim.h"
namespace frc {
namespace sim {
namespace lowfi {
class ADXLThreeAxisAccelerometerSim {
public:
ADXLThreeAxisAccelerometerSim(
hal::ThreeAxisAccelerometerData& accelerometerWrapper);
AccelerometerSim& GetXWrapper();
AccelerometerSim& GetYWrapper();
AccelerometerSim& GetZWrapper();
protected:
hal::ThreeAxisAccelerometerData& m_accelerometerWrapper;
SimpleAccelerometerSim m_xWrapper;
SimpleAccelerometerSim m_yWrapper;
SimpleAccelerometerSim m_zWrapper;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

30
simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/ADXRS450_SpiGyroSim.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "ADXRS450_SpiGyroWrapperData.h"
#include "lowfisim/GyroSim.h"
namespace frc {
namespace sim {
namespace lowfi {
class ADXRS450_SpiGyroSim : public GyroSim {
public:
explicit ADXRS450_SpiGyroSim(int spiPort);
void SetAngle(double angle) override;
double GetAngle() override;
protected:
hal::ADXRS450_SpiGyroWrapper m_gyroWrapper;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

30
simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/WpiAnalogGyroSim.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "lowfisim/GyroSim.h"
#include "simulation/AnalogGyroSim.h"
namespace frc {
namespace sim {
namespace lowfi {
class WpiAnalogGyroSim : public GyroSim {
public:
explicit WpiAnalogGyroSim(int index);
void SetAngle(double angle) override;
double GetAngle() override;
protected:
frc::sim::AnalogGyroSim m_gyroSimulator;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

53
simulation/lowfi_simulation/src/test/native/cpp/lowfisim/AccelerometerSimulatorTest.cpp Normal file
View File

@@ -0,0 +1,53 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#include "ADXL345_I2CAccelerometerData.h"
#include "frc/ADXL345_I2C.h"
#include "gtest/gtest.h"
#include "lowfisim/wpisimulators/ADXLThreeAxisAccelerometerSim.h"
TEST(AccelerometerTests, TestADXL345_I2CAccelerometerWrapper) {
const double EPSILON = 1 / 256.0;
frc::I2C::Port port = frc::I2C::kOnboard;
frc::ADXL345_I2C accel{port};
EXPECT_NEAR(0, accel.GetX(), EPSILON);
EXPECT_NEAR(0, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
hal::ADXL345_I2CData rawAdxSim(port);
frc::sim::lowfi::ADXLThreeAxisAccelerometerSim accelerometerSim(rawAdxSim);
frc::sim::lowfi::AccelerometerSim& xWrapper = accelerometerSim.GetXWrapper();
frc::sim::lowfi::AccelerometerSim& yWrapper = accelerometerSim.GetYWrapper();
frc::sim::lowfi::AccelerometerSim& zWrapper = accelerometerSim.GetZWrapper();
xWrapper.SetAcceleration(1.45);
EXPECT_NEAR(1.45, accel.GetX(), EPSILON);
EXPECT_NEAR(0, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
EXPECT_NEAR(1.45, xWrapper.GetAcceleration(), EPSILON);
EXPECT_NEAR(0, yWrapper.GetAcceleration(), EPSILON);
EXPECT_NEAR(0, zWrapper.GetAcceleration(), EPSILON);
yWrapper.SetAcceleration(-.67);
EXPECT_NEAR(1.45, accel.GetX(), EPSILON);
EXPECT_NEAR(-.67, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
EXPECT_NEAR(1.45, xWrapper.GetAcceleration(), EPSILON);
EXPECT_NEAR(-.67, yWrapper.GetAcceleration(), EPSILON);
EXPECT_NEAR(0, zWrapper.GetAcceleration(), EPSILON);
zWrapper.SetAcceleration(2.42);
EXPECT_NEAR(1.45, accel.GetX(), EPSILON);
EXPECT_NEAR(-.67, accel.GetY(), EPSILON);
EXPECT_NEAR(2.42, accel.GetZ(), EPSILON);
EXPECT_NEAR(1.45, xWrapper.GetAcceleration(), EPSILON);
EXPECT_NEAR(-.67, yWrapper.GetAcceleration(), EPSILON);
EXPECT_NEAR(2.42, zWrapper.GetAcceleration(), EPSILON);
}

40
simulation/lowfi_simulation/src/test/native/cpp/lowfisim/GyroSimulatorTest.cpp Normal file
View File

@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. 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. */
/*----------------------------------------------------------------------------*/
#include "ADXRS450_SpiGyroWrapperData.h"
#include "frc/ADXRS450_Gyro.h"
#include "frc/AnalogGyro.h"
#include "gtest/gtest.h"
#include "lowfisim/wpisimulators/ADXRS450_SpiGyroSim.h"
#include "lowfisim/wpisimulators/WpiAnalogGyroSim.h"
void TestGyro(frc::sim::lowfi::GyroSim& sim, frc::Gyro& gyro) {
const double EPSILON = .00001;
EXPECT_NEAR(0, gyro.GetAngle(), EPSILON);
EXPECT_NEAR(0, sim.GetAngle(), EPSILON);
sim.SetAngle(45.13);
EXPECT_NEAR(45.13, gyro.GetAngle(), EPSILON);
EXPECT_NEAR(45.13, sim.GetAngle(), EPSILON);
}
TEST(GyroSimulatorTests, TestAnalogGyro) {
int port = 1;
frc::AnalogGyro gyro{port};
frc::sim::lowfi::WpiAnalogGyroSim sim{port};
TestGyro(sim, gyro);
}
TEST(GyroSimulatorTests, TestSpiGyro) {
frc::SPI::Port port = frc::SPI::kOnboardCS0;
frc::sim::lowfi::ADXRS450_SpiGyroSim sim{port};
frc::ADXRS450_Gyro gyro{port};
TestGyro(sim, gyro);
}