126 lines
3.9 KiB
C++
126 lines
3.9 KiB
C++
#pragma once
|
|
#include <Arduino.h>
|
|
#include <vector>
|
|
#include "sensors.h"
|
|
#include "robotconfig.h"
|
|
|
|
// ============================================================================
|
|
// Behavior IDs
|
|
// ============================================================================
|
|
|
|
enum BehaviorID : uint8_t {
|
|
BEHAVIOR_FOCUS = 1, // Focus behavior (radar tracking)
|
|
// Add more behavior IDs here as needed
|
|
};
|
|
|
|
// ============================================================================
|
|
// Base Behavior Class
|
|
// ============================================================================
|
|
|
|
class Behavior {
|
|
public:
|
|
Behavior();
|
|
virtual ~Behavior() = default;
|
|
|
|
// Get list of motor IDs this behavior controls
|
|
const std::vector<uint8_t>& getControlledMotors() const { return controlledMotors; }
|
|
|
|
// Add a motor to the controlled list
|
|
void addMotor(uint8_t motorID);
|
|
|
|
// Remove a motor from the controlled list
|
|
void removeMotor(uint8_t motorID);
|
|
|
|
// Clear all controlled motors
|
|
void clearMotors();
|
|
|
|
// Virtual method to update the behavior (called each frame)
|
|
// Returns true if the behavior is active and wants to control motors
|
|
virtual bool update() = 0;
|
|
|
|
// Virtual method to get the desired position for a motor
|
|
// Returns true if this behavior wants to control this motor, false otherwise
|
|
virtual bool getMotorPosition(uint8_t motorID, uint16_t& position) = 0;
|
|
|
|
protected:
|
|
std::vector<uint8_t> controlledMotors;
|
|
};
|
|
|
|
// ============================================================================
|
|
// Focus Behavior - Tracks radar targets with eyes/neck
|
|
// ============================================================================
|
|
|
|
class FocusBehavior : public Behavior {
|
|
public:
|
|
FocusBehavior();
|
|
|
|
// Update behavior - check radar for targets
|
|
bool update() override;
|
|
|
|
// Get motor position for a controlled motor
|
|
bool getMotorPosition(uint8_t motorID, uint16_t& position) override;
|
|
|
|
private:
|
|
bool isActive;
|
|
uint16_t currentPosition; // Current position for motors 14 and 15
|
|
|
|
// Configuration
|
|
static constexpr uint8_t FOCUS_MOTOR_1 = 14;
|
|
static constexpr uint8_t FOCUS_MOTOR_2 = 15;
|
|
static constexpr uint16_t POSITION_CENTER = 2047;
|
|
static constexpr uint16_t POSITION_MIN = 1700;
|
|
static constexpr uint16_t POSITION_MAX = 2300;
|
|
static constexpr float RADAR_X_MIN = -80.0f;
|
|
static constexpr float RADAR_X_MAX = 80.0f;
|
|
|
|
// Calculate motor position from radar x coordinate
|
|
uint16_t calculatePositionFromRadarX(float radarX);
|
|
};
|
|
|
|
// ============================================================================
|
|
// Behavior Manager - Manages active behaviors and resolves motor conflicts
|
|
// ============================================================================
|
|
|
|
class BehaviorManager {
|
|
public:
|
|
BehaviorManager();
|
|
|
|
// Add a behavior to the manager with an ID
|
|
void addBehavior(BehaviorID id, Behavior* behavior);
|
|
|
|
// Remove a behavior from the manager
|
|
void removeBehavior(Behavior* behavior);
|
|
|
|
// Enable/disable a behavior by ID
|
|
void setBehaviorEnabled(BehaviorID id, bool enabled);
|
|
|
|
// Check if a behavior is enabled
|
|
bool isBehaviorEnabled(BehaviorID id) const;
|
|
|
|
// Get count of registered behaviors
|
|
uint8_t getBehaviorCount() const;
|
|
|
|
// Get behavior info at index (for iteration)
|
|
// Returns true if index is valid and fills out id and enabled
|
|
bool getBehaviorInfo(uint8_t index, BehaviorID& id, bool& enabled) const;
|
|
|
|
// Update all enabled behaviors (call each frame)
|
|
void update();
|
|
|
|
// Check if a behavior wants to control a specific motor
|
|
// Returns true if a behavior provides a position, false otherwise
|
|
bool getMotorPosition(uint8_t motorID, uint16_t& position);
|
|
|
|
private:
|
|
struct BehaviorEntry {
|
|
BehaviorID id;
|
|
Behavior* behavior;
|
|
};
|
|
|
|
std::vector<BehaviorEntry> behaviors;
|
|
bool enabledStates[256] = {false}; // Track enabled state by ID
|
|
};
|
|
|
|
// Global behavior manager instance
|
|
extern BehaviorManager behaviorManager;
|