diff --git a/firmware/controllers/system/DcMotor.cpp b/firmware/controllers/system/DcMotor.cpp new file mode 100644 index 0000000000..3ef1e0b89a --- /dev/null +++ b/firmware/controllers/system/DcMotor.cpp @@ -0,0 +1,52 @@ +/** + * @file DcMotor.cpp + * @author Matthew Kennedy, (c) 2018 + */ +#include "DcMotor.h" + +TwoPinDcMotor::TwoPinDcMotor(SimplePwm* pwm, OutputPin* dir1, OutputPin* dir2) + : m_dir1(dir1) + , m_dir2(dir2) + , m_pwm(pwm) +{ + +} + +void TwoPinDcMotor::Set(float duty) +{ + bool dir; + + if(duty < 0) + { + dir = false; + duty = -duty; + } + else + { + dir = true; + } + + // Clamp + if(duty > 0.95f) + { + duty = 0.95f; + } + // Disable for very small duty + else if (duty < 0.01f) + { + duty = 0.0f; + } + + if(duty < 0.01f) + { + m_dir1->setValue(false); + m_dir2->setValue(false); + } + else + { + m_dir1->setValue(dir); + m_dir2->setValue(!dir); + } + + m_pwm->setSimplePwmDutyCycle(duty); +} diff --git a/firmware/controllers/system/DcMotor.h b/firmware/controllers/system/DcMotor.h new file mode 100644 index 0000000000..79efa96b81 --- /dev/null +++ b/firmware/controllers/system/DcMotor.h @@ -0,0 +1,25 @@ +/** + * @file DcMotor.h + * @author Matthew Kennedy, (c) 2018 + */ +#pragma once + +#include "pwm_generator_logic.h" + +class DcMotor +{ +public: + virtual void Set(float duty) = 0; +}; + +class TwoPinDcMotor : public DcMotor +{ +private: + OutputPin* const m_dir1; + OutputPin* const m_dir2; + SimplePwm* const m_pwm; +public: + TwoPinDcMotor(SimplePwm* pwm, OutputPin* dir1, OutputPin* dir2); + + virtual void Set(float duty) override; +}; diff --git a/firmware/controllers/system/system.mk b/firmware/controllers/system/system.mk index bffc2e5385..20b90ca958 100644 --- a/firmware/controllers/system/system.mk +++ b/firmware/controllers/system/system.mk @@ -5,4 +5,5 @@ SYSTEMSRC_CPP = $(PROJECT_DIR)/controllers/system/pwm_generator_logic.cpp \ $(PROJECT_DIR)/controllers/system/signal_executor_sleep.cpp \ $(PROJECT_DIR)/controllers/system/event_queue.cpp \ $(PROJECT_DIR)/controllers/system/efiGpio.cpp \ + $(PROJECT_DIR)/controllers/system/DcMotor.cpp \ $(PROJECT_DIR)/controllers/system/SingleTimerExecutor.cpp \ No newline at end of file