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

Add simple motor simulation classes (#1117)

Browse Source
This commit is contained in:
PJ Reiniger
2018-07-12 23:11:26 -04:00
committed by Peter Johnson
parent 57fc614074
commit 76c901ce78
22 changed files with 820 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
simulation/lowfi_simulation/src/dev/native/cpp/main.cpp Normal file
View File

@@ -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 <iostream>
#include <HAL/HAL.h>
int main() {
std::cout << "Hello World" << std::endl;
std::cout << HAL_GetRuntimeType() << std::endl;
}

25
simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorEncoderConnector.cpp Normal file
View File

@@ -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

40
simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/MotorModel/SimpleMotorModel.cpp Normal file
View File

@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#include "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

27
simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiEncoderSim.cpp Normal file
View File

@@ -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<int>(position / m_encoderSimulator.GetDistancePerPulse()));
}
void WpiEncoderSim::SetVelocity(double velocity) {
m_encoderSimulator.SetPeriod(1.0 / velocity);
}
} // namespace lowfi
} // namespace sim
} // namespace frc

37
simulation/lowfi_simulation/src/main/native/cpp/LowFiSim/WpiSimulators/WpiMotorSim.cpp Normal file
View File

@@ -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

22
simulation/lowfi_simulation/src/main/native/include/LowFiSim/EncoderSim.h Normal file
View File

@@ -0,0 +1,22 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
namespace frc {
namespace sim {
namespace lowfi {
class EncoderSim {
public:
virtual void SetPosition(double position) = 0;
virtual void SetVelocity(double velocity) = 0;
};
} // namespace lowfi
} // namespace sim
} // namespace frc

30
simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorEncoderConnector.h Normal file
View File

@@ -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

28
simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/MotorModel.h Normal file
View File

@@ -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

40
simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorModel/SimpleMotorModel.h Normal file
View File

@@ -0,0 +1,40 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#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

23
simulation/lowfi_simulation/src/main/native/include/LowFiSim/MotorSim.h Normal file
View File

@@ -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

30
simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiEncoderSim.h Normal file
View File

@@ -0,0 +1,30 @@
/*----------------------------------------------------------------------------*/
/* Copyright (c) 2008-2018 FIRST. All Rights Reserved. */
/* Open Source Software - may be modified and shared by FRC teams. The code */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project. */
/*----------------------------------------------------------------------------*/
#pragma once
#include "LowFiSim/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

36
simulation/lowfi_simulation/src/main/native/include/LowFiSim/WpiSimulators/WpiMotorSim.h Normal file
View File

@@ -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

106
simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorEncoderSimulatorTest.cpp Normal file
View File

@@ -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());
}

33
simulation/lowfi_simulation/src/test/native/cpp/LowFiSim/MotorModel/SimpleMotorModelSimulationTest.cpp Normal file
View File

@@ -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());
}

14
simulation/lowfi_simulation/src/test/native/cpp/main.cpp Normal file
View File

@@ -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;
}