Camera.cpp

Go to the documentation of this file.

#include <Camera.h>
#include <algorithm>    // std::min, std::max

Camera::Camera()
{
    m_Camera = NULL;
    m_Stick = NULL;
    m_CameraServoPosition = kCameraFirstPos;
    m_CameraMove = 0;
}

Camera::~Camera() {
    // Delete allocated member objects
    delete m_Camera; m_Camera = NULL;
    delete m_Stick; m_Stick = NULL;
}

/*
 * Called once, to perform initialization that isn't appropriate for constructor.
 */
void Camera::ComponentInit() {
    // Initialize Camera & joystick
    if (!m_Camera) m_Camera = new Servo(kCameraChannel);
    if (!m_Stick) m_Stick = new Joystick(kStickChannel);

    // Move camera servo to initial position
    m_Camera->Set(m_CameraServoPosition);
}

/*
 * Component-specific sensory input collector.
 * Read state of camera control buttons and set
 * move command accordingly.
 */
void Camera::Sense() {
    // Store move command; +1 for up, -1 for down, 0 for just hanging around
    m_CameraMove =
            m_Stick->GetRawButton(kCameraUp) ? 1 :
            m_Stick->GetRawButton(kCameraDown) ? -1 : 0;
}

/*
 * Component-specific worker function.
 * Process the move command received by Sense(), ensuring
 * camera servo remains within motion bounds.
 * Report operation to SmartDashboard.
 */
void Camera::DoWork() {
    static const uint32_t kNoMove = 0;

    // Apply most recent move command.
    float newPosition = m_CameraServoPosition + m_CameraMove * kCameraIncrement;

    /*
     * Validate move, it must be within bounds.
     * Test the upper bounds by picking the min of the new position or range max,
     * then the lower bounds by picking the max of the new position or range min.
     */
    float temp = 0.0; // trick to get around constexpr rules
    newPosition = std::min(newPosition,temp=kMaxCameraServoPosition);
    newPosition = std::max(newPosition,temp=kMinCameraServoPosition);

    /*
     *  Operate camera servo if move is within bounds.
     *  If the new position is the same as the old one, then cancel move command.
     */
    m_CameraMove = (newPosition == m_CameraServoPosition) ? kNoMove : m_CameraMove;
    if (m_CameraMove) {
        m_Camera->Set(m_CameraServoPosition = newPosition);
    }

    // Report operation
    // Todo: this should be made optional, with some way to set on / off.
    std::string pan;  // to report camera motion
    switch (m_CameraMove) {
      case -1:
          pan.assign("Pan down");
          break;
      case +1:
          pan.assign("Pan up");
          break;
      default:
          pan.assign("Stationary");
          break;
    }

    SmartDashboard::PutString("Camera", pan);

    // Clear move command for this iteration
    m_CameraMove = kNoMove;

    return;
}