Skip to content

Commit fdc96ef

Browse files
arloeitreimarloeitreim
committed
Controls Task Behavior Changes
1 parent cc1c7a0 commit fdc96ef

5 files changed

Lines changed: 100 additions & 15 deletions

File tree

‎include/NacelleConfig.hpp‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,7 @@ namespace LED {
8585

8686
namespace ENCODER {
8787
constexpr uint_fast16_t START_RUN_2_RPM = 200; // CONFIG
88-
constexpr uint_fast16_t TARGET_RPM = 2200; // CONFIG
88+
inline uint_fast16_t TARGET_RPM = 2200; // CONFIG
8989
constexpr uint_fast16_t MAX_RPM = 3000; // CONFIG
9090
constexpr uint_fast8_t MAX_RPS = MAX_RPM / UNITS::SECS_PER_MIN;
9191
constexpr uint_fast16_t MIN_T_mS_PER_REV = 1000 / MAX_RPS;
@@ -160,17 +160,19 @@ namespace PITCHING {
160160
// todo: Ftarget rpm var
161161

162162
/* PID Config */
163-
constexpr float PITCH_Kp = 0.127f; // CONFIG - 0.005f last year // TODO
163+
constexpr float PITCH_Kp = 0.08f; // CONFIG - 0.005f last year // TODO
164164
constexpr float PITCH_Ki = 0.000f; // CONFIG - 0.001f last year // TODO
165165
constexpr float PITCH_Kd = 0.0f;
166+
167+
inline bool enableRpmOutput = false; // CONFIG - whether to output RPM to log
166168
} // namespace PITCHING
167169

168170
namespace RUN {
169171
// Task Execution Intervals
170172
enum TASK_INTERVALS : uint_fast32_t {
171173
TI_FSM_mS = 100, // CONFIG - 100 ms (10 Hz)
172174
TI_POLL_SENSORS_mS = 2, // CONFIG - 2 ms (500 Hz) // todo - change?
173-
TI_PITCH_mS = 10, // CONFIG - 10 ms (100 Hz)
175+
TI_PITCH_mS = 100, // CONFIG - 10 ms (100 Hz)
174176
TI_RECV_ms = 50, // CONFIG - 100 ms (10 Hz)
175177
TI_SEND_ms =
176178
ENCODER::MEAS_TIME_DELTA_MS, // CONFIG - 5 - 10 ms (200 - 100 Hz)

‎include/NacelleContainer.hpp‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -7,12 +7,14 @@
77
#include <etl/format_spec.h>
88
#include <etl/string.h>
99
#include <etl/to_string.h>
10+
#include <cstdlib> // or <cmath>
1011

1112
// Custom Includes
1213
#include "NacelleComms.hpp"
1314
#include "NacelleTasks.hpp"
1415
#include "PID.hpp"
1516

17+
1618
/**
1719
* @brief Class to manage the container for nacelle data
1820
*/
@@ -148,6 +150,27 @@ class NacelleContainer {
148150
return (currentRPM > ENCODER::TARGET_RPM);
149151
}
150152

153+
154+
// checks if the rpm has been in a steady state (within tolerance) for a certain duration
155+
bool isTargetRPMExceededHysteresis() {
156+
const TickType_t now = xTaskGetTickCount();
157+
if (std::abs(static_cast<int32_t>(currentRPM) -
158+
static_cast<int32_t>(ENCODER::TARGET_RPM)) < steadyRpmTolerance) {
159+
if (initialTime != 0) {
160+
if ((now - initialTime) >= pdMS_TO_TICKS(MIN_STEADY_STATE_DURATION_MS)) {
161+
return true;
162+
}
163+
}
164+
else {
165+
initialTime = xTaskGetTickCount();
166+
}
167+
}
168+
else {
169+
initialTime = 0;
170+
}
171+
return false;
172+
}
173+
151174
static constexpr uint_fast8_t LOG_STRING_SIZE =
152175
3 + 7 + 5 + 10 + 5 + ((6 + 1) * 3) + 7 + 1 +
153176
1; // TODO - improve this and null terminator may not be needed
@@ -267,4 +290,7 @@ class NacelleContainer {
267290
std::atomic<ESTOP_TYPE_FAST> safetyFlag = ESTOP_TYPE_FAST::NONE;
268291
bool powerPositive = false;
269292
bool enableSafetyFlag = true;
293+
TickType_t initialTime = 0;
294+
constexpr static uint32_t MIN_STEADY_STATE_DURATION_MS = 5000; // 5 seconds
295+
constexpr static uint8_t steadyRpmTolerance = 20; // RPM tolerance for steady state check
270296
};

‎include/NacelleFSM.hpp‎

Lines changed: 37 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,8 @@
1515
class NacelleFSM {
1616
public: // MARK: Public
1717
static constexpr const char *TAG = "NFSM";
18+
float power = 0;
19+
float targetPower = 38; // W
1820

1921
/**
2022
* @brief Construct a new Nacelle FSM object
@@ -146,7 +148,10 @@ class NacelleFSM {
146148
nacelle.isTargetRPMExceeded()) {
147149
// sRunLoad -> sCurtail
148150
currentState = FSMCommon::States::sCurtail;
149-
151+
152+
vTaskResume(
153+
nacelle.mainTaskDescriptions[NacelleContainer::TID_PITCH]
154+
.pxHandle);
150155
// Pitch is already set to adjust (fine), which will detect the new
151156
// state (PI)
152157
nacelle.pitchPIDController.enable(
@@ -157,17 +162,39 @@ class NacelleFSM {
157162

158163
return UPDATE_RESULT::STATE_CHANGED;
159164
} else if ((currentState == FSMCommon::States::sCurtail) &&
160-
!nacelle.isTargetRPMExceeded()) {
165+
nacelle.isTargetRPMExceededHysteresis()) {
166+
167+
constexpr float POWER_TOLERANCE = 0.06f;
168+
constexpr int_fast16_t RPM_STEP = 40;
169+
170+
float dPower = power - targetPower;
171+
172+
if ((dPower) > POWER_TOLERANCE * targetPower) {
173+
nacelle.pitchPIDController.setTarget(ENCODER::TARGET_RPM - RPM_STEP); // todo adjust decrement amount
174+
ENCODER::TARGET_RPM = ENCODER::TARGET_RPM - RPM_STEP; // todo adjust decrement amount
175+
ESP_LOGI(TAG, "Decreasing target RPM to %u due to overpower condition", ENCODER::TARGET_RPM);
176+
}
177+
else if (dPower < -POWER_TOLERANCE * targetPower) {
178+
nacelle.pitchPIDController.setTarget(ENCODER::TARGET_RPM + RPM_STEP); // todo adjust increment amount
179+
ENCODER::TARGET_RPM = ENCODER::TARGET_RPM + RPM_STEP; // todo adjust increment amount
180+
ESP_LOGI(TAG, "Increasing target RPM to %u due to underpower condition", ENCODER::TARGET_RPM);
181+
}
182+
else {
183+
nacelle.pitchPIDController.disable();
184+
ESP_LOGI(TAG, "Power within acceptable range, PID disabled");
185+
}
161186
// sCurtail -> sRunLoad
162-
currentState = FSMCommon::States::sRunLoad;
187+
//currentState = FSMCommon::States::sRunLoad;
163188

164189
// Pitch is already set to adjust (PI), which will detect the new
165190
// state (fine)
166-
nacelle.pitchPIDController.disable();
191+
192+
//nacelle.pitchPIDController.disable();
193+
167194
// DONE: signal load (unset targetRPMExceeded)
168195
// Load will be sent RPM info elsewhere
169-
170-
return UPDATE_RESULT::STATE_CHANGED;
196+
return UPDATE_RESULT::NO_CHANGE;
197+
//return UPDATE_RESULT::STATE_CHANGED;
171198
} else {
172199
return UPDATE_RESULT::NO_CHANGE;
173200
}
@@ -176,6 +203,10 @@ class NacelleFSM {
176203
return UPDATE_RESULT::ERROR;
177204
}
178205

206+
void setPower(float power) {
207+
this->power = power;
208+
}
209+
179210
private: // MARK: Private
180211
NacelleContainer &nacelle; // DONE: switch to by reference
181212

‎lib/SerialInterface/SerialInterface.hpp‎

Lines changed: 18 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -5,16 +5,16 @@
55
#include "NacelleTasks.hpp"
66
#include "NacelleContainer.hpp"
77
#include "NacelleConfig.hpp"
8-
98
//using TaskFunction = void (*)(void *);
109

1110
class SerialInterface {
1211
public:
1312
static constexpr const char *TAG = "serialInterface";
1413

15-
SerialInterface(int baudRate_, NacelleContainer &nacelle)
14+
SerialInterface(int baudRate_, NacelleContainer &nacelle, NacelleFSM &nacelleFSM)
1615
: baudRate(baudRate_),
17-
nacelle(nacelle) {
16+
nacelle(nacelle),
17+
nacelleFSM(nacelleFSM) {
1818
}
1919

2020
void begin() {
@@ -31,7 +31,7 @@ class SerialInterface {
3131
line.trim();
3232
ESP_LOGI(TAG, "%s", line.c_str());
3333
stringArray parts = split(line, ' ');
34-
34+
3535
if (parts.count > 0) {
3636
command = parts.tokens[0];
3737
if (command.equalsIgnoreCase("pid")) {
@@ -94,6 +94,19 @@ class SerialInterface {
9494
nacelle.setEnableSafetyFlag(true);
9595
ESP_LOGI(TAG, "Enabled safety (OVERSPEED ESTOP)");
9696
}
97+
else if (command.equalsIgnoreCase("enableRpmOutput")) {
98+
PITCHING::enableRpmOutput = true;
99+
ESP_LOGI(TAG, "Enabled RPM output to log");
100+
}
101+
else if (command.equalsIgnoreCase("disableRpmOutput")) {
102+
PITCHING::enableRpmOutput = false;
103+
ESP_LOGI(TAG, "Disabled RPM output to log");
104+
105+
}
106+
else if (command.equalsIgnoreCase("setPower")) {
107+
nacelleFSM.setPower(parts.tokens[1].toFloat());
108+
ESP_LOGI(TAG, "Updated power: %.2f", nacelleFSM.power);
109+
}
97110
else if (command.equalsIgnoreCase("help")) {
98111
Serial.println("Available commands:");
99112
Serial.println(" pid <Kp> <Ki> <Kd> - Set all PID constants");
@@ -124,6 +137,7 @@ class SerialInterface {
124137
static SerialInterface *instance_;
125138
int baudRate;
126139
NacelleContainer &nacelle;
140+
NacelleFSM &nacelleFSM;
127141
int position = 0;
128142
bool FSM_ENABLED = true;
129143
String command = "";

‎src/main.cpp‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -83,7 +83,7 @@ PID pitchPIDController = PID(
8383
NacelleContainer nacelle(pitchActuator, pitchPIDController, nacelleComms);
8484
NacelleFSM nacelleFSM(nacelle);
8585

86-
SerialInterface serialInterface(WTbCommonConfig::SERIAL_BAUD, nacelle);
86+
SerialInterface serialInterface(WTbCommonConfig::SERIAL_BAUD, nacelle, nacelleFSM);
8787
/**
8888
* MARK: Setup
8989
* @details put your setup code here, to run once:
@@ -276,6 +276,10 @@ void setup() {
276276
[[noreturn]] void
277277
vTaskUpdateFSM([[maybe_unused]] void *pvParameters) { // NOSONAR
278278
while (true) {
279+
280+
if (PITCHING::enableRpmOutput) {
281+
Serial.println(nacelle.currentRPM);
282+
}
279283
static TickType_t xLastWakeTime = xTaskGetTickCount();
280284
ESP_LOGV(TAG, "Updating FSM");
281285

@@ -390,7 +394,14 @@ vTaskPollSensors([[maybe_unused]] void *pvParameters) { // NOSONAR
390394
auto pidOutput = static_cast<uint_fast16_t>(
391395
pitchPIDController.compute(nacelle.currentRPM)); // DONE - input
392396
ESP_LOGI(TAG, "PID Output: %u", pidOutput);
393-
pitchActuator.writePosMicros(pidOutput);
397+
//pitchActuator.writePosMicros(pidOutput);
398+
if (pidOutput >= PITCHING::SERVO_MIN_uS_2026 &&
399+
pidOutput <= PITCHING::SERVO_MAX_uS_2026) {
400+
ESP_LOGI(TAG, "PID Output: %.1f", pidOutput);
401+
pitchActuator.writePosMicros(static_cast<int>(pidOutput));
402+
} else {
403+
ESP_LOGW(TAG, "PID skipped/invalid output: %.1f", pidOutput);
404+
}
394405
} else {
395406
// No updates needed
396407
ESP_LOGI(TAG, "No pitch updates needed in state %d, supsending",
@@ -406,6 +417,7 @@ vTaskPollSensors([[maybe_unused]] void *pvParameters) { // NOSONAR
406417
&xLastWakeTime, pdMS_TO_TICKS(RUN::TASK_INTERVALS::TI_PITCH_mS));
407418
if (xWasDelayed != pdTRUE) {
408419
ESP_LOGE(TAG, "Timing not met!");
420+
xLastWakeTime = xTaskGetTickCount();
409421
}
410422
}
411423
}

0 commit comments

Comments
 (0)