ServoRingBuffer.cpp

00001 #include "mbed.h"
00002 #include "ServoRingBuffer.h"
00003 
00004 /**
00005  *  Creates a ServoRingBuffer object
00006  */
00007 ServoRingBuffer::ServoRingBuffer()
00008 {
00009 
00010     // Start timer
00011     t.start();
00012     purge();
00013 }
00014 
00015 /**
00016  *  Clears a ServoRingBuffer object
00017  *  by setting the indices to zero
00018  */
00019 void ServoRingBuffer::purge(void)
00020 {
00021     lastWritten=0;
00022     lastRead=0;
00023 }
00024 
00025 /**
00026  *  Adds data, filling the time from the timer
00027  *
00028  * @param data The object with the data in it
00029  */
00030 void ServoRingBuffer::write(spindleData data){
00031     
00032     static unsigned int currentIdx;
00033     currentIdx = (lastWritten + 1) % BUFFER_SIZE;
00034 
00035     if(writesRemaining()==1) {
00036         //FREAK OUT!!!!!
00037         printf("Buffer overflow error!  Aborting Data Aquisition\n");
00038         return;
00039     }
00040     ringBuffer[currentIdx]=data;
00041     ringBuffer[currentIdx].time = t.read_ms();
00042     lastWritten = currentIdx;
00043     
00044 }
00045 
00046 /**
00047  *  Adds data, filling the time from the timer
00048  *
00049  *  @param pos Input position
00050  *  @param force Input force
00051  */
00052 void ServoRingBuffer::write(unsigned short pos0, unsigned short force0, unsigned short pos1, unsigned short force1)
00053 {
00054     static unsigned int currentIdx;
00055     currentIdx = (lastWritten + 1) % BUFFER_SIZE;
00056 
00057     if(writesRemaining()==1) {
00058         //FREAK OUT!!!!!
00059         printf("Buffer overflow error!  Aborting Data Aquisition\n");
00060         return;
00061     }
00062     ringBuffer[currentIdx].myServoData[0].pos = pos0;
00063     ringBuffer[currentIdx].myServoData[0].force = force0;
00064     ringBuffer[currentIdx].myServoData[1].pos = pos1;
00065     ringBuffer[currentIdx].myServoData[1].force = force1;
00066     ringBuffer[currentIdx].time = t.read_ms();
00067     lastWritten = currentIdx;
00068 }
00069 
00070 /**
00071  *  Writes each unread line of a ServoRingBuffer object
00072  *  to serial as hex.
00073  */
00074 void ServoRingBuffer::dumpBufferToSerial(void)
00075 {
00076     unsigned int ii;
00077     static unsigned int currentReadIdx;
00078     for (ii = 0; ii < readsRemaining(); ii ++) {
00079         currentReadIdx = (lastRead + 1) % BUFFER_SIZE;
00080         printf("<%x,%x,%x,%x,%x>\n", ringBuffer[currentReadIdx].myServoData[0].pos,
00081                                      ringBuffer[currentReadIdx].myServoData[0].force,
00082                                      ringBuffer[currentReadIdx].myServoData[1].pos,
00083                                      ringBuffer[currentReadIdx].myServoData[1].force,
00084                                      ringBuffer[currentReadIdx].time);
00085         lastRead = currentReadIdx;
00086     }
00087 }
00088 
00089 /**
00090  *  Writes each unread line of a text file on an SD card
00091  *  as hex.
00092  */
00093 void ServoRingBuffer::dumpBufferToSD(FILE * txtFile)
00094 {
00095     unsigned int ii;
00096     unsigned int jj;
00097     static unsigned int currentReadIdx;
00098     for (ii = 0; ii < readsRemaining(); ii ++) {
00099         currentReadIdx = (lastRead + 1) % BUFFER_SIZE;
00100         fprintf(txtFile,"%d,", ringBuffer[currentReadIdx].time);
00101         for(jj=0;jj<NUMBER_OF_SPINDLES;jj++)
00102         {
00103             fprintf(txtFile,"%d,%d,", ringBuffer[currentReadIdx].myServoData[jj].pos,
00104                                           ringBuffer[currentReadIdx].myServoData[jj].force);
00105         }
00106         fprintf(txtFile,"%d,", ringBuffer[currentReadIdx].direction);
00107         fprintf(txtFile,"%d\n", ringBuffer[currentReadIdx].cycle);
00108         lastRead = currentReadIdx;
00109     }
00110 }
00111 
00112 /**
00113  *  Calculates how many spaces are left in the buffer for writing
00114  *
00115  *  @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!
00116  *
00117  *  @retval Number of entries available for writing (But never go below 1)
00118  */
00119 unsigned int ServoRingBuffer::writesRemaining(void)
00120 {
00121     return (BUFFER_SIZE + lastRead - lastWritten) % BUFFER_SIZE;
00122 }
00123 
00124 /**
00125  *  Calculates how many entries in the buffer are written but unread
00126  *
00127  *  @retval Number of entries written but unread
00128  */
00129 unsigned int ServoRingBuffer::readsRemaining(void)
00130 {
00131     return (BUFFER_SIZE + lastWritten - lastRead) % BUFFER_SIZE;
00132 }
00133 
00134 /**
00135  *  Normalized current fullness of buffer, for checking health of buffer
00136  *
00137  *  @retval Fraction of buffer with unread data, 0 -> 1
00138  */
00139 float ServoRingBuffer::percentFull(void)
00140 {
00141     return float(readsRemaining())/BUFFER_SIZE;
00142 }