DriverStation.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 "DriverStation.h"
#include "AnalogChannel.h"
#include "Synchronized.h"
#include "Timer.h"
#include "NetworkCommunication/FRCComm.h"
#include "NetworkCommunication/UsageReporting.h"
#include "MotorSafetyHelper.h"
#include "Utility.h"
#include "WPIErrors.h"
#include <strLib.h>

const uint32_t DriverStation::kBatteryModuleNumber;
const uint32_t DriverStation::kBatteryChannel;
const uint32_t DriverStation::kJoystickPorts;
const uint32_t DriverStation::kJoystickAxes;
constexpr float DriverStation::kUpdatePeriod;
DriverStation* DriverStation::m_instance = NULL;
uint8_t DriverStation::m_updateNumber = 0;

DriverStation::DriverStation()
        : m_controlData (NULL)
        , m_digitalOut (0)
        , m_batteryChannel (NULL)
        , m_statusDataSemaphore (semMCreate(SEM_Q_PRIORITY | SEM_DELETE_SAFE | SEM_INVERSION_SAFE))
        , m_task ("DriverStation", (FUNCPTR)DriverStation::InitTask)
        , m_dashboardHigh(m_statusDataSemaphore)
        , m_dashboardLow(m_statusDataSemaphore)
        , m_dashboardInUseHigh(&m_dashboardHigh)
        , m_dashboardInUseLow(&m_dashboardLow)
        , m_newControlData(0)
        , m_packetDataAvailableSem (0)
        , m_enhancedIO()
        , m_waitForDataSem(0)
        , m_approxMatchTimeOffset(-1.0)
        , m_userInDisabled(false)
        , m_userInAutonomous(false)
        , m_userInTeleop(false)
        , m_userInTest(false)
{
        // Create a new semaphore
        m_packetDataAvailableSem = semBCreate (SEM_Q_PRIORITY, SEM_EMPTY);
        m_newControlData = semBCreate (SEM_Q_FIFO, SEM_EMPTY);

        // Register that semaphore with the network communications task.
        // It will signal when new packet data is available. 
        setNewDataSem(m_packetDataAvailableSem);

        m_waitForDataSem = semBCreate (SEM_Q_PRIORITY, SEM_EMPTY);

        m_controlData = new FRCCommonControlData;

        // initialize packet number and control words to zero;
        m_controlData->packetIndex = 0;
        m_controlData->control = 0;

        // set all joystick axis values to neutral; buttons to OFF
        m_controlData->stick0Axis1 = m_controlData->stick0Axis2 = m_controlData->stick0Axis3 = 0;
        m_controlData->stick1Axis1 = m_controlData->stick1Axis2 = m_controlData->stick1Axis3 = 0;
        m_controlData->stick2Axis1 = m_controlData->stick2Axis2 = m_controlData->stick2Axis3 = 0;
        m_controlData->stick3Axis1 = m_controlData->stick3Axis2 = m_controlData->stick3Axis3 = 0;
        m_controlData->stick0Axis4 = m_controlData->stick0Axis5 = m_controlData->stick0Axis6 = 0;
        m_controlData->stick1Axis4 = m_controlData->stick1Axis5 = m_controlData->stick1Axis6 = 0;
        m_controlData->stick2Axis4 = m_controlData->stick2Axis5 = m_controlData->stick2Axis6 = 0;
        m_controlData->stick3Axis4 = m_controlData->stick3Axis5 = m_controlData->stick3Axis6 = 0;
        m_controlData->stick0Buttons = 0;
        m_controlData->stick1Buttons = 0;
        m_controlData->stick2Buttons = 0;
        m_controlData->stick3Buttons = 0;

        // initialize the analog and digital data.
        m_controlData->analog1 = 0;
        m_controlData->analog2 = 0;
        m_controlData->analog3 = 0;
        m_controlData->analog4 = 0;
        m_controlData->dsDigitalIn = 0;

        m_batteryChannel = new AnalogChannel(kBatteryModuleNumber, kBatteryChannel);

        AddToSingletonList();

        if (!m_task.Start((int32_t)this))
        {
                wpi_setWPIError(DriverStationTaskError);
        }
}

