163 lines
3.4 KiB
C++
163 lines
3.4 KiB
C++
#include "pre_operational_state.h"
|
|
#include "motor_controller.h"
|
|
|
|
#include "p_controller.h"
|
|
|
|
#include <stdlib.h>
|
|
#include <zephyr/kernel.h>
|
|
#include <zephyr/sys/printk.h>
|
|
|
|
PreOperationalState::PreOperationalState(P_controller &p_controller, P_controller &pi_controller)
|
|
: p_controller(p_controller),
|
|
pi_controller(pi_controller),
|
|
pi_selected(true),
|
|
previous_time(0),
|
|
led_state(false),
|
|
input_mode(NONE),
|
|
input_index(0)
|
|
{
|
|
}
|
|
|
|
void PreOperationalState::enter()
|
|
{
|
|
printk("State: Pre-operational\n");
|
|
|
|
// The selection survives a reset, so report it (used by capture_serial.py).
|
|
printk("Controller: %s\n", pi_selected ? "PI" : "P");
|
|
|
|
context->stop_motor();
|
|
|
|
context->led_off();
|
|
|
|
led_state = false;
|
|
|
|
previous_time = k_uptime_get();
|
|
}
|
|
|
|
void PreOperationalState::update()
|
|
{
|
|
const int64_t current_time = k_uptime_get();
|
|
|
|
if (current_time - previous_time >= 500)
|
|
{
|
|
previous_time = current_time;
|
|
|
|
led_state = !led_state;
|
|
|
|
if (led_state)
|
|
{
|
|
context->led_on();
|
|
}
|
|
else
|
|
{
|
|
context->led_off();
|
|
}
|
|
}
|
|
}
|
|
|
|
void PreOperationalState::command(char c)
|
|
{
|
|
// We are currently entering a Kp or Ti value
|
|
if (input_mode != NONE)
|
|
{
|
|
// Enter pressed
|
|
if (c == '\n' || c == '\r')
|
|
{
|
|
if (input_index > 0)
|
|
{
|
|
// End the string
|
|
input_buffer[input_index] = '\0';
|
|
|
|
// Convert text to a number
|
|
const double value = atof(input_buffer);
|
|
|
|
if (input_mode == ENTER_KP)
|
|
{
|
|
context->set_Kp(value);
|
|
|
|
printk("Kp set to: %.4f\n", value);
|
|
}
|
|
else if (input_mode == ENTER_TI)
|
|
{
|
|
context->set_Ti(value);
|
|
|
|
printk("Ti set to: %.4f\n", value);
|
|
}
|
|
}
|
|
|
|
// Finished entering a value
|
|
input_index = 0;
|
|
input_mode = NONE;
|
|
|
|
return;
|
|
}
|
|
|
|
// Store number characters
|
|
if ((c >= '0' && c <= '9') ||
|
|
c == '.' ||
|
|
c == '-')
|
|
{
|
|
if (input_index < sizeof(input_buffer) - 1)
|
|
{
|
|
input_buffer[input_index++] = c;
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
// Normal state commands
|
|
if (c == 'o' || c == 'O')
|
|
{
|
|
printk("Command: set operational\n");
|
|
|
|
context->to_operational();
|
|
}
|
|
|
|
else if (c == 'r' || c == 'R')
|
|
{
|
|
printk("Command: reset\n");
|
|
|
|
context->to_initialization();
|
|
}
|
|
|
|
else if (c == 'k' || c == 'K')
|
|
{
|
|
input_mode = ENTER_KP;
|
|
input_index = 0;
|
|
|
|
printk("Enter Kp and press Enter:\n");
|
|
}
|
|
|
|
else if (c == 't' || c == 'T')
|
|
{
|
|
input_mode = ENTER_TI;
|
|
input_index = 0;
|
|
|
|
printk("Enter Ti and press Enter:\n");
|
|
}
|
|
|
|
// Select controller algorithm, the controller is passed to the motor controller
|
|
else if (c == 'c' || c == 'C')
|
|
{
|
|
pi_selected = !pi_selected;
|
|
|
|
if (pi_selected)
|
|
{
|
|
context->set_controller(pi_controller);
|
|
printk("Controller: PI\n");
|
|
}
|
|
else
|
|
{
|
|
context->set_controller(p_controller);
|
|
printk("Controller: P\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
void PreOperationalState::exit()
|
|
{
|
|
context->led_off();
|
|
}
|