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

Implement sim devices for ADXL345, ADXL362, ADXRS450, Ultrasonic

Browse Source 
This makes the halsim_adx_gyro_accelerometer simulation plugin and
the accelerometer part of lowfi_simulation obsolete.
This commit is contained in:
Peter Johnson
2019-09-29 00:57:16 -07:00
parent aa90645865
commit a9f0e46680
42 changed files with 371 additions and 1337 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
settings.gradle
View File

@@ -24,7 +24,6 @@ include 'wpilibjIntegrationTests'
include 'wpilibj'
include 'simulation:halsim_print'
include 'simulation:halsim_lowfi'
include 'simulation:halsim_adx_gyro_accelerometer'
include 'simulation:halsim_ds_nt'
include 'simulation:gz_msgs'
include 'simulation:frc_gazebo_plugins'

199
simulation/halsim_adx_gyro_accelerometer/build.gradle
View File

@@ -1,199 +0,0 @@
apply plugin: 'cpp'
apply plugin: 'google-test-test-suite'
apply plugin: 'visual-studio'
apply plugin: 'edu.wpi.first.NativeUtils'
apply plugin: SingleNativeBuild
apply plugin: ExtraTasks
ext {
nativeName = 'halsim_adx_gyro_accelerometer'
}
apply from: "${rootDir}/shared/config.gradle"
if (!project.hasProperty('onlylinuxathena')) {
ext.skiplinuxathena = true
ext {
sharedCvConfigs = [halsim_adx_gyro_accelerometerTest: []]
staticCvConfigs = [:]
useJava = false
useCpp = true
}
apply from: "${rootDir}/shared/opencv.gradle"
ext {
staticGtestConfigs = [:]
}
staticGtestConfigs["${nativeName}Test"] = []
apply from: "${rootDir}/shared/googletest.gradle"
project(':').libraryBuild.dependsOn build
ext {
chipObjectComponents = ["$nativeName".toString(), "${nativeName}Base".toString(), "${nativeName}Dev".toString(), "${nativeName}Test".toString()]
netCommComponents = ["$nativeName".toString(), "${nativeName}Base".toString(), "${nativeName}Dev".toString(), "${nativeName}Test".toString()]
useNiJava = false
}
apply from: "${rootDir}/shared/nilibraries.gradle"
nativeUtils.exportsConfigs {
halsim_adx_gyro_accelerometer {
x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
'_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
'_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
'_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure',
'_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure',
'_TI5?AVfailure', '_CT??_R0?AVout_of_range', '_CTA3?AVout_of_range',
'_TI3?AVout_of_range', '_CT??_R0?AVbad_cast']
}
}
model {
components {
"${nativeName}Base"(NativeLibrarySpec) {
sources {
cpp {
source {
srcDirs = ['src/main/native/cpp']
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/main/native/include'
}
}
}
binaries.all {
if (it instanceof SharedLibraryBinarySpec) {
it.buildable = false
return
}
if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
it.buildable = false
return
}
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
}
}
"${nativeName}"(NativeLibrarySpec) {
sources {
cpp {
source {
srcDirs "${rootDir}/shared/singlelib"
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/main/native/include'
}
}
}
binaries.all {
if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
it.buildable = false
return
}
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
}
}
// By default, a development executable will be generated. This is to help the case of
// testing specific functionality of the library.
"${nativeName}Dev"(NativeExecutableSpec) {
targetBuildTypes 'debug'
sources {
cpp {
source {
srcDirs 'src/dev/native/cpp'
include '**/*.cpp'
lib library: "${nativeName}"
}
exportedHeaders {
srcDirs 'src/dev/native/include'
}
}
}
binaries.all {
if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
it.buildable = false
return
}
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib library: nativeName, linkage: 'shared'
}
}
}
binaries {
withType(GoogleTestTestSuiteBinarySpec) {
if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
lib project: ':ntcore', library: 'ntcore', linkage: 'shared'
lib project: ':cscore', library: 'cscore', linkage: 'shared'
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib library: nativeName, linkage: 'shared'
} else {
it.buildable = false
}
}
}
}
apply from: "publish.gradle"
}
model {
testSuites {
if (!project.hasProperty('onlylinuxathena') && !project.hasProperty('onlylinuxraspbian') && !project.hasProperty('onlylinuxaarch64bionic')) {
"${nativeName}Test"(GoogleTestTestSuiteSpec) {
for(NativeComponentSpec c : $.components) {
if (c.name == nativeName) {
testing c
break
}
}
sources {
cpp {
source {
srcDirs 'src/test/native/cpp'
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/test/native/include', 'src/main/native/cpp'
}
}
}
}
}
}
tasks {
def c = $.components
project.tasks.create('runCpp', Exec) {
def found = false
c.each {
if (it in NativeExecutableSpec && it.name == "${nativeName}Dev") {
it.binaries.each {
if (!found) {
def arch = it.targetPlatform.architecture.name
if (arch == 'x86-64' || arch == 'x86') {
dependsOn it.tasks.install
commandLine it.tasks.install.runScriptFile.get().asFile.toString()
found = true
}
}
}
}
}
}
}
}
tasks.withType(RunTestExecutable) {
args "--gtest_output=xml:test_detail.xml"
outputs.dir outputDir
}

63
simulation/halsim_adx_gyro_accelerometer/publish.gradle
View File

@@ -1,63 +0,0 @@
apply plugin: 'maven-publish'
def baseArtifactId = nativeName
def artifactGroupId = 'edu.wpi.first.halsim'
def zipBaseName = "_GROUP_edu_wpi_first_halsim_ID_${nativeName}_CLS"
def outputsFolder = file("$project.buildDir/outputs")
task cppSourcesZip(type: Zip) {
destinationDir = outputsFolder
baseName = zipBaseName
classifier = "sources"
from(licenseFile) {
into '/'
}
from('src/main/native/cpp') {
into '/'
}
}
task cppHeadersZip(type: Zip) {
destinationDir = outputsFolder
baseName = zipBaseName
classifier = "headers"
from(licenseFile) {
into '/'
}
from('src/main/native/include') {
into '/'
}
}
build.dependsOn cppSourcesZip
build.dependsOn cppHeadersZip
addTaskToCopyAllOutputs(cppSourcesZip)
addTaskToCopyAllOutputs(cppHeadersZip)
model {
publishing {
def halsim_adx_gyro_accelerometerTaskList = createComponentZipTasks($.components, [nativeName], zipBaseName, Zip, project, includeStandardZipFormat)
publications {
cpp(MavenPublication) {
halsim_adx_gyro_accelerometerTaskList.each {
artifact it
}
artifact cppHeadersZip
artifact cppSourcesZip
artifactId = baseArtifactId
groupId artifactGroupId
version wpilibVersioning.version.get()
}
}
}
}

8
simulation/halsim_adx_gyro_accelerometer/src/dev/native/cpp/main.cpp
View File

@@ -1,8 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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. */
/*----------------------------------------------------------------------------*/
int main() {}

73
simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp
View File

