File state.h
Go to the documentation of this file
#pragma once
#include <cstddef>
#include <cstdint>
#include <span>
#ifdef ARDUINO
#include "freertos/FreeRTOS.h"
#include "freertos/semphr.h"
#include <AccelStepper.h>
#include <Arduino.h>
#else
// Forward declare the non-arduino types
class AccelStepper;
#endif
#include "command.h"
#include "command_queue.h"
#include "fpm_adapter.hpp"
#include "logger.h"
#include "shared_types.h"
#include "target.h"
#include "utilities.h"
#include "vector.hpp"
using fixed = fixed_16_16;
class Command;
namespace cerializer {
class Config;
}
struct ConfigParameters {
fixed projectile_speed = projectileSpeed;
fixed turret_height = altitude;
fixed projectile_max_range = 40.0;
};
class SystemState {
public:
// -- Constructors --
SystemState();
// -- Public Methods --
Target* currentTarget();
std::span<Target> currentTargetArray();
inline size_t size();
void updateTarget(
auto& targetArray,
const uint8_t idx,
const bool valid,
PositionVector& newPosition,
const uint16_t indifferenceMargin = 0
);
void updateTargetById(
auto& targetArray,
const uint8_t id,
const bool valid,
PositionVector& newPosition,
const uint16_t indifferenceMargin = 0
);
void updateNearestTarget(const bool valid, PositionVector& newPosition, const uint16_t indifferenceMargin = 0);
void updateNearestTarget2d(const bool valid, PositionVector& newPosition, const uint16_t indifferenceMargin = 0);
void setTarget(TargetSource source, uint8_t index, uint8_t speed = 0xFF);
void setFire(bool active);
void clearFire(bool active);
void setMove(bool active);
void setStrategy(TurretStrategy strategy);
void setStance(TurretStance stance);
bool getFireState();
bool getMoveState();
bool shouldCheckTargetValidity();
bool shouldCheckFiringConditions();
bool targetIsPotentiallyValid();
void queueSelectTarget(TargetSource source, uint8_t index);
void queueFire(uint16_t milliseconds);
void queueCeaseFire(uint16_t milliseconds);
void updateConfig(cerializer::Config* config);
void processCommandQueue();
void actualizeState();
Target& fetchTarget(const uint8_t idx);
uint8_t fetchNearestTargetIdx(const PositionVector& point);
uint8_t fetchNearestTarget2dIdx(const PositionVector& point);
fixed targetTravelDistance();
// -- Public Attributes --
const int stepFraction = 16;
const int h_max = 500;
const int v_max = 1000;
const int h_min = -500;
const int v_min = -1000;
const fixed angleToStep{0.1125};
ConfigParameters config;
AccelStepper stepperA;
AccelStepper stepperB;
TargetSource target_source;
std::array<Target, 32> cvTarget;
std::array<Target, 3> radarTarget;
Target staticTarget;
Target* selectedTarget = &staticTarget; //< A reference to the currently selected target
const fixed currentYaw();
const fixed currentPitch();
TurretStrategy currentStrategy() const { return strategy; }
private:
// -- Private Methods --
void actualizePosition();
void actualizeFiring();
PositionVector targetAimpoint();
PositionVector getAimpoint();
// -- Private Attributes --
const int motorInterfaceType = 1;
const int stepPinA = 32;
const int dirPinA = 33;
const int stepPinB = 25;
const int dirPinB = 26;
const int firePin = 2;
bool moveState = true;
bool fireState = false;
bool needTrackingUpdate = false;
bool targetChangeProcessing = false;
bool fireOrderProcessing = false;
uint8_t trackingSpeed = 255;
TurretStrategy strategy = TurretStrategy::RANDOM;
TurretStance stance;
CommandQueue commandQueue;
};
// ======================================================================================
// --- Inline-Defined Methods ---
// ======================================================================================
inline size_t SystemState::size() {
return currentTargetArray().size();
}
inline void SystemState::updateTarget(
auto& targetArray,
const uint8_t idx,
const bool valid,
PositionVector& newPosition,
const uint16_t indifferenceMargin
) {
bool doUpdate = true;
bool activeTarget = false;
if (selectedTarget == &targetArray[idx]) {
activeTarget = true;
}
if (activeTarget && indifferenceMargin > 0) {
auto oldTarget = targetArray[idx];
auto oldTargetPos = oldTarget.Position();
if (oldTarget.valid && oldTargetPos) {
auto travelAngle = oldTargetPos.angleTo(newPosition) / angleToStep;
doUpdate = (travelAngle) > indifferenceMargin;
}
}
if (doUpdate) {
// Need something that can indicate that this is a reduced dimension measurement, so we only update fields that
// are real
targetArray[idx].Update(newPosition);
targetArray[idx].valid = valid;
if (activeTarget) {
needTrackingUpdate = true;
}
}
}
inline void SystemState::updateTargetById(
auto& targetArray,
const uint8_t id,
const bool valid,
PositionVector& newPosition,
const uint16_t indifferenceMargin
) {
auto pred_id = [&](const Target& item) { return item.id == id; };
auto found = std::ranges::find_if(targetArray, pred_id);
if (found == targetArray.end()) {
auto pred_valid = [&](const Target& item) { return item.valid == false; };
found = std::ranges::find_if(targetArray, pred_valid);
}
if (found == targetArray.end()) {
auto pred_seen = [&](const Target& item) { return item.seen; };
found = std::ranges::min_element(targetArray, std::ranges::less{}, pred_seen);
}
updateTarget(targetArray, found->index, valid, newPosition, indifferenceMargin);
targetArray[found->index].id = id;
};
inline Target& SystemState::fetchTarget(const uint8_t idx) {
return currentTargetArray()[idx];
}
inline uint8_t SystemState::fetchNearestTargetIdx(const PositionVector& point) {
auto distance = [&](Target& item) {
auto pos = item.Position();
return pow(pos.X_coord - point.X_coord, 2) + pow(pos.Y_coord - point.Y_coord, 2) +
pow(pos.Z_coord - point.Z_coord, 2);
};
auto res = std::ranges::min_element(currentTargetArray(), std::ranges::less{}, distance);
return static_cast<uint8_t>(std::ranges::distance(currentTargetArray().begin(), res));
}
inline uint8_t SystemState::fetchNearestTarget2dIdx(const PositionVector& point) {
auto distance = [&](Target& item) {
auto pos = item.Position();
return pow(pos.X_coord - point.X_coord, 2) + pow(pos.Y_coord - point.Y_coord, 2);
};
auto res = std::ranges::min_element(currentTargetArray(), std::ranges::less{}, distance);
return static_cast<uint8_t>(std::ranges::distance(currentTargetArray().begin(), res));
}
inline bool SystemState::targetIsPotentiallyValid() {
auto target = currentTarget();
return target->valid && (target->Position().magnitude() <= config.projectile_max_range * fixed(1.1));
}
inline const fixed SystemState::currentYaw() {
return angleToStep * (stepperA.currentPosition() + stepperB.currentPosition()) / 2;
}
inline const fixed SystemState::currentPitch() {
return angleToStep * (stepperA.currentPosition() - stepperB.currentPosition()) / 2;
}