From 76c901ce784c6c76c030c8997be2b841780c60b7 Mon Sep 17 00:00:00 2001 From: PJ Reiniger Date: Thu, 12 Jul 2018 23:11:26 -0400 Subject: [PATCH] Add simple motor simulation classes (#1117) --- .../native/include/MockData/EncoderData.h | 7 + .../native/include/Simulation/EncoderSim.h | 15 ++ .../main/native/sim/MockData/EncoderData.cpp | 21 +- .../native/sim/MockData/EncoderDataInternal.h | 2 +- settings.gradle | 1 + simulation/lowfi_simulation/build.gradle | 189 ++++++++++++++++++ simulation/lowfi_simulation/publish.gradle | 81 ++++++++ .../src/dev/native/cpp/main.cpp | 15 ++ .../cpp/LowFiSim/MotorEncoderConnector.cpp | 25 +++ .../LowFiSim/MotorModel/SimpleMotorModel.cpp | 40 ++++ .../LowFiSim/WpiSimulators/WpiEncoderSim.cpp | 27 +++ .../LowFiSim/WpiSimulators/WpiMotorSim.cpp | 37 ++++ .../main/native/include/LowFiSim/EncoderSim.h | 22 ++ .../include/LowFiSim/MotorEncoderConnector.h | 30 +++ .../include/LowFiSim/MotorModel/MotorModel.h | 28 +++ .../LowFiSim/MotorModel/SimpleMotorModel.h | 40 ++++ .../main/native/include/LowFiSim/MotorSim.h | 23 +++ .../LowFiSim/WpiSimulators/WpiEncoderSim.h | 30 +++ .../LowFiSim/WpiSimulators/WpiMotorSim.h | 36 ++++ .../LowFiSim/MotorEncoderSimulatorTest.cpp | 106 ++++++++++ .../SimpleMotorModelSimulationTest.cpp | 33 +++ .../src/test/native/cpp/main.cpp | 14 ++ 22 files changed, 820 insertions(+), 2 deletions(-) create mode 100644 simulation/lowfi_simulation/build.gradle create mode 100644 simulation/lowfi_simulation/publish.gradle create mode 100644 simulation/lowfi_simulation/src/dev/native/cpp/main.cpp create mode 100644 simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorEncoderConnector.cpp create mode 100644 simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorModel/SimpleMotorModel.cpp create mode 100644 simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiEncoderSim.cpp create mode 100644 simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiMotorSim.cpp create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/EncoderSim.h create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorEncoderConnector.h create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/MotorModel.h create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/SimpleMotorModel.h create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorSim.h create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiEncoderSim.h create mode 100644 simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiMotorSim.h create mode 100644 simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorEncoderSimulatorTest.cpp create mode 100644 simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorModel/SimpleMotorModelSimulationTest.cpp create mode 100644 simulation/lowfi_simulation/src/test/native/cpp/main.cpp diff --git a/hal/src/main/native/include/MockData/EncoderData.h b/hal/src/main/native/include/MockData/EncoderData.h index 81ab8a5506..4dea5c8316 100644 --- a/hal/src/main/native/include/MockData/EncoderData.h +++ b/hal/src/main/native/include/MockData/EncoderData.h @@ -81,6 +81,13 @@ void HALSIM_CancelEncoderSamplesToAverageCallback(int32_t index, int32_t uid); int32_t HALSIM_GetEncoderSamplesToAverage(int32_t index); void HALSIM_SetEncoderSamplesToAverage(int32_t index, int32_t samplesToAverage); +int32_t HALSIM_RegisterEncoderDistancePerPulseCallback( + int32_t index, HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify); +void HALSIM_CancelEncoderDistancePerPulseCallback(int32_t index, int32_t uid); +double HALSIM_GetEncoderDistancePerPulse(int32_t index); +void HALSIM_SetEncoderDistancePerPulse(int32_t index, double distancePerPulse); + void HALSIM_RegisterEncoderAllCallbacks(int32_t index, HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify); diff --git a/hal/src/main/native/include/Simulation/EncoderSim.h b/hal/src/main/native/include/Simulation/EncoderSim.h index 2cf891653e..2446fcb9d0 100644 --- a/hal/src/main/native/include/Simulation/EncoderSim.h +++ b/hal/src/main/native/include/Simulation/EncoderSim.h @@ -123,6 +123,21 @@ class EncoderSim { HALSIM_SetEncoderSamplesToAverage(m_index, samplesToAverage); } + std::unique_ptr RegisterDistancePerPulseCallback( + NotifyCallback callback, bool initialNotify) { + auto store = std::make_unique( + m_index, -1, callback, &HALSIM_CancelEncoderDistancePerPulseCallback); + store->SetUid(HALSIM_RegisterEncoderDistancePerPulseCallback( + m_index, &CallbackStoreThunk, store.get(), initialNotify)); + return store; + } + double GetDistancePerPulse() { + return HALSIM_GetEncoderDistancePerPulse(m_index); + } + void SetDistancePerPulse(double distancePerPulse) { + HALSIM_SetEncoderDistancePerPulse(m_index, distancePerPulse); + } + void ResetData() { HALSIM_ResetEncoderData(m_index); } private: diff --git a/hal/src/main/native/sim/MockData/EncoderData.cpp b/hal/src/main/native/sim/MockData/EncoderData.cpp index 497aff26d1..2732431da1 100644 --- a/hal/src/main/native/sim/MockData/EncoderData.cpp +++ b/hal/src/main/native/sim/MockData/EncoderData.cpp @@ -39,7 +39,7 @@ void EncoderData::ResetData() { m_reverseDirectionCallbacks = nullptr; m_samplesToAverage = 0; m_samplesToAverageCallbacks = nullptr; - m_distancePerPulse = 0; + m_distancePerPulse = 1; m_distancePerPulseCallbacks = nullptr; } @@ -544,6 +544,25 @@ void HALSIM_SetEncoderSamplesToAverage(int32_t index, SimEncoderData[index].SetSamplesToAverage(samplesToAverage); } +int32_t HALSIM_RegisterEncoderDistancePerPulseCallback( + int32_t index, HAL_NotifyCallback callback, void* param, + HAL_Bool initialNotify) { + return SimEncoderData[index].RegisterDistancePerPulseCallback(callback, param, + initialNotify); +} + +void HALSIM_CancelEncoderDistancePerPulseCallback(int32_t index, int32_t uid) { + SimEncoderData[index].CancelDistancePerPulseCallback(uid); +} + +double HALSIM_GetEncoderDistancePerPulse(int32_t index) { + return SimEncoderData[index].GetDistancePerPulse(); +} + +void HALSIM_SetEncoderDistancePerPulse(int32_t index, double distancePerPulse) { + SimEncoderData[index].SetDistancePerPulse(distancePerPulse); +} + void HALSIM_RegisterEncoderAllCallbacks(int32_t index, HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) { diff --git a/hal/src/main/native/sim/MockData/EncoderDataInternal.h b/hal/src/main/native/sim/MockData/EncoderDataInternal.h index 42789d8842..11f2f4163b 100644 --- a/hal/src/main/native/sim/MockData/EncoderDataInternal.h +++ b/hal/src/main/native/sim/MockData/EncoderDataInternal.h @@ -106,7 +106,7 @@ class EncoderData { std::shared_ptr m_reverseDirectionCallbacks = nullptr; std::atomic m_samplesToAverage{0}; std::shared_ptr m_samplesToAverageCallbacks = nullptr; - std::atomic m_distancePerPulse{0}; + std::atomic m_distancePerPulse{1}; std::shared_ptr m_distancePerPulseCallbacks = nullptr; }; extern EncoderData* SimEncoderData; diff --git a/settings.gradle b/settings.gradle index aa6c048c3d..f12f105428 100644 --- a/settings.gradle +++ b/settings.gradle @@ -24,5 +24,6 @@ include 'simulation:halsim_ds_nt' include 'simulation:gz_msgs' include 'simulation:frc_gazebo_plugins' include 'simulation:halsim_gazebo' +include 'simulation:lowfi_simulation' include 'cameraserver' include 'myRobot' diff --git a/simulation/lowfi_simulation/build.gradle b/simulation/lowfi_simulation/build.gradle new file mode 100644 index 0000000000..83f45d7d94 --- /dev/null +++ b/simulation/lowfi_simulation/build.gradle @@ -0,0 +1,189 @@ +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 = 'lowfi_sim' +} + +apply from: "${rootDir}/shared/config.gradle" + +if (!project.hasProperty('onlyAthena')) { + + ext { + sharedCvConfigs = [lowfi_simTest: []] + 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 + + + model { + exportsConfigs { + lowfi_sim(ExportsConfig) { + x86ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure', + '_CT??_R0?AVbad_cast', + '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure', + '_TI5?AVfailure'] + x64ExcludeSymbols = ['_CT??_R0?AV_System_error', '_CT??_R0?AVexception', '_CT??_R0?AVfailure', + '_CT??_R0?AVbad_cast', + '_CT??_R0?AVruntime_error', '_CT??_R0?AVsystem_error', '_CTA5?AVfailure', + '_TI5?AVfailure'] + } + } + components { + "${nativeName}Base"(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs = ['src/main/native/cpp', "$buildDir/generated/cpp"] + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include' + } + } + } + binaries.all { + if (it instanceof SharedLibraryBinarySpec) { + it.buildable = false + return + } + if (it.targetPlatform.architecture.name == 'athena') { + it.buildable = false + return + } + lib project: ':hal', library: 'hal', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + project(':ni-libraries').addNiLibrariesToLinker(it) + } + } + "${nativeName}"(NativeLibrarySpec) { + sources { + cpp { + source { + srcDirs "${rootDir}/shared/singlelib" + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include' + } + } + } + binaries.all { + if (it.targetPlatform.architecture.name == 'athena') { + it.buildable = false + return + } + lib project: ':hal', library: 'hal', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + project(':ni-libraries').addNiLibrariesToLinker(it) + } + } + // By default, a development executable will be generated. This is to help the case of + // testing specific functionality of the library. + "${nativeName}Dev"(NativeExecutableSpec) { + sources { + cpp { + source { + srcDirs 'src/dev/native/cpp' + include '**/*.cpp' + lib library: 'lowfi_sim' + } + exportedHeaders { + srcDirs 'src/dev/native/include' + } + } + } + binaries.all { + if (it.targetPlatform.architecture.name == 'athena') { + it.buildable = false + return + } + lib project: ':hal', library: 'hal', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + project(':ni-libraries').addNiLibrariesToLinker(it) + } + } + } + binaries { + withType(GoogleTestTestSuiteBinarySpec) { + lib project: ':ntcore', library: 'ntcore', linkage: 'shared' + lib project: ':cscore', library: 'cscore', linkage: 'shared' + lib project: ':hal', library: 'hal', linkage: 'shared' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'shared' + lib project: ':cameraserver', library: 'cameraserver', linkage: 'shared' + lib project: ':wpilibc', library: 'wpilibc', linkage: 'shared' + project(':ni-libraries').addNiLibrariesToLinker(it) + lib library: nativeName, linkage: 'shared' + } + } + } + + apply from: "publish.gradle" +} + +model { + + testSuites { + if (!project.hasProperty('onlyAthena')) { + "${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 + + found = true + } + } + } + } + } + } + } +} + +tasks.withType(RunTestExecutable) { + args "--gtest_output=xml:test_detail.xml" + outputs.dir outputDir +} diff --git a/simulation/lowfi_simulation/publish.gradle b/simulation/lowfi_simulation/publish.gradle new file mode 100644 index 0000000000..cd16178841 --- /dev/null +++ b/simulation/lowfi_simulation/publish.gradle @@ -0,0 +1,81 @@ +apply plugin: 'maven-publish' + +def pubVersion = '' +if (project.hasProperty("publishVersion")) { + pubVersion = project.publishVersion +} else { + pubVersion = WPILibVersion.version +} + +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 lowfiSimTaskList = createComponentZipTasks($.components, nativeName, zipBaseName, Zip, project, includeStandardZipFormat) + def allTask + if (!project.hasProperty('jenkinsBuild')) { + allTask = createAllCombined(lowfiSimTaskList, nativeName, zipBaseName, Zip, project) + } + + + + publications { + cpp(MavenPublication) { + lowfiSimTaskList.each { + artifact it + } + + if (!project.hasProperty('jenkinsBuild')) { + artifact allTask + } + + artifact cppHeadersZip + artifact cppSourcesZip + + + artifactId = baseArtifactId + groupId artifactGroupId + version pubVersion + } + } + } +} diff --git a/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp b/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp new file mode 100644 index 0000000000..737d601bab --- /dev/null +++ b/simulation/lowfi_simulation/src/dev/native/cpp/main.cpp @@ -0,0 +1,15 @@ +/*----------------------------------------------------------------------------*/ +/* 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 + +#include + +int main() { + std::cout << "Hello World" << std::endl; + std::cout << HAL_GetRuntimeType() << std::endl; +} diff --git a/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorEncoderConnector.cpp b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorEncoderConnector.cpp new file mode 100644 index 0000000000..2e471fbff0 --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorEncoderConnector.cpp @@ -0,0 +1,25 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "LowFiSim/MotorEncoderConnector.h" + +namespace frc { +namespace sim { +namespace lowfi { + +MotorEncoderConnector::MotorEncoderConnector(MotorSim& motorController, + EncoderSim& encoder) + : motorSimulator(motorController), encoderSimulator(encoder) {} + +void MotorEncoderConnector::Update() { + encoderSimulator.SetPosition(motorSimulator.GetPosition()); + encoderSimulator.SetVelocity(motorSimulator.GetVelocity()); +} + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorModel/SimpleMotorModel.cpp b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorModel/SimpleMotorModel.cpp new file mode 100644 index 0000000000..d3a0b5871d --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorModel/SimpleMotorModel.cpp @@ -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 "LowFiSim/MotorModel/SimpleMotorModel.h" + +namespace frc { +namespace sim { +namespace lowfi { + +SimpleMotorModel::SimpleMotorModel(double maxSpeed) : m_maxSpeed(maxSpeed) {} + +void SimpleMotorModel::Reset() { + m_position = 0; + m_velocity = 0; +} + +void SimpleMotorModel::SetVoltage(double voltage) { + m_voltagePercentage = voltage / kMaxExpectedVoltage; +} + +void SimpleMotorModel::Update(double elapsedTime) { + m_velocity = m_maxSpeed * m_voltagePercentage; + m_position += m_velocity * elapsedTime; +} + +double SimpleMotorModel::GetPosition() const { return m_position; } + +double SimpleMotorModel::GetVelocity() const { return m_velocity; } + +double SimpleMotorModel::GetAcceleration() const { return 0; } + +double SimpleMotorModel::GetCurrent() const { return 0; } + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiEncoderSim.cpp b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiEncoderSim.cpp new file mode 100644 index 0000000000..0cec44333d --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiEncoderSim.cpp @@ -0,0 +1,27 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "LowFiSim/WpiSimulators/WpiEncoderSim.h" + +namespace frc { +namespace sim { +namespace lowfi { + +WpiEncoderSim::WpiEncoderSim(int index) : m_encoderSimulator(index) {} + +void WpiEncoderSim::SetPosition(double position) { + m_encoderSimulator.SetCount( + static_cast(position / m_encoderSimulator.GetDistancePerPulse())); +} + +void WpiEncoderSim::SetVelocity(double velocity) { + m_encoderSimulator.SetPeriod(1.0 / velocity); +} + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiMotorSim.cpp b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiMotorSim.cpp new file mode 100644 index 0000000000..e82ea12220 --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiMotorSim.cpp @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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/WpiMotorSim.h" + +namespace frc { +namespace sim { +namespace lowfi { + +WpiMotorSim::WpiMotorSim(int index, MotorModel& motorModelSimulator) + : m_motorModelSimulation(motorModelSimulator), m_pwmSimulator(index) {} + +void WpiMotorSim::Update(double elapsedTime) { + m_motorModelSimulation.SetVoltage(m_pwmSimulator.GetSpeed() * + kDefaultVoltage); + m_motorModelSimulation.Update(elapsedTime); +} + +double WpiMotorSim::GetPosition() const { + return m_motorModelSimulation.GetPosition(); +} + +double WpiMotorSim::GetVelocity() const { + return m_motorModelSimulation.GetVelocity(); +} + +double WpiMotorSim::GetAcceleration() const { + return m_motorModelSimulation.GetAcceleration(); +} + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/EncoderSim.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/EncoderSim.h new file mode 100644 index 0000000000..b17aff413c --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/EncoderSim.h @@ -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 EncoderSim { + public: + virtual void SetPosition(double position) = 0; + virtual void SetVelocity(double velocity) = 0; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorEncoderConnector.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorEncoderConnector.h new file mode 100644 index 0000000000..0cf48734e9 --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorEncoderConnector.h @@ -0,0 +1,30 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "EncoderSim.h" +#include "MotorSim.h" + +namespace frc { +namespace sim { +namespace lowfi { + +class MotorEncoderConnector { + public: + MotorEncoderConnector(MotorSim& motorController, EncoderSim& encoder); + + void Update(); + + private: + MotorSim& motorSimulator; + EncoderSim& encoderSimulator; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/MotorModel.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/MotorModel.h new file mode 100644 index 0000000000..aeffcb78dc --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/MotorModel.h @@ -0,0 +1,28 @@ +/*----------------------------------------------------------------------------*/ +/* 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 MotorModel { + public: + virtual void Reset() = 0; + virtual void SetVoltage(double voltage) = 0; + virtual void Update(double elapsedTime) = 0; + + virtual double GetPosition() const = 0; + virtual double GetVelocity() const = 0; + virtual double GetAcceleration() const = 0; + virtual double GetCurrent() const = 0; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/SimpleMotorModel.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/SimpleMotorModel.h new file mode 100644 index 0000000000..10384039fe --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/SimpleMotorModel.h @@ -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. */ +/*----------------------------------------------------------------------------*/ + +#pragma once + +#include "LowFiSim/MotorModel/MotorModel.h" + +namespace frc { +namespace sim { +namespace lowfi { + +class SimpleMotorModel : public MotorModel { + public: + explicit SimpleMotorModel(double maxSpeed); + + void Reset() override; + void SetVoltage(double voltage) override; + void Update(double elapsedTime) override; + + double GetPosition() const override; + double GetVelocity() const override; + double GetAcceleration() const override; + double GetCurrent() const override; + + protected: + double m_maxSpeed; + double m_voltagePercentage{0}; + double m_position{0}; + double m_velocity{0}; + + static constexpr double kMaxExpectedVoltage = 12; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorSim.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorSim.h new file mode 100644 index 0000000000..0be2fa2723 --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorSim.h @@ -0,0 +1,23 @@ +/*----------------------------------------------------------------------------*/ +/* 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 MotorSim { + public: + virtual double GetPosition() const = 0; + virtual double GetVelocity() const = 0; + virtual double GetAcceleration() const = 0; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiEncoderSim.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiEncoderSim.h new file mode 100644 index 0000000000..484c30cad0 --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiEncoderSim.h @@ -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/EncoderSim.h" +#include "Simulation/EncoderSim.h" + +namespace frc { +namespace sim { +namespace lowfi { + +class WpiEncoderSim : public EncoderSim { + public: + explicit WpiEncoderSim(int index); + + void SetPosition(double position) override; + void SetVelocity(double velocity) override; + + protected: + frc::sim::EncoderSim m_encoderSimulator; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiMotorSim.h b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiMotorSim.h new file mode 100644 index 0000000000..8ca4d9d5a4 --- /dev/null +++ b/simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiMotorSim.h @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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/MotorModel/MotorModel.h" +#include "LowFiSim/MotorSim.h" +#include "Simulation/PWMSim.h" + +namespace frc { +namespace sim { +namespace lowfi { + +class WpiMotorSim : public MotorSim { + public: + explicit WpiMotorSim(int index, MotorModel& motorModelSimulator); + + void Update(double elapsedTime); + double GetPosition() const override; + double GetVelocity() const override; + double GetAcceleration() const override; + + private: + MotorModel& m_motorModelSimulation; + frc::sim::PWMSim m_pwmSimulator; + + static constexpr double kDefaultVoltage = 12.0; +}; + +} // namespace lowfi +} // namespace sim +} // namespace frc diff --git a/simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorEncoderSimulatorTest.cpp b/simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorEncoderSimulatorTest.cpp new file mode 100644 index 0000000000..71e8fe7651 --- /dev/null +++ b/simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorEncoderSimulatorTest.cpp @@ -0,0 +1,106 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "Encoder.h" +#include "LowFiSim/MotorEncoderConnector.h" +#include "LowFiSim/MotorModel/SimpleMotorModel.h" +#include "LowFiSim/WpiSimulators/WpiEncoderSim.h" +#include "LowFiSim/WpiSimulators/WpiMotorSim.h" +#include "Talon.h" +#include "gtest/gtest.h" + +TEST(MotorEncoderConnectorTest, TestWithoutDistancePerPulseFullSpeed) { + frc::Talon talon{3}; + frc::Encoder encoder{3, 1}; + + frc::sim::lowfi::SimpleMotorModel motorModelSim(6000); + frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim); + frc::sim::lowfi::WpiEncoderSim encoderSim(0); + frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim); + + talon.Set(-1); + motorSim.Update(1); + connector.Update(); + + // Position + EXPECT_EQ(-6000, encoder.Get()); + EXPECT_DOUBLE_EQ(-6000, encoder.GetDistance()); + + // Velocity + EXPECT_DOUBLE_EQ(-1.0 / 6000, encoder.GetPeriod()); + EXPECT_DOUBLE_EQ(-6000, encoder.GetRate()); +} + +TEST(MotorEncoderConnectorTest, TestWithoutDistancePerPulseRealisitcUpdate) { + frc::Talon talon{3}; + frc::Encoder encoder{3, 1}; + + frc::sim::lowfi::SimpleMotorModel motorModelSim(6000); + frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim); + frc::sim::lowfi::WpiEncoderSim encoderSim(0); + frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim); + + talon.Set(0.5); + motorSim.Update(.02); + connector.Update(); + + // Position + EXPECT_EQ(60, encoder.Get()); + EXPECT_DOUBLE_EQ(60, encoder.GetDistance()); + + // Velocity + EXPECT_DOUBLE_EQ(1.0 / 3000, encoder.GetPeriod()); + EXPECT_DOUBLE_EQ(3000, encoder.GetRate()); +} + +TEST(MotorEncoderConnectorTest, TestWithDistancePerPulseFullSpeed) { + frc::Talon talon{3}; + frc::Encoder encoder{3, 1}; + encoder.SetDistancePerPulse(.001); + + frc::sim::lowfi::SimpleMotorModel motorModelSim(6000); + frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim); + frc::sim::lowfi::WpiEncoderSim encoderSim(0); + frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim); + + talon.Set(-1); + + motorSim.Update(1); + connector.Update(); + + // Position + EXPECT_EQ(-6000000, encoder.Get()); + EXPECT_DOUBLE_EQ(-6000, encoder.GetDistance()); + + // Velocity + EXPECT_EQ(-1.0 / 6000, encoder.GetPeriod()); + EXPECT_DOUBLE_EQ(-6, encoder.GetRate()); +} + +TEST(MotorEncoderConnectorTest, TestWithDistancePerPulseRealistic) { + frc::Talon talon{3}; + frc::Encoder encoder{3, 1}; + encoder.SetDistancePerPulse(.001); + + frc::sim::lowfi::SimpleMotorModel motorModelSim(6000); + frc::sim::lowfi::WpiMotorSim motorSim(3, motorModelSim); + frc::sim::lowfi::WpiEncoderSim encoderSim(0); + frc::sim::lowfi::MotorEncoderConnector connector(motorSim, encoderSim); + + talon.Set(0.5); + + motorSim.Update(.02); + connector.Update(); + + // Position + EXPECT_EQ(60000, encoder.Get()); + EXPECT_DOUBLE_EQ(60, encoder.GetDistance()); + + // Velocity + EXPECT_EQ(1.0 / 3000, encoder.GetPeriod()); + EXPECT_DOUBLE_EQ(3, encoder.GetRate()); +} diff --git a/simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorModel/SimpleMotorModelSimulationTest.cpp b/simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorModel/SimpleMotorModelSimulationTest.cpp new file mode 100644 index 0000000000..c12328a7cd --- /dev/null +++ b/simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorModel/SimpleMotorModelSimulationTest.cpp @@ -0,0 +1,33 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "LowFiSim/MotorModel/SimpleMotorModel.h" +#include "gtest/gtest.h" + +TEST(SimpleMotorModelSimulationTest, TestSimpleModel) { + frc::sim::lowfi::SimpleMotorModel motorModelSim(200); + + // Test forward voltage + motorModelSim.SetVoltage(6); + motorModelSim.Update(.5); + + EXPECT_DOUBLE_EQ(50, motorModelSim.GetPosition()); + EXPECT_DOUBLE_EQ(100, motorModelSim.GetVelocity()); + + // Test Reset + motorModelSim.Reset(); + EXPECT_DOUBLE_EQ(0, motorModelSim.GetPosition()); + EXPECT_DOUBLE_EQ(0, motorModelSim.GetVelocity()); + + // Test negative voltage + motorModelSim.Reset(); + motorModelSim.SetVoltage(-3); + motorModelSim.Update(.06); + + EXPECT_DOUBLE_EQ(-3, motorModelSim.GetPosition()); + EXPECT_DOUBLE_EQ(-50, motorModelSim.GetVelocity()); +} diff --git a/simulation/lowfi_simulation/src/test/native/cpp/main.cpp b/simulation/lowfi_simulation/src/test/native/cpp/main.cpp new file mode 100644 index 0000000000..1e5ecf004c --- /dev/null +++ b/simulation/lowfi_simulation/src/test/native/cpp/main.cpp @@ -0,0 +1,14 @@ +/*----------------------------------------------------------------------------*/ +/* 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 "gtest/gtest.h" + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + int ret = RUN_ALL_TESTS(); + return ret; +}