diff --git a/andino_firmware/src/app.cpp b/andino_firmware/src/app.cpp index aa606aae..d9d38b52 100644 --- a/andino_firmware/src/app.cpp +++ b/andino_firmware/src/app.cpp @@ -68,6 +68,7 @@ #include "commands.h" #include "constants.h" +#include "digital_out_arduino.h" #include "encoder.h" #include "hw.h" #include "motor.h" @@ -96,10 +97,17 @@ char argv2[16]; namespace andino { -Motor App::left_motor_(Hw::kLeftMotorEnableGpioPin, Hw::kLeftMotorForwardGpioPin, - Hw::kLeftMotorBackwardGpioPin); -Motor App::right_motor_(Hw::kRightMotorEnableGpioPin, Hw::kRightMotorForwardGpioPin, - Hw::kRightMotorBackwardGpioPin); +DigitalOutArduino App::left_motor_enable_gpio_pin_(Hw::kLeftMotorEnableGpioPin); +PwmOutArduino App::left_motor_forward_gpio_pin_(Hw::kLeftMotorForwardGpioPin); +PwmOutArduino App::left_motor_backward_gpio_pin_(Hw::kLeftMotorBackwardGpioPin); +Motor App::left_motor_(&left_motor_enable_gpio_pin_, &left_motor_forward_gpio_pin_, + &left_motor_backward_gpio_pin_); + +DigitalOutArduino App::right_motor_enable_gpio_pin_(Hw::kRightMotorEnableGpioPin); +PwmOutArduino App::right_motor_forward_gpio_pin_(Hw::kRightMotorForwardGpioPin); +PwmOutArduino App::right_motor_backward_gpio_pin_(Hw::kRightMotorBackwardGpioPin); +Motor App::right_motor_(&right_motor_enable_gpio_pin_, &right_motor_forward_gpio_pin_, + &right_motor_backward_gpio_pin_); Encoder App::left_encoder_(Hw::kLeftEncoderChannelAGpioPin, Hw::kLeftEncoderChannelBGpioPin); Encoder App::right_encoder_(Hw::kRightEncoderChannelAGpioPin, Hw::kRightEncoderChannelBGpioPin); @@ -118,9 +126,10 @@ void App::setup() { left_encoder_.init(); right_encoder_.init(); - // Enable motors. - left_motor_.set_state(true); - right_motor_.set_state(true); + left_motor_.begin(); + left_motor_.enable(true); + right_motor_.begin(); + right_motor_.enable(true); left_pid_controller_.reset(left_encoder_.read()); right_pid_controller_.reset(right_encoder_.read()); diff --git a/andino_firmware/src/app.h b/andino_firmware/src/app.h index b0745f6e..5b94e303 100644 --- a/andino_firmware/src/app.h +++ b/andino_firmware/src/app.h @@ -29,9 +29,11 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include "digital_out_arduino.h" #include "encoder.h" #include "motor.h" #include "pid.h" +#include "pwm_out_arduino.h" namespace andino { @@ -88,8 +90,16 @@ class App { // TODO(jballoffet): Parse arguments within callback method. static void cmd_set_pid_tuning_gains(const char* arg1, const char* arg2); - /// Motors (one per wheel). + /// Left wheel motor. + static DigitalOutArduino left_motor_enable_gpio_pin_; + static PwmOutArduino left_motor_forward_gpio_pin_; + static PwmOutArduino left_motor_backward_gpio_pin_; static Motor left_motor_; + + /// Right wheel motor. + static DigitalOutArduino right_motor_enable_gpio_pin_; + static PwmOutArduino right_motor_forward_gpio_pin_; + static PwmOutArduino right_motor_backward_gpio_pin_; static Motor right_motor_; /// Encoders (one per wheel). diff --git a/andino_firmware/src/motor.cpp b/andino_firmware/src/motor.cpp index 1720bdab..313fc1db 100644 --- a/andino_firmware/src/motor.cpp +++ b/andino_firmware/src/motor.cpp @@ -64,17 +64,14 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include "motor.h" -#include +#include "digital_out.h" +#include "pwm_out.h" namespace andino { -void Motor::set_state(bool enabled) { - if (enabled) { - digitalWrite(enable_gpio_pin_, HIGH); - } else { - digitalWrite(enable_gpio_pin_, LOW); - } -} +void Motor::begin() { enable_gpio_pin_->begin(); } + +void Motor::enable(bool enabled) { enable_gpio_pin_->write(enabled ? 1 : 0); } void Motor::set_speed(int speed) { bool forward = true; @@ -89,11 +86,11 @@ void Motor::set_speed(int speed) { // The motor speed is controlled by sending a PWM wave to the corresponding pin. if (forward) { - analogWrite(forward_gpio_pin_, speed); - analogWrite(backward_gpio_pin_, 0); + forward_gpio_pin_->write(speed); + backward_gpio_pin_->write(0); } else { - analogWrite(backward_gpio_pin_, speed); - analogWrite(forward_gpio_pin_, 0); + backward_gpio_pin_->write(speed); + forward_gpio_pin_->write(0); } } diff --git a/andino_firmware/src/motor.h b/andino_firmware/src/motor.h index ae675163..b50821ee 100644 --- a/andino_firmware/src/motor.h +++ b/andino_firmware/src/motor.h @@ -64,6 +64,9 @@ // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #pragma once +#include "digital_out.h" +#include "pwm_out.h" + namespace andino { /// @brief This class allows to control a DC motor by enabling it and setting its speed. The @@ -76,15 +79,19 @@ class Motor { /// @param enable_gpio_pin Motor enable GPIO pin. /// @param forward_gpio_pin Motor forward GPIO pin. /// @param backward_gpio_pin Motor backward GPIO pin. - Motor(int enable_gpio_pin, int forward_gpio_pin, int backward_gpio_pin) + Motor(const DigitalOut* enable_gpio_pin, const PwmOut* forward_gpio_pin, + const PwmOut* backward_gpio_pin) : enable_gpio_pin_(enable_gpio_pin), forward_gpio_pin_(forward_gpio_pin), backward_gpio_pin_(backward_gpio_pin) {} - /// @brief Sets the motor state. + /// @brief Initializes the motor. + void begin(); + + /// @brief Enables the motor. /// - /// @param enabled Motor state. - void set_state(bool enabled); + /// @param enabled True to enable the motor, false otherwise. + void enable(bool enabled); /// @brief Sets the motor speed. /// @@ -99,13 +106,13 @@ class Motor { static constexpr int kMaxSpeed{255}; /// Motor enable GPIO pin. - int enable_gpio_pin_; + const DigitalOut* enable_gpio_pin_; /// Motor forward GPIO pin. - int forward_gpio_pin_; + const PwmOut* forward_gpio_pin_; /// Motor backward GPIO pin. - int backward_gpio_pin_; + const PwmOut* backward_gpio_pin_; }; } // namespace andino