153 lines
2.5 KiB
C++
153 lines
2.5 KiB
C++
#include "motor_controller.h"
|
|
#include "state.h"
|
|
|
|
#include <Arduino.h>
|
|
|
|
|
|
MotorController::MotorController(
|
|
RotaryEncoderDriver& encoder,
|
|
MotorDriver& motor,
|
|
//P_controller& controller,
|
|
//PI_controller& controller;
|
|
Controller& controller,
|
|
State& initialization,
|
|
State& operational,
|
|
State& stopped
|
|
)
|
|
: current_state(nullptr),
|
|
initialization_state(initialization),
|
|
operational_state(operational),
|
|
stopped_state(stopped),
|
|
encoder(encoder),
|
|
motor(motor),
|
|
controller(controller),
|
|
led(LED_PIN),
|
|
reference_rpm(DEFAULT_REFERENCE_RPM)
|
|
{
|
|
initialization_state.set_context(this);
|
|
operational_state.set_context(this);
|
|
stopped_state.set_context(this);
|
|
}
|
|
|
|
|
|
// void MotorController::set_states(
|
|
// State* initialization,
|
|
// State* operational,
|
|
// State* stopped
|
|
// )
|
|
|
|
// initialization_state = initialization;
|
|
// operational_state = operational;
|
|
// stopped_state = stopped;
|
|
|
|
|
|
|
|
void MotorController::begin()
|
|
{
|
|
led.init();
|
|
led.set_lo();
|
|
|
|
transition_to(initialization_state);
|
|
}
|
|
|
|
|
|
void MotorController::transition_to(State& new_state)
|
|
{
|
|
if (current_state != nullptr)
|
|
{
|
|
current_state->exit();
|
|
}
|
|
|
|
current_state = &new_state;
|
|
|
|
current_state->set_context(this);
|
|
|
|
current_state->enter();
|
|
}
|
|
|
|
|
|
void MotorController::update()
|
|
{
|
|
if (current_state != nullptr)
|
|
{
|
|
current_state->update();
|
|
}
|
|
}
|
|
|
|
|
|
void MotorController::command(char c)
|
|
{
|
|
if (current_state != nullptr)
|
|
{
|
|
current_state->command(c);
|
|
}
|
|
}
|
|
|
|
|
|
void MotorController::to_initialization()
|
|
{
|
|
transition_to(initialization_state);
|
|
}
|
|
|
|
|
|
void MotorController::to_operational()
|
|
{
|
|
transition_to(operational_state);
|
|
}
|
|
|
|
|
|
void MotorController::run_speed_controller()
|
|
{
|
|
if (encoder.new_speed_available())
|
|
{
|
|
double actual = encoder.rpm();
|
|
|
|
double pwm_percent =
|
|
controller.update(reference_rpm, actual);
|
|
|
|
double pwm =
|
|
pwm_percent * PWM_PER_PERCENT;
|
|
|
|
motor.set(pwm);
|
|
}
|
|
}
|
|
|
|
|
|
void MotorController::stop_motor()
|
|
{
|
|
motor.stop();
|
|
}
|
|
|
|
|
|
void MotorController::reset_speed_measurement()
|
|
{
|
|
encoder.reset_speed_measurement();
|
|
}
|
|
|
|
void MotorController::to_stopped()
|
|
{
|
|
transition_to(stopped_state);
|
|
}
|
|
|
|
bool MotorController::fault_active()
|
|
{
|
|
return motor.fault_active();
|
|
}
|
|
|
|
void MotorController::reset_controller()
|
|
{
|
|
controller.reset();
|
|
}
|
|
|
|
void MotorController::led_on()
|
|
{
|
|
led.set_hi();
|
|
}
|
|
|
|
|
|
void MotorController::led_off()
|
|
{
|
|
led.set_lo();
|
|
}
|
|
|