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

#include "Counter.h"
#include "DigitalInput.h"
#include "DigitalOutput.h"
#include "Timer.h"
#include "Utility.h"
#include "WPIErrors.h"

const double Ultrasonic::kPingTime;     
const UINT32 Ultrasonic::kPriority;     
const double Ultrasonic::kMaxUltrasonicTime;    
const double Ultrasonic::kSpeedOfSoundInchesPerSec;
Task Ultrasonic::m_task("UltrasonicChecker", (FUNCPTR)UltrasonicChecker); // task doing the round-robin automatic sensing
Ultrasonic *Ultrasonic::m_firstSensor = NULL; // head of the ultrasonic sensor list
bool Ultrasonic::m_automaticEnabled = false; // automatic round robin mode
SEM_ID Ultrasonic::m_semaphore = 0;

void Ultrasonic::UltrasonicChecker()
{
        Ultrasonic *u = NULL;
        while (m_automaticEnabled)
        {
                if (u == NULL) u = m_firstSensor;
                if (u == NULL) return;
                if (u->IsEnabled())
                        u->m_pingChannel->Pulse(kPingTime);     // do the ping
                u = u->m_nextSensor;
                Wait(0.1);                                                      // wait for ping to return
        }
}

void Ultrasonic::Initialize()
{
        bool originalMode = m_automaticEnabled;
        if (m_semaphore == 0) m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
        SetAutomaticMode(false); // kill task when adding a new sensor
        semTake(m_semaphore, WAIT_FOREVER); // link this instance on the list
        {
                m_nextSensor = m_firstSensor;
                m_firstSensor = this;
        }
        semGive(m_semaphore);

        m_counter = new Counter(m_echoChannel); // set up counter for this sensor
        m_counter->SetMaxPeriod(1.0);
        m_counter->SetSemiPeriodMode(true);
        m_counter->Reset();
        m_counter->Start();
        m_enabled = true; // make it available for round robin scheduling
        SetAutomaticMode(originalMode);
}

Ultrasonic::Ultrasonic(UINT32 pingChannel, UINT32 echoChannel, DistanceUnit units)
{
        m_pingChannel = new DigitalOutput(pingChannel);
        m_echoChannel = new DigitalInput(echoChannel);
        m_allocatedChannels = true;
        m_units = units;
        Initialize();
}

Ultrasonic::Ultrasonic(DigitalOutput *pingChannel, DigitalInput *echoChannel, DistanceUnit units)
{
        if (pingChannel == NULL || echoChannel == NULL)
        {
                wpi_setWPIError(NullParameter);
                return;
        }
        m_allocatedChannels = false;
        m_pingChannel = pingChannel;
        m_echoChannel = echoChannel;
        m_units = units;
        Initialize();
}

Ultrasonic::Ultrasonic(DigitalOutput &pingChannel, DigitalInput &echoChannel, DistanceUnit units)
{
        m_allocatedChannels = false;
        m_pingChannel = &pingChannel;
        m_echoChannel = &echoChannel;
        m_units = units;
        Initialize();
}

Ultrasonic::Ultrasonic(UINT8 pingModuleNumber, UINT32 pingChannel,
                UINT8 echoModuleNumber, UINT32 echoChannel, DistanceUnit units)
{
        m_pingChannel = new DigitalOutput(pingModuleNumber, pingChannel);
        m_echoChannel = new DigitalInput(echoModuleNumber, echoChannel);
        m_allocatedChannels = true;
        m_units = units;
        Initialize();
}

Ultrasonic::~Ultrasonic()
{
        bool wasAutomaticMode = m_automaticEnabled;
        SetAutomaticMode(false);
        if (m_allocatedChannels)
        {
                delete m_pingChannel;
                delete m_echoChannel;
        }
        wpi_assert(m_firstSensor != NULL);

        semTake(m_semaphore, WAIT_FOREVER);
        {
                if (this == m_firstSensor)
                {
                        m_firstSensor = m_nextSensor;
                        if (m_firstSensor == NULL)
                        {
                                SetAutomaticMode(false);
                        }
                }
                else
                {
                        wpi_assert(m_firstSensor->m_nextSensor != NULL);
                        for (Ultrasonic *s = m_firstSensor; s != NULL; s = s->m_nextSensor)
                        {
                                if (this == s->m_nextSensor)
                                {
                                        s->m_nextSensor = s->m_nextSensor->m_nextSensor;
                                        break;
                                }
                        }
                }
        }
        semGive(m_semaphore);
        if (m_firstSensor != NULL && wasAutomaticMode)
                SetAutomaticMode(true);
}

void Ultrasonic::SetAutomaticMode(bool enabling)
{
        if (enabling == m_automaticEnabled)
                return; // ignore the case of no change

        m_automaticEnabled = enabling;
        if (enabling)
        {
                // enabling automatic mode.
                // Clear all the counters so no data is valid
                for (Ultrasonic *u = m_firstSensor; u != NULL; u = u->m_nextSensor)
                {
                        u->m_counter->Reset();
                }
                // Start round robin task
                wpi_assert(m_task.Verify() == false);   // should be false since was previously disabled
                m_task.Start();
        }
        else
        {
                // disabling automatic mode. Wait for background task to stop running.
                while (m_task.Verify())
                        Wait(0.15);     // just a little longer than the ping time for round-robin to stop

                // clear all the counters (data now invalid) since automatic mode is stopped
                for (Ultrasonic *u = m_firstSensor; u != NULL; u = u->m_nextSensor)
                {
                        u->m_counter->Reset();
                }
                m_task.Stop();
        }
}

void Ultrasonic::Ping()
{
        // TODO: Either assert or disable, not both.
        wpi_assert(!m_automaticEnabled);
        SetAutomaticMode(false); // turn off automatic round robin if pinging single sensor
        m_counter->Reset(); // reset the counter to zero (invalid data now)
        m_pingChannel->Pulse(kPingTime); // do the ping to start getting a single range
}

bool Ultrasonic::IsRangeValid()
{
        return m_counter->Get() > 1;
}

double Ultrasonic::GetRangeInches()
{
        if (IsRangeValid())
                return m_counter->GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
        else
                return 0;
}

double Ultrasonic::GetRangeMM()
{
        return GetRangeInches() * 25.4;
}

double Ultrasonic::PIDGet()
{
        switch(m_units) 
        {
        case Ultrasonic::kInches:
                return GetRangeInches();
        case Ultrasonic::kMilliMeters:
                return GetRangeMM();
        default:
                return 0.0;
        }
}

void Ultrasonic::SetDistanceUnits(DistanceUnit units)
{
        m_units = units;
}

Ultrasonic::DistanceUnit Ultrasonic::GetDistanceUnits()
{
        return m_units;
}