73 lines
1.2 KiB
C++
73 lines
1.2 KiB
C++
#include "operational_state.h"
|
|
#include "motor_controller.h"
|
|
|
|
#include <zephyr/sys/printk.h>
|
|
|
|
void OperationalState::enter()
|
|
{
|
|
printk("State: Operational\n");
|
|
|
|
context->reset_controller();
|
|
|
|
context->led_on();
|
|
|
|
sample_count = 0;
|
|
|
|
#if LOG_CSV
|
|
printk("time_ms,ref_rpm,actual_rpm,pwm\n");
|
|
#endif
|
|
}
|
|
|
|
|
|
void OperationalState::update()
|
|
{
|
|
if (context->fault_active())
|
|
{
|
|
printk("Fault detected\n");
|
|
|
|
context->to_stopped();
|
|
|
|
return;
|
|
}
|
|
|
|
if (context->run_speed_controller())
|
|
{
|
|
++sample_count;
|
|
|
|
#if LOG_CSV
|
|
// 4 ms á milli sýna, tíminn byrjar á 0 við hverja innkomu í Operational
|
|
printk("%u,%.1f,%.2f,%.1f\n",
|
|
static_cast<unsigned int>(sample_count * 4U),
|
|
context->get_reference_rpm(),
|
|
context->get_actual_rpm(),
|
|
context->get_pwm());
|
|
#endif
|
|
}
|
|
}
|
|
|
|
|
|
void OperationalState::command(char c)
|
|
{
|
|
if (c == 'r' || c == 'R')
|
|
{
|
|
printk("Command: reset\n");
|
|
|
|
context->to_initialization();
|
|
}
|
|
|
|
else if (c == 'p' || c == 'P')
|
|
{
|
|
printk("Command: set pre-operational\n");
|
|
|
|
context->to_pre_operational();
|
|
}
|
|
}
|
|
|
|
|
|
void OperationalState::exit()
|
|
{
|
|
context->stop_motor();
|
|
|
|
context->led_off();
|
|
}
|