File content as of revision 2:da00ed8a1cd5:

#include "mbed.h"
#include "ServoRingBuffer.h"

/**
 *  Creates a ServoRingBuffer object
 */
ServoRingBuffer::ServoRingBuffer()
{

    // Start timer
    t.start();
    purge();
}

/**
 *  Clears a ServoRingBuffer object
 *  by setting the indices to zero
 */
void ServoRingBuffer::purge(void)
{
    lastWritten=0;
    lastRead=0;
}

/**
 *  Adds data, filling the time from the timer
 *
 * @param data The object with the data in it
 */
void ServoRingBuffer::write(spindleData data){
    
    static unsigned int currentIdx;
    currentIdx = (lastWritten + 1) % BUFFER_SIZE;

    if(writesRemaining()==1) {
        //FREAK OUT!!!!!
        printf("Buffer overflow error!  Aborting Data Aquisition\n");
        return;
    }
    ringBuffer[currentIdx]=data;
    ringBuffer[currentIdx].time = t.read_ms();
    lastWritten = currentIdx;
    
}

/**
 *  Adds data, filling the time from the timer
 *
 *  @param pos Input position
 *  @param force Input force
 */
void ServoRingBuffer::write(unsigned short pos0, unsigned short force0, unsigned short pos1, unsigned short force1)
{
    static unsigned int currentIdx;
    currentIdx = (lastWritten + 1) % BUFFER_SIZE;

    if(writesRemaining()==1) {
        //FREAK OUT!!!!!
        printf("Buffer overflow error!  Aborting Data Aquisition\n");
        return;
    }
    ringBuffer[currentIdx].myServoData[0].pos = pos0;
    ringBuffer[currentIdx].myServoData[0].force = force0;
    ringBuffer[currentIdx].myServoData[1].pos = pos1;
    ringBuffer[currentIdx].myServoData[1].force = force1;
    ringBuffer[currentIdx].time = t.read_ms();
    lastWritten = currentIdx;
}

/**
 *  Writes each unread line of a ServoRingBuffer object
 *  to serial as hex.
 */
void ServoRingBuffer::dumpBufferToSerial(void)
{
    unsigned int ii;
    static unsigned int currentReadIdx;
    for (ii = 0; ii < readsRemaining(); ii ++) {
        currentReadIdx = (lastRead + 1) % BUFFER_SIZE;
        printf("<%x,%x,%x,%x,%x>\n", ringBuffer[currentReadIdx].myServoData[0].pos,
                                     ringBuffer[currentReadIdx].myServoData[0].force,
                                     ringBuffer[currentReadIdx].myServoData[1].pos,
                                     ringBuffer[currentReadIdx].myServoData[1].force,
                                     ringBuffer[currentReadIdx].time);
        lastRead = currentReadIdx;
    }
}

/**
 *  Writes each unread line of a text file on an SD card
 *  as hex.
 */
void ServoRingBuffer::dumpBufferToSD(FILE * txtFile)
{
    unsigned int ii;
    unsigned int jj;
    static unsigned int currentReadIdx;
    for (ii = 0; ii < readsRemaining(); ii ++) {
        currentReadIdx = (lastRead + 1) % BUFFER_SIZE;
        fprintf(txtFile,"%d,", ringBuffer[currentReadIdx].time);
        for(jj=0;jj<NUMBER_OF_SPINDLES;jj++)
        {
            fprintf(txtFile,"%d,%d,", ringBuffer[currentReadIdx].myServoData[jj].pos,
                                          ringBuffer[currentReadIdx].myServoData[jj].force);
        }
        fprintf(txtFile,"%d,", ringBuffer[currentReadIdx].direction);
        fprintf(txtFile,"%d\n", ringBuffer[currentReadIdx].cycle);
        lastRead = currentReadIdx;
    }
}

/**
 *  Calculates how many spaces are left in the buffer for writing
 *
 *  @note If this reads 1, you actually can't write anymore, because an entirely full buffer is identical to an entirely empty one, so we must always leave one entry blank.  Sorry!
 *
 *  @retval Number of entries available for writing (But never go below 1)
 */
unsigned int ServoRingBuffer::writesRemaining(void)
{
    return (BUFFER_SIZE + lastRead - lastWritten) % BUFFER_SIZE;
}

/**
 *  Calculates how many entries in the buffer are written but unread
 *
 *  @retval Number of entries written but unread
 */
unsigned int ServoRingBuffer::readsRemaining(void)
{
    return (BUFFER_SIZE + lastWritten - lastRead) % BUFFER_SIZE;
}

/**
 *  Normalized current fullness of buffer, for checking health of buffer
 *
 *  @retval Fraction of buffer with unread data, 0 -> 1
 */
float ServoRingBuffer::percentFull(void)
{
    return float(readsRemaining())/BUFFER_SIZE;
}