MotorSafetyHelper.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 "MotorSafetyHelper.h"

#include "DriverStation.h"
#include "MotorSafety.h"
#include "Synchronized.h"
#include "Timer.h"
#include "WPIErrors.h"

#include <stdio.h>

MotorSafetyHelper *MotorSafetyHelper::m_headHelper = NULL;
SEM_ID MotorSafetyHelper::m_listMutex = NULL;

MotorSafetyHelper::MotorSafetyHelper(MotorSafety *safeObject)
{
        if (m_listMutex == NULL)
                m_listMutex = semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE);
        m_safeObject = safeObject;
        m_enabled = false;
        m_expiration = DEFAULT_SAFETY_EXPIRATION;
        m_stopTime = Timer::GetFPGATimestamp();

        Synchronized sync(m_listMutex);
        m_nextHelper = m_headHelper;
        m_headHelper = this;
}


MotorSafetyHelper::~MotorSafetyHelper()
{
        semTake(m_listMutex, WAIT_FOREVER);
        if (m_headHelper == this)
        {
                m_headHelper = m_nextHelper;
        }
        else
        {
                MotorSafetyHelper *prev = NULL;
                MotorSafetyHelper *cur = m_headHelper;
                while (cur != this && cur != NULL)
                        prev = cur, cur = cur->m_nextHelper;
                if (cur == this)
                        prev->m_nextHelper = cur->m_nextHelper;
        }
        if (m_headHelper == NULL)
        {
                semDelete(m_listMutex);
                m_listMutex = NULL;
        }
        else
        {
                semGive(m_listMutex);
        }
}

/*
 * Feed the motor safety object.
 * Resets the timer on this object that is used to do the timeouts.
 */
void MotorSafetyHelper::Feed()
{
        m_stopTime = Timer::GetFPGATimestamp() + m_expiration;
}

/*
 * Set the expiration time for the corresponding motor safety object.
 * @param expirationTime The timeout value in seconds.
 */
void MotorSafetyHelper::SetExpiration(float expirationTime)
{
        m_expiration = expirationTime;
}

float MotorSafetyHelper::GetExpiration()
{
        return m_expiration;
}

bool MotorSafetyHelper::IsAlive()
{
        return !m_enabled || m_stopTime > Timer::GetFPGATimestamp();
}

void MotorSafetyHelper::Check()
{
        if (!m_enabled) return;
        if (DriverStation::GetInstance()->IsDisabled()) return;
        if (m_stopTime < Timer::GetFPGATimestamp())
        {
                char buf[128];
                char desc[64];
                m_safeObject->GetDescription(desc);
                snprintf(buf, 128, "%s... Output not updated often enough.", desc);
                wpi_setWPIErrorWithContext(Timeout, buf);
                m_safeObject->StopMotor();
        }
}

void MotorSafetyHelper::SetSafetyEnabled(bool enabled)
{
        m_enabled = enabled;
}

bool MotorSafetyHelper::IsSafetyEnabled()
{
        return m_enabled;
}

//TODO: these should be synchronized with the setting methods in case it's called from a different thread
void MotorSafetyHelper::CheckMotors()
{
        Synchronized sync(m_listMutex);
        for (MotorSafetyHelper *msh = m_headHelper; msh != NULL; msh = msh->m_nextHelper)
        {
                msh->Check();
        }
}