98 lines
3.0 KiB
C++
98 lines
3.0 KiB
C++
#pragma once
|
|
#include <Arduino.h>
|
|
#include <vector>
|
|
#include "sensors.h"
|
|
#include "robotconfig.h"
|
|
|
|
// ============================================================================
|
|
// 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
|
|
void addBehavior(Behavior* behavior);
|
|
|
|
// Remove a behavior from the manager
|
|
void removeBehavior(Behavior* behavior);
|
|
|
|
// Update all 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:
|
|
std::vector<Behavior*> behaviors;
|
|
};
|
|
|
|
// Global behavior manager instance
|
|
extern BehaviorManager behaviorManager;
|