MRD Lab / ServoRingBuffer

Custom ring buffer written from scratch to allow data collection at a rigid rate, while allowing writing that data to either serial or an SD card whenever time allows.

Dependents:   SpindleBot_1_5b

ServoRingBuffer.cpp@2:da00ed8a1cd5, 2015-04-29 (annotated)

Committer:
labmrd
Date:
Wed Apr 29 21:52:23 2015 +0000
Revision:
2:da00ed8a1cd5
Parent:
0:01fb4588b738 
Collaborating!!!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
labmrd 0:01fb4588b738 1 #include "mbed.h"
labmrd 0:01fb4588b738 2 #include "ServoRingBuffer.h"
labmrd 0:01fb4588b738 3
labmrd 0:01fb4588b738 4 /**
labmrd 0:01fb4588b738 5 * Creates a ServoRingBuffer object
labmrd 0:01fb4588b738 6 */
labmrd 0:01fb4588b738 7 ServoRingBuffer::ServoRingBuffer()
labmrd 0:01fb4588b738 8 {
labmrd 0:01fb4588b738 9
labmrd 0:01fb4588b738 10 // Start timer
labmrd 0:01fb4588b738 11 t.start();
labmrd 0:01fb4588b738 12 purge();
labmrd 0:01fb4588b738 13 }
labmrd 0:01fb4588b738 14
labmrd 0:01fb4588b738 15 /**
labmrd 0:01fb4588b738 16 * Clears a ServoRingBuffer object
labmrd 0:01fb4588b738 17 * by setting the indices to zero
labmrd 0:01fb4588b738 18 */
labmrd 0:01fb4588b738 19 void ServoRingBuffer::purge(void)
labmrd 0:01fb4588b738 20 {
labmrd 0:01fb4588b738 21 lastWritten=0;
labmrd 0:01fb4588b738 22 lastRead=0;
labmrd 0:01fb4588b738 23 }
labmrd 0:01fb4588b738 24
labmrd 0:01fb4588b738 25 /**
labmrd 0:01fb4588b738 26 * Adds data, filling the time from the timer
labmrd 0:01fb4588b738 27 *
labmrd 0:01fb4588b738 28 * @param data The object with the data in it
labmrd 0:01fb4588b738 29 */
labmrd 0:01fb4588b738 30 void ServoRingBuffer::write(spindleData data){
labmrd 0:01fb4588b738 31
labmrd 0:01fb4588b738 32 static unsigned int currentIdx;
labmrd 0:01fb4588b738 33 currentIdx = (lastWritten + 1) % BUFFER_SIZE;
labmrd 0:01fb4588b738 34
labmrd 0:01fb4588b738 35 if(writesRemaining()==1) {
labmrd 0:01fb4588b738 36 //FREAK OUT!!!!!
labmrd 0:01fb4588b738 37 printf("Buffer overflow error! Aborting Data Aquisition\n");
labmrd 0:01fb4588b738 38 return;
labmrd 0:01fb4588b738 39 }
labmrd 0:01fb4588b738 40 ringBuffer[currentIdx]=data;
labmrd 0:01fb4588b738 41 ringBuffer[currentIdx].time = t.read_ms();
labmrd 0:01fb4588b738 42 lastWritten = currentIdx;
labmrd 0:01fb4588b738 43
labmrd 0:01fb4588b738 44 }
labmrd 0:01fb4588b738 45
labmrd 0:01fb4588b738 46 /**
labmrd 0:01fb4588b738 47 * Adds data, filling the time from the timer
labmrd 0:01fb4588b738 48 *
labmrd 0:01fb4588b738 49 * @param pos Input position
labmrd 0:01fb4588b738 50 * @param force Input force
labmrd 0:01fb4588b738 51 */
labmrd 0:01fb4588b738 52 void ServoRingBuffer::write(unsigned short pos0, unsigned short force0, unsigned short pos1, unsigned short force1)
labmrd 0:01fb4588b738 53 {
labmrd 0:01fb4588b738 54 static unsigned int currentIdx;
labmrd 0:01fb4588b738 55 currentIdx = (lastWritten + 1) % BUFFER_SIZE;
labmrd 0:01fb4588b738 56
labmrd 0:01fb4588b738 57 if(writesRemaining()==1) {
labmrd 0:01fb4588b738 58 //FREAK OUT!!!!!
labmrd 0:01fb4588b738 59 printf("Buffer overflow error! Aborting Data Aquisition\n");
labmrd 0:01fb4588b738 60 return;
labmrd 0:01fb4588b738 61 }
labmrd 0:01fb4588b738 62 ringBuffer[currentIdx].myServoData[0].pos = pos0;
labmrd 0:01fb4588b738 63 ringBuffer[currentIdx].myServoData[0].force = force0;
labmrd 0:01fb4588b738 64 ringBuffer[currentIdx].myServoData[1].pos = pos1;
labmrd 0:01fb4588b738 65 ringBuffer[currentIdx].myServoData[1].force = force1;
labmrd 0:01fb4588b738 66 ringBuffer[currentIdx].time = t.read_ms();
labmrd 0:01fb4588b738 67 lastWritten = currentIdx;
labmrd 0:01fb4588b738 68 }
labmrd 0:01fb4588b738 69
labmrd 0:01fb4588b738 70 /**
labmrd 0:01fb4588b738 71 * Writes each unread line of a ServoRingBuffer object
labmrd 0:01fb4588b738 72 * to serial as hex.
labmrd 0:01fb4588b738 73 */
labmrd 0:01fb4588b738 74 void ServoRingBuffer::dumpBufferToSerial(void)
labmrd 0:01fb4588b738 75 {
labmrd 0:01fb4588b738 76 unsigned int ii;
labmrd 0:01fb4588b738 77 static unsigned int currentReadIdx;
labmrd 0:01fb4588b738 78 for (ii = 0; ii < readsRemaining(); ii ++) {
labmrd 0:01fb4588b738 79 currentReadIdx = (lastRead + 1) % BUFFER_SIZE;
labmrd 0:01fb4588b738 80 printf("<%x,%x,%x,%x,%x>\n", ringBuffer[currentReadIdx].myServoData[0].pos,
labmrd 0:01fb4588b738 81 ringBuffer[currentReadIdx].myServoData[0].force,
labmrd 0:01fb4588b738 82 ringBuffer[currentReadIdx].myServoData[1].pos,
labmrd 0:01fb4588b738 83 ringBuffer[currentReadIdx].myServoData[1].force,
labmrd 0:01fb4588b738 84 ringBuffer[currentReadIdx].time);
labmrd 0:01fb4588b738 85 lastRead = currentReadIdx;
labmrd 0:01fb4588b738 86 }
labmrd 0:01fb4588b738 87 }
labmrd 0:01fb4588b738 88
labmrd 0:01fb4588b738 89 /**
labmrd 0:01fb4588b738 90 * Writes each unread line of a text file on an SD card
labmrd 0:01fb4588b738 91 * as hex.
labmrd 0:01fb4588b738 92 */
labmrd 0:01fb4588b738 93 void ServoRingBuffer::dumpBufferToSD(FILE * txtFile)
labmrd 0:01fb4588b738 94 {
labmrd 0:01fb4588b738 95 unsigned int ii;
labmrd 0:01fb4588b738 96 unsigned int jj;
labmrd 0:01fb4588b738 97 static unsigned int currentReadIdx;
labmrd 0:01fb4588b738 98 for (ii = 0; ii < readsRemaining(); ii ++) {
labmrd 0:01fb4588b738 99 currentReadIdx = (lastRead + 1) % BUFFER_SIZE;
labmrd 2:da00ed8a1cd5 100 fprintf(txtFile,"%d,", ringBuffer[currentReadIdx].time);
labmrd 0:01fb4588b738 101 for(jj=0;jj<NUMBER_OF_SPINDLES;jj++)
labmrd 0:01fb4588b738 102 {
labmrd 0:01fb4588b738 103 fprintf(txtFile,"%d,%d,", ringBuffer[currentReadIdx].myServoData[jj].pos,
labmrd 0:01fb4588b738 104 ringBuffer[currentReadIdx].myServoData[jj].force);
labmrd 0:01fb4588b738 105 }
labmrd 0:01fb4588b738 106 fprintf(txtFile,"%d,", ringBuffer[currentReadIdx].direction);
labmrd 0:01fb4588b738 107 fprintf(txtFile,"%d\n", ringBuffer[currentReadIdx].cycle);
labmrd 0:01fb4588b738 108 lastRead = currentReadIdx;
labmrd 0:01fb4588b738 109 }
labmrd 0:01fb4588b738 110 }
labmrd 0:01fb4588b738 111
labmrd 0:01fb4588b738 112 /**
labmrd 0:01fb4588b738 113 * Calculates how many spaces are left in the buffer for writing
labmrd 0:01fb4588b738 114 *
labmrd 0:01fb4588b738 115 * @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!
labmrd 0:01fb4588b738 116 *
labmrd 0:01fb4588b738 117 * @retval Number of entries available for writing (But never go below 1)
labmrd 0:01fb4588b738 118 */
labmrd 0:01fb4588b738 119 unsigned int ServoRingBuffer::writesRemaining(void)
labmrd 0:01fb4588b738 120 {
labmrd 0:01fb4588b738 121 return (BUFFER_SIZE + lastRead - lastWritten) % BUFFER_SIZE;
labmrd 0:01fb4588b738 122 }
labmrd 0:01fb4588b738 123
labmrd 0:01fb4588b738 124 /**
labmrd 0:01fb4588b738 125 * Calculates how many entries in the buffer are written but unread
labmrd 0:01fb4588b738 126 *
labmrd 0:01fb4588b738 127 * @retval Number of entries written but unread
labmrd 0:01fb4588b738 128 */
labmrd 0:01fb4588b738 129 unsigned int ServoRingBuffer::readsRemaining(void)
labmrd 0:01fb4588b738 130 {
labmrd 0:01fb4588b738 131 return (BUFFER_SIZE + lastWritten - lastRead) % BUFFER_SIZE;
labmrd 0:01fb4588b738 132 }
labmrd 0:01fb4588b738 133
labmrd 0:01fb4588b738 134 /**
labmrd 0:01fb4588b738 135 * Normalized current fullness of buffer, for checking health of buffer
labmrd 0:01fb4588b738 136 *
labmrd 0:01fb4588b738 137 * @retval Fraction of buffer with unread data, 0 -> 1
labmrd 0:01fb4588b738 138 */
labmrd 0:01fb4588b738 139 float ServoRingBuffer::percentFull(void)
labmrd 0:01fb4588b738 140 {
labmrd 0:01fb4588b738 141 return float(readsRemaining())/BUFFER_SIZE;
labmrd 0:01fb4588b738 142 }