DriverStation::~DriverStation()
{
        m_task.Stop();
        semDelete(m_statusDataSemaphore);
        delete m_batteryChannel;
        delete m_controlData;
        m_instance = NULL;
        semDelete(m_waitForDataSem);
        // Unregister our semaphore.
        setNewDataSem(0);
        semDelete(m_packetDataAvailableSem);
}

void DriverStation::InitTask(DriverStation *ds)
{
        ds->Run();
}

void DriverStation::Run()
{
        int period = 0;
        while (true)
        {
                semTake(m_packetDataAvailableSem, WAIT_FOREVER);
                SetData();
                m_enhancedIO.UpdateData();
                GetData();
                semFlush(m_waitForDataSem);
                if (++period >= 4)
                {
                        MotorSafetyHelper::CheckMotors();
                        period = 0;
                }
                if (m_userInDisabled)
                        FRC_NetworkCommunication_observeUserProgramDisabled();
                if (m_userInAutonomous)
                        FRC_NetworkCommunication_observeUserProgramAutonomous();
        if (m_userInTeleop)
            FRC_NetworkCommunication_observeUserProgramTeleop();
        if (m_userInTest)
            FRC_NetworkCommunication_observeUserProgramTest();
        }
}

DriverStation* DriverStation::GetInstance()
{
        if (m_instance == NULL)
        {
                m_instance = new DriverStation();
        }
        return m_instance;
}

void DriverStation::GetData()
{
        static bool lastEnabled = false;
        getCommonControlData(m_controlData, WAIT_FOREVER);
        if (!lastEnabled && IsEnabled()) 
        {
                // If starting teleop, assume that autonomous just took up 15 seconds
                if (IsAutonomous())
                        m_approxMatchTimeOffset = Timer::GetFPGATimestamp();
                else
                        m_approxMatchTimeOffset = Timer::GetFPGATimestamp() - 15.0;
        }
        else if (lastEnabled && !IsEnabled())
        {
                m_approxMatchTimeOffset = -1.0;
        }
        lastEnabled = IsEnabled();
        semGive(m_newControlData);
}

void DriverStation::SetData()
{
        char *userStatusDataHigh;
        int32_t userStatusDataHighSize;
        char *userStatusDataLow;
        int32_t userStatusDataLowSize;

        Synchronized sync(m_statusDataSemaphore);

        m_dashboardInUseHigh->GetStatusBuffer(&userStatusDataHigh, &userStatusDataHighSize);
        m_dashboardInUseLow->GetStatusBuffer(&userStatusDataLow, &userStatusDataLowSize);
        setStatusData(GetBatteryVoltage(), m_digitalOut, m_updateNumber,
                userStatusDataHigh, userStatusDataHighSize, userStatusDataLow, userStatusDataLowSize, WAIT_FOREVER);
        
        m_dashboardInUseHigh->Flush();
        m_dashboardInUseLow->Flush();
}

float DriverStation::GetBatteryVoltage()
{
        if (m_batteryChannel == NULL)
                wpi_setWPIError(NullParameter);

        // The Analog bumper has a voltage divider on the battery source.
        // Vbatt *--/\/\/\--* Vsample *--/\/\/\--* Gnd
        //         680 Ohms            1000 Ohms
        return m_batteryChannel->GetAverageVoltage() * (1680.0 / 1000.0);
}

float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis)
{
        if (axis < 1 || axis > kJoystickAxes)
        {
                wpi_setWPIError(BadJoystickAxis);
                return 0.0;
        }

        int8_t value;
        switch (stick)
        {
                case 1:
                        value = m_controlData->stick0Axes[axis-1];
                        break;
                case 2:
                        value = m_controlData->stick1Axes[axis-1];
                        break;
                case 3:
                        value = m_controlData->stick2Axes[axis-1];
                        break;
                case 4:
                        value = m_controlData->stick3Axes[axis-1];
                        break;
                default:
                        wpi_setWPIError(BadJoystickIndex);
                        return 0.0;
        }
        
        float result;
        if (value < 0)
                result = ((float) value) / 128.0;
        else
                result = ((float) value) / 127.0;
        wpi_assert(result <= 1.0 && result >= -1.0);
        if (result > 1.0)
                result = 1.0;
        else if (result < -1.0)
                result = -1.0;
        return result;
}

