Servo.cpp

/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                                                         */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "Servo.h"

const float Servo::kMaxServoAngle;
const float Servo::kMinServoAngle;

void Servo::InitServo()
{
        // TODO: compute the appropriate values based on digital loop timing
        SetBounds(245, 0, 0, 0, 11);
        SetPeriodMultiplier(kPeriodMultiplier_4X);
}

Servo::Servo(UINT32 channel) : SafePWM(channel)
{
        InitServo();
}

Servo::Servo(UINT8 moduleNumber, UINT32 channel) : SafePWM(moduleNumber, channel)
{
        InitServo();
}

Servo::~Servo()
{
}

void Servo::Set(float value)
{
        SetPosition(value);
}

void Servo::SetOffline() {
        SetRaw(0);
}

float Servo::Get()
{
        return GetPosition();
}

void Servo::SetAngle(float degrees)
{
        if (degrees < kMinServoAngle)
        {
                degrees = kMinServoAngle;
        }
        else if (degrees > kMaxServoAngle)
        {
                degrees = kMaxServoAngle;
        }

        SetPosition(((float) (degrees - kMinServoAngle)) / GetServoAngleRange());
}

float Servo::GetAngle()
{
        return (float)GetPosition() * GetServoAngleRange() + kMinServoAngle;
}