161 lines
3.4 KiB
C++
161 lines
3.4 KiB
C++
#include "pre_operational_state.h"
|
|
#include "motor_controller.h"
|
|
|
|
#include <Arduino.h>
|
|
#include <stdlib.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),
|
|
//waiting_for_kp(false),
|
|
//waiting_for_ti(false),
|
|
input_mode(NONE),
|
|
input_index(0)
|
|
{
|
|
}
|
|
|
|
void PreOperationalState::enter()
|
|
{
|
|
Serial.println("State: Pre-operational");
|
|
|
|
context->stop_motor();
|
|
|
|
context->led_off();
|
|
|
|
led_state = false;
|
|
|
|
previous_time = millis();
|
|
}
|
|
|
|
void PreOperationalState::update()
|
|
{
|
|
unsigned long current_time = millis();
|
|
|
|
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
|
|
double value = atof(input_buffer);
|
|
|
|
if (input_mode == ENTER_KP)
|
|
{
|
|
context->set_Kp(value);
|
|
|
|
Serial.print("Kp set to: ");
|
|
Serial.println(value);
|
|
}
|
|
else if (input_mode == ENTER_TI)
|
|
{
|
|
context->set_Ti(value);
|
|
|
|
Serial.print("Ti set to: ");
|
|
Serial.println(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;
|
|
input_index++;
|
|
}
|
|
}
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
// Normal state commands
|
|
if (c == 'o' || c == 'O')
|
|
{
|
|
Serial.println("Command: set operational");
|
|
|
|
context->to_operational();
|
|
}
|
|
|
|
else if (c == 'r' || c == 'R')
|
|
{
|
|
Serial.println("Command: reset");
|
|
|
|
context->to_initialization();
|
|
}
|
|
|
|
else if (c == 'k' || c == 'K')
|
|
{
|
|
input_mode = ENTER_KP;
|
|
input_index = 0;
|
|
|
|
Serial.println("Enter Kp and press Enter:");
|
|
}
|
|
|
|
else if (c == 't' || c == 'T')
|
|
{
|
|
input_mode = ENTER_TI;
|
|
input_index = 0;
|
|
|
|
Serial.println("Enter Ti and press Enter:");
|
|
}
|
|
|
|
// 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);
|
|
Serial.println("Controller: PI");
|
|
}
|
|
else
|
|
{
|
|
context->set_controller(p_controller);
|
|
Serial.println("Controller: P");
|
|
}
|
|
}
|
|
}
|
|
|
|
void PreOperationalState::exit()
|
|
{
|
|
context->led_off();
|
|
} |