short DriverStation::GetStickButtons(uint32_t stick)
{
        if (stick < 1 || stick > 4)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "stick must be between 1 and 4");

        switch (stick)
        {
        case 1:
                return m_controlData->stick0Buttons;
        case 2:
                return m_controlData->stick1Buttons;
        case 3:
                return m_controlData->stick2Buttons;
        case 4:
                return m_controlData->stick3Buttons;
        }
        return 0;
}

// 5V divided by 10 bits
#define kDSAnalogInScaling ((float)(5.0 / 1023.0))

float DriverStation::GetAnalogIn(uint32_t channel)
{
        if (channel < 1 || channel > 4)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 4");

        static uint8_t reported_mask = 0;
        if (!(reported_mask & (1 >> channel)))
        {
                nUsageReporting::report(nUsageReporting::kResourceType_DriverStationCIO, channel, nUsageReporting::kDriverStationCIO_Analog);
                reported_mask |= (1 >> channel);
        }

        switch (channel)
        {
        case 1:
                return kDSAnalogInScaling * m_controlData->analog1;
        case 2:
                return kDSAnalogInScaling * m_controlData->analog2;
        case 3:
                return kDSAnalogInScaling * m_controlData->analog3;
        case 4:
                return kDSAnalogInScaling * m_controlData->analog4;
        }
        return 0.0;
}

bool DriverStation::GetDigitalIn(uint32_t channel)
{
        if (channel < 1 || channel > 8)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");

        static uint8_t reported_mask = 0;
        if (!(reported_mask & (1 >> channel)))
        {
                nUsageReporting::report(nUsageReporting::kResourceType_DriverStationCIO, channel, nUsageReporting::kDriverStationCIO_DigitalIn);
                reported_mask |= (1 >> channel);
        }

        return ((m_controlData->dsDigitalIn >> (channel-1)) & 0x1) ? true : false;
}

void DriverStation::SetDigitalOut(uint32_t channel, bool value) 
{
        if (channel < 1 || channel > 8)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");

        static uint8_t reported_mask = 0;
        if (!(reported_mask & (1 >> channel)))
        {
                nUsageReporting::report(nUsageReporting::kResourceType_DriverStationCIO, channel, nUsageReporting::kDriverStationCIO_DigitalOut);
                reported_mask |= (1 >> channel);
        }

        m_digitalOut &= ~(0x1 << (channel-1));
        m_digitalOut |= ((uint8_t)value << (channel-1));
}

bool DriverStation::GetDigitalOut(uint32_t channel) 
{
        if (channel < 1 || channel > 8)
                wpi_setWPIErrorWithContext(ParameterOutOfRange, "channel must be between 1 and 8");

        return ((m_digitalOut >> (channel-1)) & 0x1) ? true : false;;
}

bool DriverStation::IsEnabled()
{
        return m_controlData->enabled;
}

bool DriverStation::IsDisabled()
{
        return !m_controlData->enabled;
}

bool DriverStation::IsAutonomous()
{
        return m_controlData->autonomous;
}

bool DriverStation::IsOperatorControl()
{
        return !(m_controlData->autonomous || m_controlData->test);
}

bool DriverStation::IsTest()
{
        return m_controlData->test;
}

bool DriverStation::IsNewControlData()
{
        return semTake(m_newControlData, NO_WAIT) == 0;
}

bool DriverStation::IsFMSAttached()
{
        return m_controlData->fmsAttached;
}

uint32_t DriverStation::GetPacketNumber()
{
        return m_controlData->packetIndex;
}

DriverStation::Alliance DriverStation::GetAlliance()
{
        if (m_controlData->dsID_Alliance == 'R') return kRed;
        if (m_controlData->dsID_Alliance == 'B') return kBlue;
        wpi_assert(false);
        return kInvalid;
}

uint32_t DriverStation::GetLocation()
{
        wpi_assert ((m_controlData->dsID_Position >= '1') && (m_controlData->dsID_Position <= '3'));
        return m_controlData->dsID_Position - '0';
}

void DriverStation::WaitForData()
{
        semTake(m_waitForDataSem, WAIT_FOREVER);
}

double DriverStation::GetMatchTime()
{
        if (m_approxMatchTimeOffset < 0.0)
                return 0.0;
        return Timer::GetFPGATimestamp() - m_approxMatchTimeOffset;
}

uint16_t DriverStation::GetTeamNumber()
{
        return m_controlData->teamID;
}