@@ -1,73 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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 <cstring>
#include <mockdata/I2CData.h>
using namespace hal;
const double ADXL345_I2CData::LSB = 1 / 0.00390625;
static void ADXL345I2C_ReadBufferCallback(const char* name, void* param,
uint8_t* buffer, uint32_t count) {
ADXL345_I2CData* sim = static_cast<ADXL345_I2CData*>(param);
sim->HandleRead(buffer, count);
}
static void ADXL345I2C_WriteBufferCallback(const char* name, void* param,
const uint8_t* buffer,
uint32_t count) {
ADXL345_I2CData* sim = static_cast<ADXL345_I2CData*>(param);
sim->HandleWrite(buffer, count);
}
ADXL345_I2CData::ADXL345_I2CData(int port) : m_port(port) {
m_readCallbackId =
HALSIM_RegisterI2CReadCallback(port, ADXL345I2C_ReadBufferCallback, this);
m_writeCallbackId = HALSIM_RegisterI2CWriteCallback(
port, ADXL345I2C_WriteBufferCallback, this);
}
ADXL345_I2CData::~ADXL345_I2CData() {
HALSIM_CancelI2CReadCallback(m_port, m_readCallbackId);
HALSIM_CancelI2CWriteCallback(m_port, m_writeCallbackId);
}
bool ADXL345_I2CData::GetInitialized() const {
return HALSIM_GetI2CInitialized(m_port);
}
void ADXL345_I2CData::ADXL345_I2CData::HandleWrite(const uint8_t* buffer,
uint32_t count) {
m_lastWriteAddress = buffer[0];
}
void ADXL345_I2CData::HandleRead(uint8_t* buffer, uint32_t count) {
bool writeAll = count == 6;
int byteIndex = 0;
if (writeAll || m_lastWriteAddress == 0x32) {
int16_t val = static_cast<int16_t>(GetX() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
if (writeAll || m_lastWriteAddress == 0x34) {
int16_t val = static_cast<int16_t>(GetY() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
if (writeAll || m_lastWriteAddress == 0x36) {
int16_t val = static_cast<int16_t>(GetZ() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
}

73
simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp
View File

@@ -1,73 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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_SpiAccelerometerData.h"
#include <cstring>
#include <mockdata/SPIData.h>
using namespace hal;
const double ADXL345_SpiAccelerometer::LSB = 1 / 0.00390625;
static void ADXL345SPI_ReadBufferCallback(const char* name, void* param,
uint8_t* buffer, uint32_t count) {
ADXL345_SpiAccelerometer* sim = static_cast<ADXL345_SpiAccelerometer*>(param);
sim->HandleRead(buffer, count);
}
static void ADXL345SPI_WriteBufferCallback(const char* name, void* param,
const uint8_t* buffer,
uint32_t count) {
ADXL345_SpiAccelerometer* sim = static_cast<ADXL345_SpiAccelerometer*>(param);
sim->HandleWrite(buffer, count);
}
ADXL345_SpiAccelerometer::ADXL345_SpiAccelerometer(int port) : m_port(port) {
m_readCallbackId =
HALSIM_RegisterSPIReadCallback(port, ADXL345SPI_ReadBufferCallback, this);
m_writeCallbackId = HALSIM_RegisterSPIWriteCallback(
port, ADXL345SPI_WriteBufferCallback, this);
}
ADXL345_SpiAccelerometer::~ADXL345_SpiAccelerometer() {
HALSIM_CancelSPIReadCallback(m_port, m_readCallbackId);
HALSIM_CancelSPIWriteCallback(m_port, m_writeCallbackId);
}
bool ADXL345_SpiAccelerometer::GetInitialized() const {
return HALSIM_GetSPIInitialized(m_port);
}
void ADXL345_SpiAccelerometer::HandleWrite(const uint8_t* buffer,
uint32_t count) {
m_lastWriteAddress = buffer[0] & 0xF;
}
void ADXL345_SpiAccelerometer::HandleRead(uint8_t* buffer, uint32_t count) {
bool writeAll = count == 7;
int byteIndex = 1;
if (writeAll || m_lastWriteAddress == 0x02) {
int16_t val = static_cast<int16_t>(GetX() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
if (writeAll || m_lastWriteAddress == 0x04) {
int16_t val = static_cast<int16_t>(GetY() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
if (writeAll || m_lastWriteAddress == 0x06) {
int16_t val = static_cast<int16_t>(GetZ() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
}

79
simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp
View File

@@ -1,79 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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 "ADXL362_SpiAccelerometerData.h"
#include <cstring>
#include <mockdata/SPIData.h>
using namespace hal;
const double ADXL362_SpiAccelerometer::LSB = 1 / 0.001;
static void ADXL362SPI_ReadBufferCallback(const char* name, void* param,
uint8_t* buffer, uint32_t count) {
ADXL362_SpiAccelerometer* sim = static_cast<ADXL362_SpiAccelerometer*>(param);
sim->HandleRead(buffer, count);
}
static void ADXL362SPI_WriteBufferCallback(const char* name, void* param,
const uint8_t* buffer,
uint32_t count) {
ADXL362_SpiAccelerometer* sim = static_cast<ADXL362_SpiAccelerometer*>(param);
sim->HandleWrite(buffer, count);
}
ADXL362_SpiAccelerometer::ADXL362_SpiAccelerometer(int port) : m_port(port) {
m_readCallbackId =
HALSIM_RegisterSPIReadCallback(port, ADXL362SPI_ReadBufferCallback, this);
m_writeCallbackId = HALSIM_RegisterSPIWriteCallback(
port, ADXL362SPI_WriteBufferCallback, this);
}
ADXL362_SpiAccelerometer::~ADXL362_SpiAccelerometer() {
HALSIM_CancelSPIReadCallback(m_port, m_readCallbackId);
HALSIM_CancelSPIWriteCallback(m_port, m_writeCallbackId);
}
bool ADXL362_SpiAccelerometer::GetInitialized() const {
return HALSIM_GetSPIInitialized(m_port);
}
void ADXL362_SpiAccelerometer::HandleWrite(const uint8_t* buffer,
uint32_t count) {
m_lastWriteAddress = buffer[1];
}
void ADXL362_SpiAccelerometer::HandleRead(uint8_t* buffer, uint32_t count) {
// Init check
if (m_lastWriteAddress == 0x02) {
uint32_t numToPut = 0xF20000;
std::memcpy(&buffer[0], &numToPut, sizeof(numToPut));
} else {
// Get Accelerations
bool writeAll = count == 8;
int byteIndex = 2;
if (writeAll || m_lastWriteAddress == 0x0E) {
int16_t val = static_cast<int16_t>(GetX() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
if (writeAll || m_lastWriteAddress == 0x10) {
int16_t val = static_cast<int16_t>(GetY() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
if (writeAll || m_lastWriteAddress == 0x12) {
int16_t val = static_cast<int16_t>(GetZ() * LSB);
std::memcpy(&buffer[byteIndex], &val, sizeof(val));
byteIndex += sizeof(val);
}
}
}

132
simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp
View File

@@ -1,132 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 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 <algorithm>
#include <cmath>
#include <cstring>
#include <hal/HALBase.h>
#include <mockdata/SPIData.h>
#ifdef _WIN32
#include "Winsock2.h"
#pragma comment(lib, "ws2_32.lib")
#else
#include <arpa/inet.h>
#endif
using namespace hal;
static constexpr double kSamplePeriod = 0.0005;
const double ADXRS450_SpiGyroWrapper::kAngleLsb = 1 / 0.0125 / kSamplePeriod;
const double ADXRS450_SpiGyroWrapper::kMaxAngleDeltaPerMessage = 0.1875;
const int ADXRS450_SpiGyroWrapper::kPacketSize = 4 + 1; // +1 for timestamp
template <class T>
constexpr const T& clamp(const T& value, const T& low, const T& high) {
return (std::max)(low, (std::min)(value, high));
}
static void ADXRS450SPI_ReadBufferCallback(const char* name, void* param,
uint8_t* buffer, uint32_t count) {
auto sim = static_cast<ADXRS450_SpiGyroWrapper*>(param);
sim->HandleRead(buffer, count);
}
static void ADXRS450SPI_ReadAutoReceivedData(const char* name, void* param,
uint32_t* buffer,
int32_t numToRead,
int32_t* outputCount) {
auto sim = static_cast<ADXRS450_SpiGyroWrapper*>(param);
sim->HandleAutoReceiveData(buffer, numToRead, *outputCount);
}
ADXRS450_SpiGyroWrapper::ADXRS450_SpiGyroWrapper(int port) : m_port(port) {
m_readCallbackId = HALSIM_RegisterSPIReadCallback(
port, ADXRS450SPI_ReadBufferCallback, this);
m_autoReceiveReadCallbackId = HALSIM_RegisterSPIReadAutoReceivedDataCallback(
port, ADXRS450SPI_ReadAutoReceivedData, this);
}
ADXRS450_SpiGyroWrapper::~ADXRS450_SpiGyroWrapper() {
HALSIM_CancelSPIReadCallback(m_port, m_readCallbackId);
HALSIM_CancelSPIReadAutoReceivedDataCallback(m_port,
m_autoReceiveReadCallbackId);
}
bool ADXRS450_SpiGyroWrapper::GetInitialized() const {
return HALSIM_GetSPIInitialized(m_port);
}
void ADXRS450_SpiGyroWrapper::ResetData() {
std::scoped_lock lock(m_angle.GetMutex());
m_angle.Reset(0.0);
m_angleDiff = 0;
}
void ADXRS450_SpiGyroWrapper::HandleRead(uint8_t* buffer, uint32_t count) {
int returnCode = 0x00400AE0;
std::memcpy(&buffer[0], &returnCode, sizeof(returnCode));
}
void ADXRS450_SpiGyroWrapper::HandleAutoReceiveData(uint32_t* buffer,
int32_t numToRead,
int32_t& outputCount) {
std::scoped_lock lock(m_angle.GetMutex());
int32_t messagesToSend =
1 + std::abs(m_angleDiff > 0
? std::ceil(m_angleDiff / kMaxAngleDeltaPerMessage)
: std::floor(m_angleDiff / kMaxAngleDeltaPerMessage));
// Zero gets passed in during the "How much data do I need to read" step.
// Else it is actually reading the accumulator
if (numToRead == 0) {
outputCount = messagesToSend * kPacketSize;
return;
}
int valuesToRead = numToRead / kPacketSize;
std::memset(&buffer[0], 0, numToRead * sizeof(uint32_t));
int32_t status = 0;
uint32_t timestamp = HAL_GetFPGATime(&status);
for (int msgCtr = 0; msgCtr < valuesToRead; ++msgCtr) {
// force the first message to be a rate of 0 to init the timestamp
double cappedDiff = (msgCtr == 0)
? 0
: clamp(m_angleDiff, -kMaxAngleDeltaPerMessage,
kMaxAngleDeltaPerMessage);
// first word is timestamp
buffer[msgCtr * kPacketSize] = timestamp;
int32_t valueToSend =
((static_cast<int32_t>(cappedDiff * kAngleLsb) << 10) & (~0x0C00000E)) |
0x04000000;
// following words have byte in LSB, in big endian order
for (int i = 4; i >= 1; --i) {
buffer[msgCtr * kPacketSize + i] =
static_cast<uint32_t>(valueToSend) & 0xffu;
valueToSend >>= 8;
}
m_angleDiff -= cappedDiff;
timestamp += kSamplePeriod * 1e6; // fpga time is in us
}
}
void ADXRS450_SpiGyroWrapper::SetAngle(double angle) {
std::scoped_lock lock(m_angle.GetMutex());
if (m_angle != angle) {
m_angleDiff += angle - m_angle;
m_angle = angle;
}
}

19
simulation/halsim_adx_gyro_accelerometer/src/main/native/cpp/ThreeAxisAccelerometerData.cpp
View File

@@ -1,19 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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 "ThreeAxisAccelerometerData.h"
using namespace hal;
ThreeAxisAccelerometerData::ThreeAxisAccelerometerData() {}
ThreeAxisAccelerometerData::~ThreeAxisAccelerometerData() {}
void ThreeAxisAccelerometerData::ResetData() {
x.Reset(0.0);
y.Reset(0.0);
z.Reset(0.0);
}

32
simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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"
namespace hal {
class ADXL345_I2CData : public ThreeAxisAccelerometerData {
public:
explicit ADXL345_I2CData(int port);
virtual ~ADXL345_I2CData();
bool GetInitialized() const override;
void HandleWrite(const uint8_t* buffer, uint32_t count);
void HandleRead(uint8_t* buffer, uint32_t count);
private:
int m_port;
int m_writeCallbackId;
int m_readCallbackId;
int m_lastWriteAddress;
static const double LSB;
};
} // namespace hal

32
simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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"
namespace hal {
class ADXL345_SpiAccelerometer : public ThreeAxisAccelerometerData {
public:
explicit ADXL345_SpiAccelerometer(int port);
virtual ~ADXL345_SpiAccelerometer();
bool GetInitialized() const override;
void HandleWrite(const uint8_t* buffer, uint32_t count);
void HandleRead(uint8_t* buffer, uint32_t count);
private:
int m_port;
int m_writeCallbackId;
int m_readCallbackId;
int m_lastWriteAddress;
static const double LSB;
};
} // namespace hal

32
simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-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"
namespace hal {
class ADXL362_SpiAccelerometer : public ThreeAxisAccelerometerData {
public:
explicit ADXL362_SpiAccelerometer(int port);
virtual ~ADXL362_SpiAccelerometer();
bool GetInitialized() const override;
void HandleWrite(const uint8_t* buffer, uint32_t count);
void HandleRead(uint8_t* buffer, uint32_t count);
private:
int m_port;
int m_writeCallbackId;
int m_readCallbackId;
int m_lastWriteAddress;
static const double LSB;
};
} // namespace hal

50
simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h
View File

@@ -1,50 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 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 <mockdata/SimDataValue.h>
namespace hal {
class ADXRS450_SpiGyroWrapper {
public:
explicit ADXRS450_SpiGyroWrapper(int port);
virtual ~ADXRS450_SpiGyroWrapper();
bool GetInitialized() const;
void HandleRead(uint8_t* buffer, uint32_t count);
void HandleAutoReceiveData(uint32_t* buffer, int32_t numToRead,
int32_t& outputCount);
int32_t RegisterAngleCallback(HAL_NotifyCallback callback, void* param,
HAL_Bool initialNotify) {
return m_angle.RegisterCallback(callback, param, initialNotify);
}
void CancelAngleCallback(int32_t uid) { m_angle.CancelCallback(uid); }
double GetAngle() { return m_angle; }
void SetAngle(double angle);
virtual void ResetData();
private:
int m_port;
int m_readCallbackId;
int m_autoReceiveReadCallbackId;
HAL_SIMDATAVALUE_DEFINE_NAME(Angle)
SimDataValue<double, HAL_MakeDouble, GetAngleName> m_angle{0.0};
double m_angleDiff = 0.0;
static const double kAngleLsb;
// The maximum difference that can fit inside of the shifted and masked data
// field, per transaction
static const double kMaxAngleDeltaPerMessage;
static const int kPacketSize;
};
} // namespace hal

39
simulation/halsim_adx_gyro_accelerometer/src/main/native/include/ThreeAxisAccelerometerData.h
View File

@@ -1,39 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2017-2019 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 <mockdata/SimDataValue.h>
namespace hal {
class ThreeAxisAccelerometerData {
HAL_SIMDATAVALUE_DEFINE_NAME(X);
HAL_SIMDATAVALUE_DEFINE_NAME(Y);
HAL_SIMDATAVALUE_DEFINE_NAME(Z);
public:
ThreeAxisAccelerometerData();
virtual ~ThreeAxisAccelerometerData();
virtual bool GetInitialized() const = 0;
double GetX() { return x; }
void SetX(double x_) { x = x_; }
double GetY() { return y; }
void SetY(double y_) { y = y_; }
double GetZ() { return z; }
void SetZ(double z_) { z = z_; }
SimDataValue<double, HAL_MakeDouble, GetXName> x{0.0};
SimDataValue<double, HAL_MakeDouble, GetYName> y{0.0};
SimDataValue<double, HAL_MakeDouble, GetZName> z{0.0};
virtual void ResetData();
};
} // namespace hal

46
simulation/halsim_adx_gyro_accelerometer/src/test/native/cpp/ADXL345_I2CAccelerometerTest.cpp
View File

@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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"
class ADXL345_I2CAccelerometerTest
: public ::testing::TestWithParam<frc::I2C::Port> {};
TEST_P(ADXL345_I2CAccelerometerTest, TestAccelerometer) {
const double EPSILON = 1 / 256.0;
frc::I2C::Port port = GetParam();
hal::ADXL345_I2CData sim{port};
frc::ADXL345_I2C accel{port};
EXPECT_NEAR(0, sim.GetX(), EPSILON);
EXPECT_NEAR(0, sim.GetY(), EPSILON);
EXPECT_NEAR(0, sim.GetZ(), EPSILON);
EXPECT_NEAR(0, accel.GetX(), EPSILON);
EXPECT_NEAR(0, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
sim.SetX(1.56);
sim.SetY(-.653);
sim.SetZ(0.11);
EXPECT_NEAR(1.56, sim.GetX(), EPSILON);
EXPECT_NEAR(-.653, sim.GetY(), EPSILON);
EXPECT_NEAR(0.11, sim.GetZ(), EPSILON);
EXPECT_NEAR(1.56, accel.GetX(), EPSILON);
EXPECT_NEAR(-.653, accel.GetY(), EPSILON);
EXPECT_NEAR(0.11, accel.GetZ(), EPSILON);
}
INSTANTIATE_TEST_SUITE_P(ADXL345_I2CAccelerometerTest,
ADXL345_I2CAccelerometerTest,
::testing::Values(frc::I2C::kOnboard, frc::I2C::kMXP));

48
simulation/halsim_adx_gyro_accelerometer/src/test/native/cpp/ADXL345_SpiAccelerometerTest.cpp
View File

@@ -1,48 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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_SpiAccelerometerData.h"
#include "frc/ADXL345_SPI.h"
#include "gtest/gtest.h"
class ADXL345_SpiAccelerometerTest
: public ::testing::TestWithParam<frc::SPI::Port> {};
TEST_P(ADXL345_SpiAccelerometerTest, TestAccelerometer) {
const double EPSILON = 1 / 256.0;
frc::SPI::Port port = GetParam();
hal::ADXL345_SpiAccelerometer sim{port};
frc::ADXL345_SPI accel{port};
EXPECT_NEAR(0, sim.GetX(), EPSILON);
EXPECT_NEAR(0, sim.GetY(), EPSILON);
EXPECT_NEAR(0, sim.GetZ(), EPSILON);
EXPECT_NEAR(0, accel.GetX(), EPSILON);
EXPECT_NEAR(0, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
sim.SetX(1.56);
sim.SetY(-.653);
sim.SetZ(0.11);
EXPECT_NEAR(1.56, sim.GetX(), EPSILON);
EXPECT_NEAR(-.653, sim.GetY(), EPSILON);
EXPECT_NEAR(0.11, sim.GetZ(), EPSILON);
EXPECT_NEAR(1.56, accel.GetX(), EPSILON);
EXPECT_NEAR(-.653, accel.GetY(), EPSILON);
EXPECT_NEAR(0.11, accel.GetZ(), EPSILON);
}
INSTANTIATE_TEST_SUITE_P(
ADXL345_SpiAccelerometerTest, ADXL345_SpiAccelerometerTest,
::testing::Values(frc::SPI::kOnboardCS0, frc::SPI::kOnboardCS1,
frc::SPI::kOnboardCS2, frc::SPI::kOnboardCS3,
frc::SPI::kMXP));

48
simulation/halsim_adx_gyro_accelerometer/src/test/native/cpp/ADXL362_SpiAccelerometerTest.cpp
View File

@@ -1,48 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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 "ADXL362_SpiAccelerometerData.h"
#include "frc/ADXL362.h"
#include "gtest/gtest.h"
class ADXL362_SpiAccelerometerTest
: public ::testing::TestWithParam<frc::SPI::Port> {};
TEST_P(ADXL362_SpiAccelerometerTest, TestAccelerometer) {
const double EPSILON = 1 / 256.0;
frc::SPI::Port port = GetParam();
hal::ADXL362_SpiAccelerometer sim{port};
frc::ADXL362 accel{port};
EXPECT_NEAR(0, sim.GetX(), EPSILON);
EXPECT_NEAR(0, sim.GetY(), EPSILON);
EXPECT_NEAR(0, sim.GetZ(), EPSILON);
EXPECT_NEAR(0, accel.GetX(), EPSILON);
EXPECT_NEAR(0, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
sim.SetX(1.56);
sim.SetY(-.653);
sim.SetZ(0.11);
EXPECT_NEAR(1.56, sim.GetX(), EPSILON);
EXPECT_NEAR(-.653, sim.GetY(), EPSILON);
EXPECT_NEAR(0.11, sim.GetZ(), EPSILON);
EXPECT_NEAR(1.56, accel.GetX(), EPSILON);
EXPECT_NEAR(-.653, accel.GetY(), EPSILON);
EXPECT_NEAR(0.11, accel.GetZ(), EPSILON);
}
INSTANTIATE_TEST_SUITE_P(
ADXL362_SpiAccelerometerTest, ADXL362_SpiAccelerometerTest,
::testing::Values(frc::SPI::kOnboardCS0, frc::SPI::kOnboardCS1,
frc::SPI::kOnboardCS2, frc::SPI::kOnboardCS3,
frc::SPI::kMXP));

41
simulation/halsim_adx_gyro_accelerometer/src/test/native/cpp/ADXRS450_SpiGyroWrapperTest.cpp
View File

@@ -1,41 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018-2019 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 "gtest/gtest.h"
class ADXRS450_SpiGyroWrapperTest
: public ::testing::TestWithParam<frc::SPI::Port> {};
TEST_P(ADXRS450_SpiGyroWrapperTest, TestAccelerometer) {
#ifdef __APPLE__
// Disable test on mac, because of unknown failures
// TODO: Finally figure out the isse.
return;
#else
const double EPSILON = .000001;
frc::SPI::Port port = GetParam();
hal::ADXRS450_SpiGyroWrapper sim{port};
frc::ADXRS450_Gyro gyro{port};
EXPECT_NEAR(0, sim.GetAngle(), EPSILON);
EXPECT_NEAR(0, gyro.GetAngle(), EPSILON);
sim.SetAngle(32.56);
EXPECT_NEAR(32.56, sim.GetAngle(), EPSILON);
EXPECT_NEAR(32.56, gyro.GetAngle(), EPSILON);
#endif
}
INSTANTIATE_TEST_SUITE_P(
ADXRS450_SpiGyroWrapperTest, ADXRS450_SpiGyroWrapperTest,
::testing::Values(frc::SPI::kOnboardCS0, frc::SPI::kOnboardCS1,
frc::SPI::kOnboardCS2, frc::SPI::kOnboardCS3,
frc::SPI::kMXP));

17
simulation/halsim_adx_gyro_accelerometer/src/test/native/cpp/main.cpp
View File

@@ -1,17 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2015-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 <hal/HAL.h>
#include "gtest/gtest.h"
int main(int argc, char** argv) {
HAL_Initialize(500, 0);
::testing::InitGoogleTest(&argc, argv);
int ret = RUN_ALL_TESTS();
return ret;
}

4
simulation/lowfi_simulation/build.gradle
View File

@@ -78,7 +78,6 @@ if (!project.hasProperty('onlylinuxathena')) {
}
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
}
}
"${nativeName}"(NativeLibrarySpec) {
@@ -100,7 +99,6 @@ if (!project.hasProperty('onlylinuxathena')) {
}
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
}
}
// By default, a development executable will be generated. This is to help the case of
@@ -126,7 +124,6 @@ if (!project.hasProperty('onlylinuxathena')) {
}
project(':hal').addHalDependency(it, 'shared')
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
lib library: nativeName, linkage: 'shared'
}
}
@@ -139,7 +136,6 @@ if (!project.hasProperty('onlylinuxathena')) {
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared'
lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared'
lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
lib library: nativeName, linkage: 'shared'
}
}

46
simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/ADXLThreeAxisAccelerometerSim.cpp
View File

@@ -1,46 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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(
[data = &m_accelerometerWrapper] { return data->GetInitialized(); },
[data = &m_accelerometerWrapper](double x) { data->x = x; },
[data = &m_accelerometerWrapper] { return data->GetX(); }),
m_yWrapper(
[data = &m_accelerometerWrapper] { return data->GetInitialized(); },
[data = &m_accelerometerWrapper](double y) { data->y = y; },
[data = &m_accelerometerWrapper] { return data->GetY(); }),
m_zWrapper(
[data = &m_accelerometerWrapper] { return data->GetInitialized(); },
[data = &m_accelerometerWrapper](double z) { data->z = z; },
[data = &m_accelerometerWrapper] { return data->GetZ(); }) {}
AccelerometerSim& ADXLThreeAxisAccelerometerSim::GetXWrapper() {
return m_xWrapper;
}
AccelerometerSim& ADXLThreeAxisAccelerometerSim::GetYWrapper() {
return m_yWrapper;
}
AccelerometerSim& ADXLThreeAxisAccelerometerSim::GetZWrapper() {
return m_zWrapper;
}
} // namespace lowfi
} // namespace sim
} // namespace frc

29
simulation/lowfi_simulation/src/main/native/cpp/lowfisim/wpisimulators/ADXRS450_SpiGyroSim.cpp
View File

@@ -1,29 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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) {}
bool ADXRS450_SpiGyroSim::IsWrapperInitialized() const {
return m_gyroWrapper.GetInitialized();
}
void ADXRS450_SpiGyroSim::SetAngle(double angle) {
m_gyroWrapper.SetAngle(angle);
}
double ADXRS450_SpiGyroSim::GetAngle() { return m_gyroWrapper.GetAngle(); }
} // namespace lowfi
} // namespace sim
} // namespace frc

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

@@ -1,35 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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

32
simulation/lowfi_simulation/src/main/native/include/lowfisim/wpisimulators/ADXRS450_SpiGyroSim.h
View File

@@ -1,32 +0,0 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2019 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);
bool IsWrapperInitialized() const override;
void SetAngle(double angle) override;
double GetAngle() override;
protected:
hal::ADXRS450_SpiGyroWrapper m_gyroWrapper;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

58
simulation/lowfi_simulation/src/test/native/cpp/lowfisim/AccelerometerSimulatorTest.cpp
View File

@@ -1,58 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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;
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();
EXPECT_FALSE(xWrapper.IsWrapperInitialized());
EXPECT_FALSE(yWrapper.IsWrapperInitialized());
EXPECT_FALSE(zWrapper.IsWrapperInitialized());
frc::ADXL345_I2C accel{port};
EXPECT_NEAR(0, accel.GetX(), EPSILON);
EXPECT_NEAR(0, accel.GetY(), EPSILON);
EXPECT_NEAR(0, accel.GetZ(), EPSILON);
EXPECT_TRUE(xWrapper.IsWrapperInitialized());
EXPECT_TRUE(yWrapper.IsWrapperInitialized());
EXPECT_TRUE(zWrapper.IsWrapperInitialized());
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);
}

47
simulation/lowfi_simulation/src/test/native/cpp/lowfisim/GyroSimulatorTest.cpp
View File

@@ -1,47 +0,0 @@
/*----------------------------------------------------------------------------*/
/* 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::sim::lowfi::WpiAnalogGyroSim sim{port};
EXPECT_FALSE(sim.IsWrapperInitialized());
frc::AnalogGyro gyro{port};
EXPECT_TRUE(sim.IsWrapperInitialized());
TestGyro(sim, gyro);
}
TEST(GyroSimulatorTests, TestSpiGyro) {
frc::SPI::Port port = frc::SPI::kOnboardCS0;
frc::sim::lowfi::ADXRS450_SpiGyroSim sim{port};
EXPECT_FALSE(sim.IsWrapperInitialized());
frc::ADXRS450_Gyro gyro{port};
EXPECT_TRUE(sim.IsWrapperInitialized());
TestGyro(sim, gyro);
}

21
wpilibc/src/main/native/cpp/ADXL345_I2C.cpp
View File

@@ -15,7 +15,15 @@
using namespace frc;
ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
: m_i2c(port, deviceAddress) {
: m_i2c(port, deviceAddress),
m_simDevice("ADXL345_I2C", port, deviceAddress) {
if (m_simDevice) {
m_simRange =
m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
}
// Turn on the measurements
m_i2c.Write(kPowerCtlRegister, kPowerCtl_Measure);
// Specify the data format to read
@@ -39,6 +47,9 @@ double ADXL345_I2C::GetY() { return GetAcceleration(kAxis_Y); }
double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
if (axis == kAxis_X && m_simX) return m_simX.Get();
if (axis == kAxis_Y && m_simY) return m_simY.Get();
if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
int16_t rawAccel = 0;
m_i2c.Read(kDataRegister + static_cast<int>(axis), sizeof(rawAccel),
reinterpret_cast<uint8_t*>(&rawAccel));
@@ -46,7 +57,13 @@ double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
}
ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() {
AllAxes data = AllAxes();
AllAxes data;
if (m_simX && m_simY && m_simZ) {
data.XAxis = m_simX.Get();
data.YAxis = m_simY.Get();
data.ZAxis = m_simZ.Get();
return data;
}
int16_t rawData[3];
m_i2c.Read(kDataRegister, sizeof(rawData),
reinterpret_cast<uint8_t*>(rawData));

23
wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
View File

@@ -15,7 +15,14 @@
using namespace frc;
ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
: m_spi(port) {
: m_spi(port), m_simDevice("ADXL345_SPI", port) {
if (m_simDevice) {
m_simRange =
m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
}
m_spi.SetClockRate(500000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnTrailingEdge();
@@ -43,6 +50,8 @@ void ADXL345_SPI::SetRange(Range range) {
commands[0] = kDataFormatRegister;
commands[1] = kDataFormat_FullRes | static_cast<uint8_t>(range & 0x03);
m_spi.Transaction(commands, commands, 2);
if (m_simRange) m_simRange.Set(range);
}
double ADXL345_SPI::GetX() { return GetAcceleration(kAxis_X); }
@@ -52,6 +61,9 @@ double ADXL345_SPI::GetY() { return GetAcceleration(kAxis_Y); }
double ADXL345_SPI::GetZ() { return GetAcceleration(kAxis_Z); }
double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
if (axis == kAxis_X && m_simX) return m_simX.Get();
if (axis == kAxis_Y && m_simY) return m_simY.Get();
if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
uint8_t buffer[3];
uint8_t command[3] = {0, 0, 0};
command[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister) +
@@ -64,7 +76,14 @@ double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
}
ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() {
AllAxes data = AllAxes();
AllAxes data;
if (m_simX && m_simY && m_simZ) {
data.XAxis = m_simX.Get();
data.YAxis = m_simY.Get();
data.ZAxis = m_simZ.Get();
return data;
}
uint8_t dataBuffer[7] = {0, 0, 0, 0, 0, 0, 0};
int16_t rawData[3];

45
wpilibc/src/main/native/cpp/ADXL362.cpp
View File

@@ -34,23 +34,34 @@ static constexpr int kPowerCtl_Measure = 0x02;
ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {}
ADXL362::ADXL362(SPI::Port port, Range range) : m_spi(port) {
ADXL362::ADXL362(SPI::Port port, Range range)
: m_spi(port), m_simDevice("ADXL362", port) {
if (m_simDevice) {
m_simRange =
m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
}
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnTrailingEdge();
m_spi.SetClockActiveLow();
m_spi.SetChipSelectActiveLow();
// Validate the part ID
uint8_t commands[3];
commands[0] = kRegRead;
commands[1] = kPartIdRegister;
commands[2] = 0;
m_spi.Transaction(commands, commands, 3);
if (commands[2] != 0xF2) {
DriverStation::ReportError("could not find ADXL362");
m_gsPerLSB = 0.0;
return;
if (!m_simDevice) {
// Validate the part ID
commands[0] = kRegRead;
commands[1] = kPartIdRegister;
commands[2] = 0;
m_spi.Transaction(commands, commands, 3);
if (commands[2] != 0xF2) {
DriverStation::ReportError("could not find ADXL362");
m_gsPerLSB = 0.0;
return;
}
}
SetRange(range);
@@ -90,6 +101,8 @@ void ADXL362::SetRange(Range range) {
commands[2] =
kFilterCtl_ODR_100Hz | static_cast<uint8_t>((range & 0x03) << 6);
m_spi.Write(commands, 3);
if (m_simRange) m_simRange.Set(range);
}
double ADXL362::GetX() { return GetAcceleration(kAxis_X); }
@@ -101,6 +114,10 @@ double ADXL362::GetZ() { return GetAcceleration(kAxis_Z); }
double ADXL362::GetAcceleration(ADXL362::Axes axis) {
if (m_gsPerLSB == 0.0) return 0.0;
if (axis == kAxis_X && m_simX) return m_simX.Get();
if (axis == kAxis_Y && m_simY) return m_simY.Get();
if (axis == kAxis_Z && m_simZ) return m_simZ.Get();
uint8_t buffer[4];
uint8_t command[4] = {0, 0, 0, 0};
command[0] = kRegRead;
@@ -113,11 +130,17 @@ double ADXL362::GetAcceleration(ADXL362::Axes axis) {
}
ADXL362::AllAxes ADXL362::GetAccelerations() {
AllAxes data = AllAxes();
AllAxes data;
if (m_gsPerLSB == 0.0) {
data.XAxis = data.YAxis = data.ZAxis = 0.0;
return data;
}
if (m_simX && m_simY && m_simZ) {
data.XAxis = m_simX.Get();
data.YAxis = m_simY.Get();
data.ZAxis = m_simZ.Get();
return data;
}
uint8_t dataBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int16_t rawData[3];

36
wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
View File

@@ -31,24 +31,32 @@ static constexpr int kSNLowRegister = 0x10;
ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
: m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
if (m_simDevice) {
m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
}
m_spi.SetClockRate(3000000);
m_spi.SetMSBFirst();
m_spi.SetSampleDataOnLeadingEdge();
m_spi.SetClockActiveHigh();
m_spi.SetChipSelectActiveLow();
// Validate the part ID
if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
DriverStation::ReportError("could not find ADXRS450 gyro");
return;
if (!m_simDevice) {
// Validate the part ID
if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
DriverStation::ReportError("could not find ADXRS450 gyro");
return;
}
m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu,
0x04000000u, 10u, 16u, true, true);
Calibrate();
}
m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu, 0x04000000u,
10u, 16u, true, true);
Calibrate();
HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
@@ -88,15 +96,21 @@ uint16_t ADXRS450_Gyro::ReadRegister(int reg) {
}
double ADXRS450_Gyro::GetAngle() const {
if (m_simAngle) return m_simAngle.Get();
return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
}
double ADXRS450_Gyro::GetRate() const {
if (m_simRate) return m_simRate.Get();
return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
kDegreePerSecondPerLSB;
}
void ADXRS450_Gyro::Reset() { m_spi.ResetAccumulator(); }
void ADXRS450_Gyro::Reset() {
if (m_simAngle) m_simAngle.Set(0.0);
if (m_simRate) m_simRate.Set(0.0);
m_spi.ResetAccumulator();
}
void ADXRS450_Gyro::Calibrate() {
Wait(0.1);

19
wpilibc/src/main/native/cpp/Ultrasonic.cpp
View File

@@ -98,7 +98,10 @@ void Ultrasonic::Ping() {
m_pingChannel->Pulse(kPingTime);
}
bool Ultrasonic::IsRangeValid() const { return m_counter.Get() > 1; }
bool Ultrasonic::IsRangeValid() const {
if (m_simRangeValid) return m_simRangeValid.Get();
return m_counter.Get() > 1;
}
void Ultrasonic::SetAutomaticMode(bool enabling) {
if (enabling == m_automaticEnabled) return; // ignore the case of no change
@@ -135,10 +138,12 @@ void Ultrasonic::SetAutomaticMode(bool enabling) {
}
double Ultrasonic::GetRangeInches() const {
if (IsRangeValid())
if (IsRangeValid()) {
if (m_simRange) return m_simRange.Get();
return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
else
} else {
return 0;
}
}
double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
@@ -177,6 +182,14 @@ void Ultrasonic::InitSendable(SendableBuilder& builder) {
}
void Ultrasonic::Initialize() {
m_simDevice = hal::SimDevice("Ultrasonic", m_echoChannel->GetChannel());
if (m_simDevice) {
m_simRangeValid = m_simDevice.CreateBoolean("Range Valid", false, true);
m_simRange = m_simDevice.CreateDouble("Range (in)", false, 0.0);
m_pingChannel->SetSimDevice(m_simDevice);
m_echoChannel->SetSimDevice(m_simDevice);
}
bool originalMode = m_automaticEnabled;
SetAutomaticMode(false); // Kill task when adding a new sensor
// Link this instance on the list

8
wpilibc/src/main/native/include/frc/ADXL345_I2C.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <hal/SimDevice.h>
#include "frc/ErrorBase.h"
#include "frc/I2C.h"
#include "frc/interfaces/Accelerometer.h"
@@ -78,6 +80,12 @@ class ADXL345_I2C : public ErrorBase,
protected:
I2C m_i2c;
hal::SimDevice m_simDevice;
hal::SimEnum m_simRange;
hal::SimDouble m_simX;
hal::SimDouble m_simY;
hal::SimDouble m_simZ;
static constexpr int kAddress = 0x1D;
static constexpr int kPowerCtlRegister = 0x2D;
static constexpr int kDataFormatRegister = 0x31;

8
wpilibc/src/main/native/include/frc/ADXL345_SPI.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <hal/SimDevice.h>
#include "frc/ErrorBase.h"
#include "frc/SPI.h"
#include "frc/interfaces/Accelerometer.h"
@@ -74,6 +76,12 @@ class ADXL345_SPI : public ErrorBase,
protected:
SPI m_spi;
hal::SimDevice m_simDevice;
hal::SimEnum m_simRange;
hal::SimDouble m_simX;
hal::SimDouble m_simY;
hal::SimDouble m_simZ;
static constexpr int kPowerCtlRegister = 0x2D;
static constexpr int kDataFormatRegister = 0x31;
static constexpr int kDataRegister = 0x32;

7
wpilibc/src/main/native/include/frc/ADXL362.h
View File

@@ -7,6 +7,8 @@
#pragma once
#include <hal/SimDevice.h>
#include "frc/ErrorBase.h"
#include "frc/SPI.h"
#include "frc/interfaces/Accelerometer.h"
@@ -81,6 +83,11 @@ class ADXL362 : public ErrorBase,
private:
SPI m_spi;
hal::SimDevice m_simDevice;
hal::SimEnum m_simRange;
hal::SimDouble m_simX;
hal::SimDouble m_simY;
hal::SimDouble m_simZ;
double m_gsPerLSB = 0.001;
};

6
wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
View File

@@ -9,6 +9,8 @@
#include <stdint.h>
#include <hal/SimDevice.h>
#include "frc/GyroBase.h"
#include "frc/SPI.h"
@@ -94,6 +96,10 @@ class ADXRS450_Gyro : public GyroBase {
private:
SPI m_spi;
hal::SimDevice m_simDevice;
hal::SimDouble m_simAngle;
hal::SimDouble m_simRate;
uint16_t ReadRegister(int reg);
};

6
wpilibc/src/main/native/include/frc/Ultrasonic.h
View File

@@ -12,6 +12,8 @@
#include <thread>
#include <vector>
#include <hal/SimDevice.h>
#include "frc/Counter.h"
#include "frc/ErrorBase.h"
#include "frc/PIDSource.h"
@@ -230,6 +232,10 @@ class Ultrasonic : public ErrorBase,
bool m_enabled = false;
Counter m_counter;
DistanceUnit m_units;
hal::SimDevice m_simDevice;
hal::SimBoolean m_simRangeValid;
hal::SimDouble m_simRange;
};
} // namespace frc

2
wpilibcExamples/build.gradle
View File

@@ -100,7 +100,6 @@ model {
lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared'
if (binary.targetPlatform.name != nativeUtils.wpi.platforms.roborio) {
lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}
@@ -150,7 +149,6 @@ model {
}
if (binary.targetPlatform.name != nativeUtils.wpi.platforms.roborio) {
lib project: ':simulation:halsim_lowfi', library: 'halsim_lowfi', linkage: 'shared'
lib project: ':simulation:halsim_adx_gyro_accelerometer', library: 'halsim_adx_gyro_accelerometer', linkage: 'shared'
lib project: ':simulation:halsim_print', library: 'halsim_print', linkage: 'shared'
lib project: ':simulation:halsim_ds_nt', library: 'halsim_ds_nt', linkage: 'shared'
}

46
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_I2C.java
View File

@@ -13,6 +13,9 @@ import java.nio.ByteOrder;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.interfaces.Accelerometer;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
@@ -64,6 +67,12 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
protected I2C m_i2c;
protected SimDevice m_simDevice;
protected SimEnum m_simRange;
protected SimDouble m_simX;
protected SimDouble m_simY;
protected SimDouble m_simZ;
/**
* Constructs the ADXL345 Accelerometer with I2C address 0x1D.
*
@@ -84,6 +93,15 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
public ADXL345_I2C(I2C.Port port, Range range, int deviceAddress) {
m_i2c = new I2C(port, deviceAddress);
// simulation
m_simDevice = SimDevice.create("ADXL345_I2C", port.value, deviceAddress);
if (m_simDevice != null) {
m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
}
// Turn on the measurements
m_i2c.write(kPowerCtlRegister, kPowerCtl_Measure);
@@ -95,7 +113,14 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
@Override
public void close() {
m_i2c.close();
if (m_i2c != null) {
m_i2c.close();
m_i2c = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
@Override
@@ -121,6 +146,10 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
// Specify the data format to read
m_i2c.write(kDataFormatRegister, kDataFormat_FullRes | value);
if (m_simRange != null) {
m_simRange.set(value);
}
}
@Override
@@ -145,6 +174,15 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
* @return Acceleration of the ADXL345 in Gs.
*/
public double getAcceleration(Axes axis) {
if (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
ByteBuffer rawAccel = ByteBuffer.allocate(2);
m_i2c.read(kDataRegister + axis.value, 2, rawAccel);
@@ -160,6 +198,12 @@ public class ADXL345_I2C implements Accelerometer, Sendable, AutoCloseable {
*/
public AllAxes getAccelerations() {
AllAxes data = new AllAxes();
if (m_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
ByteBuffer rawData = ByteBuffer.allocate(6);
m_i2c.read(kDataRegister, 6, rawData);

45
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
View File

@@ -13,6 +13,9 @@ import java.nio.ByteOrder;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.interfaces.Accelerometer;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
@@ -67,6 +70,12 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
protected SPI m_spi;
protected SimDevice m_simDevice;
protected SimEnum m_simRange;
protected SimDouble m_simX;
protected SimDouble m_simY;
protected SimDouble m_simZ;
/**
* Constructor.
*
@@ -75,13 +84,28 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
*/
public ADXL345_SPI(SPI.Port port, Range range) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("ADXL345_SPI", port.value);
if (m_simDevice != null) {
m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
}
init(range);
SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
}
@Override
public void close() {
m_spi.close();
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
@@ -131,6 +155,10 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
// Specify the data format to read
byte[] commands = new byte[]{kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
m_spi.write(commands, commands.length);
if (m_simRange != null) {
m_simRange.set(value);
}
}
@Override
@@ -155,6 +183,15 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
* @return Acceleration of the ADXL345 in Gs.
*/
public double getAcceleration(ADXL345_SPI.Axes axis) {
if (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
ByteBuffer transferBuffer = ByteBuffer.allocate(3);
transferBuffer.put(0,
(byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
@@ -172,6 +209,12 @@ public class ADXL345_SPI implements Accelerometer, Sendable, AutoCloseable {
*/
public ADXL345_SPI.AllAxes getAccelerations() {
ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
if (m_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
if (m_spi != null) {
ByteBuffer dataBuffer = ByteBuffer.allocate(7);
// Select the data address.

62
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
View File

@@ -12,6 +12,9 @@ import java.nio.ByteOrder;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.NetworkTableEntry;
import edu.wpi.first.wpilibj.interfaces.Accelerometer;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
@@ -62,6 +65,13 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
}
private SPI m_spi;
private SimDevice m_simDevice;
private SimEnum m_simRange;
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
private double m_gsPerLSB;
/**
@@ -82,22 +92,33 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
public ADXL362(SPI.Port port, Range range) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("ADXL362", port.value);
if (m_simDevice != null) {
m_simRange = m_simDevice.createEnum("Range", true, new String[] {"2G", "4G", "8G", "16G"}, 0);
m_simX = m_simDevice.createDouble("X Accel", false, 0.0);
m_simX = m_simDevice.createDouble("Y Accel", false, 0.0);
m_simZ = m_simDevice.createDouble("Z Accel", false, 0.0);
}
m_spi.setClockRate(3000000);
m_spi.setMSBFirst();
m_spi.setSampleDataOnTrailingEdge();
m_spi.setClockActiveLow();
m_spi.setChipSelectActiveLow();
// Validate the part ID
ByteBuffer transferBuffer = ByteBuffer.allocate(3);
transferBuffer.put(0, kRegRead);
transferBuffer.put(1, kPartIdRegister);
m_spi.transaction(transferBuffer, transferBuffer, 3);
if (transferBuffer.get(2) != (byte) 0xF2) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
return;
if (m_simDevice == null) {
// Validate the part ID
transferBuffer.put(0, kRegRead);
transferBuffer.put(1, kPartIdRegister);
m_spi.transaction(transferBuffer, transferBuffer, 3);
if (transferBuffer.get(2) != (byte) 0xF2) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
return;
}
}
setRange(range);
@@ -118,6 +139,10 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
@Override
@@ -150,6 +175,10 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
byte[] commands = new byte[]{kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz
| value)};
m_spi.write(commands, commands.length);
if (m_simRange != null) {
m_simRange.set(value);
}
}
@@ -175,6 +204,15 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
* @return Acceleration of the ADXL362 in Gs.
*/
public double getAcceleration(ADXL362.Axes axis) {
if (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
if (m_spi == null) {
return 0.0;
}
@@ -195,6 +233,12 @@ public class ADXL362 implements Accelerometer, Sendable, AutoCloseable {
*/
public ADXL362.AllAxes getAccelerations() {
ADXL362.AllAxes data = new ADXL362.AllAxes();
if (m_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
if (m_spi != null) {
ByteBuffer dataBuffer = ByteBuffer.allocate(8);
// Select the data address.

66
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
View File

@@ -12,6 +12,9 @@ import java.nio.ByteOrder;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -42,6 +45,11 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
private SPI m_spi;
private SimDevice m_simDevice;
private SimBoolean m_simConnected;
private SimDouble m_simAngle;
private SimDouble m_simRate;
/**
* Constructor. Uses the onboard CS0.
*/
@@ -57,31 +65,49 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
public ADXRS450_Gyro(SPI.Port port) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("ADXRS450_Gyro", port.value);
if (m_simDevice != null) {
m_simConnected = m_simDevice.createBoolean("Connected", false, true);
m_simAngle = m_simDevice.createDouble("Angle", false, 0.0);
m_simRate = m_simDevice.createDouble("Rate", false, 0.0);
}
m_spi.setClockRate(3000000);
m_spi.setMSBFirst();
m_spi.setSampleDataOnLeadingEdge();
m_spi.setClockActiveHigh();
m_spi.setChipSelectActiveLow();
// Validate the part ID
if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
false);
return;
if (m_simDevice == null) {
// Validate the part ID
if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value,
false);
return;
}
m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
true, true);
calibrate();
}
m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16,
true, true);
calibrate();
HAL.report(tResourceType.kResourceType_ADXRS450, port.value);
SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
}
/**
* Determine if the gyro is connected.
*
* @return true if it is connected, false otherwise.
*/
public boolean isConnected() {
if (m_simConnected != null) {
return m_simConnected.get();
}
return m_spi != null;
}
@@ -133,6 +159,12 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
@Override
public void reset() {
if (m_simAngle != null) {
m_simAngle.set(0.0);
}
if (m_simRate != null) {
m_simRate.set(0.0);
}
if (m_spi != null) {
m_spi.resetAccumulator();
}
@@ -147,10 +179,17 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
@Override
public double getAngle() {
if (m_simAngle != null) {
return m_simAngle.get();
}
if (m_spi == null) {
return 0.0;
}
@@ -159,6 +198,9 @@ public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, Sendable
@Override
public double getRate() {
if (m_simRate != null) {
return m_simRate.get();
}
if (m_spi == null) {
return 0.0;
}

25
wpilibj/src/main/java/edu/wpi/first/wpilibj/Ultrasonic.java
View File

@@ -12,6 +12,9 @@ import java.util.List;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.smartdashboard.SendableBuilder;
import edu.wpi.first.wpilibj.smartdashboard.SendableRegistry;
@@ -59,6 +62,10 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
private static int m_instances;
protected PIDSourceType m_pidSource = PIDSourceType.kDisplacement;
private SimDevice m_simDevice;
private SimBoolean m_simRangeValid;
private SimDouble m_simRange;
/**
* Background task that goes through the list of ultrasonic sensors and pings each one in turn.
* The counter is configured to read the timing of the returned echo pulse.
@@ -94,6 +101,13 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
* then automatic mode is restored.
*/
private synchronized void initialize() {
m_simDevice = SimDevice.create("Ultrasonic", m_echoChannel.getChannel());
if (m_simDevice != null) {
m_simRangeValid = m_simDevice.createBoolean("Range Valid", false, true);
m_simRange = m_simDevice.createDouble("Range (in)", false, 0.0);
m_pingChannel.setSimDevice(m_simDevice);
m_echoChannel.setSimDevice(m_simDevice);
}
if (m_task == null) {
m_task = new UltrasonicChecker();
}
@@ -216,6 +230,11 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
if (!m_sensors.isEmpty() && wasAutomaticMode) {
setAutomaticMode(true);
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
@@ -285,6 +304,9 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
* @return true if the range is valid
*/
public boolean isRangeValid() {
if (m_simRangeValid != null) {
return m_simRangeValid.get();
}
return m_counter.get() > 1;
}
@@ -296,6 +318,9 @@ public class Ultrasonic implements PIDSource, Sendable, AutoCloseable {
*/
public double getRangeInches() {
if (isRangeValid()) {
if (m_simRange != null) {
return m_simRange.get();
}
return m_counter.getPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
} else {
return 0;