Revision 0:e85788b14028, committed 2016-02-12

Comitter:

aktk

Date:

Fri Feb 12 11:02:15 2016 +0000

Child:

1:2053662b1167

Commit message:

Distance data range : [0:1024] -> [0:256];

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TRP105F_Spline.cpp	Fri Feb 12 11:02:15 2016 +0000
@@ -0,0 +1,592 @@
+#define DEBUG
+#include "TRP105F_Spline.h"
+
+//  To get voltage of TRP105F
+AnalogIn g_Sensor_Voltage(p16);
+//  To get sample distance via seral com
+Serial g_Serial_Signal(USBTX, USBRX);
+
+LocalFileSystem local("local");  // マウントポイントを定義（ディレクトリパスになる）
+// for debug
+#ifdef DEBUG
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+#endif
+
+TRP105FS::TRP105FS()
+    :_Data_Input_Type(SYSTEM)
+{
+    _Sample_Num = 5;
+    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
+    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
+
+    //calibrateSensor();
+}
+
+TRP105FS::TRP105FS(
+    unsigned int arg_num
+)
+    :_Data_Input_Type(SYSTEM)
+{
+    if(arg_num > _ENUM) _Sample_Num = _ENUM;
+    else _Sample_Num = arg_num;
+
+    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
+    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
+
+    //calibrateSensor();
+}
+
+TRP105FS::TRP105FS(
+    unsigned int arg_num,
+    DataInType arg_dit
+)
+    :_Data_Input_Type(arg_dit)
+{
+    if(arg_num > _ENUM) _Sample_Num = _ENUM;
+    else _Sample_Num = arg_num;
+
+    _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
+    _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
+
+    //calibrateSensor();
+}
+
+TRP105FS::~TRP105FS()
+{
+    free(_Sample_Set);
+    free(_u_spline);
+}
+
+unsigned short TRP105FS::getDistance()
+{
+    int idx;
+    unsigned short pv = 0;
+
+    //low pass filter
+    for(int i = 0; i < 10; i++)
+        pv += g_Sensor_Voltage.read_u16() / 10;
+
+    idx = _getNearest(_LIDX, _RIDX, pv);
+
+    if (idx != 0xFFFF)    //  unless occuring error
+        return _Set[idx].dst;
+    else
+        return 0xFFFF;
+}
+
+/*
+    Function to find a set whose vol member is nearest a voltage from the sensor, recursively.
+
+                   SHORT                                                          LONG
+    +------------>  HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
+    |(if voltage form sensor < threshold[cidx])   |||   (if threshold[cidx] < voltage form sensor)|
+    |               HIGH[lidx , ... , cidx]LOW    |||    HIGH[cidx+1 , ... , ridx]LOW             |
+    |                |                                                            |               |
+    +----------------+                                                            +---------------+
+*/
+int TRP105FS::_getNearest(
+    int arg_lidx,
+    int arg_ridx,
+    unsigned short arg_vol
+)
+{
+    int cidx = (arg_lidx + arg_ridx) / 2;
+
+    //  When the number of element to compare is only one, return it as result.
+    if(arg_lidx == arg_ridx)
+        return cidx;
+    //  If the voltage from the sensor is lower than the center threshold
+    //  (_set[cidx] > _threshold[cidx] > _set[cidx+1])
+    else if(arg_vol > _Threshold[cidx])
+        return _getNearest(arg_lidx, cidx,     arg_vol);
+    //  If the voltage from the sensor is higher than the center threshold
+    else if(arg_vol < _Threshold[cidx])
+        return _getNearest(cidx + 1, arg_ridx, arg_vol);
+    //  If the voltage from the sensor eauals the center threshold
+    else //(arg_vol == _Treshold[cidx].vol)
+        return cidx;
+}
+
+void TRP105FS::_sampleData()
+{
+    int     tmp;
+    char    sig;
+    unsigned short tmp_vol;
+    VDset tmp_set[_ENUM];   // for bucket sort
+
+    //  Evry set, first get dst data via serai com,
+    //  next, get vol data,
+    //  and then do same for next index set.
+    for(int i = 0; i < _Sample_Num; i++) {
+        //
+        //  Recieve a Distance datus and store it into member
+        //
+        if(_Data_Input_Type == KEYBORD) {
+            g_Serial_Signal.putc('>');
+            _Sample_Set[i].dst = 0;
+            do {
+                sig = g_Serial_Signal.getc();
+                if('0' <= sig && sig <= '9') {
+                    _Sample_Set[i].dst = 10 * _Sample_Set[i].dst + sig - 48;
+                    g_Serial_Signal.putc(char(sig));
+                } else if(sig == 0x08) {
+                    _Sample_Set[i].dst = 0;
+                    g_Serial_Signal.printf("[canseled!]");
+                    g_Serial_Signal.putc('\n');
+                    g_Serial_Signal.putc('>');
+                }
+            } while (!(sig == 0x0a || sig == 0x0d));
+            g_Serial_Signal.putc('\n');
+        } else { //_Data_Input_Type == SYSTEM
+            _Sample_Set[i].dst = g_Serial_Signal.getc();
+        }
+
+        //  if input data is over the bound calibrate it below
+        if (_Sample_Set[i].dst < 0)
+            _Sample_Set[i].dst = 0;
+        else if (_ENUM < _Sample_Set[i].dst)
+            _Sample_Set[i].dst = _ENUM;
+        //
+        //  Recieve a Voltage datus and store it into member
+        //
+        //  LOW PASS FILTERED
+        //  Get 10 data and store mean as a sample.
+        //  After get one original sample, system waits for 0.1 sec,
+        //  thus it takes 1 sec evry sampling.
+        _Sample_Set[i].vol = 0;
+        for(int j = 0; j < 10; j++) {
+            //unsigned short 's range [0 , 65535]
+            //the Number of significant figures of read voltage is 3 or 4.
+            tmp_vol = g_Sensor_Voltage.read_u16();
+#ifdef DEBUG
+            g_Serial_Signal.printf("%d,",tmp_vol);
+#endif
+            _Sample_Set[i].vol += (tmp_vol / 10);
+            wait(0.1);
+        }
+#ifdef DEBUG
+        g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].vol);
+#endif
+    }
+    //
+    //  Sort set data array in distanceAscending order
+    //
+    //  Bucket sort
+    for(int i = 0; i < _ENUM; i++)
+        tmp_set[i].dst = 0xaaaa;
+    tmp = 0;
+    for(int i = 0; i < _Sample_Num; i++) {
+        //  use dst as index for dst range [2,20]
+        if (tmp_set[_Sample_Set[i].dst].dst == 0xaaaa) {
+            tmp_set[_Sample_Set[i].dst].dst = _Sample_Set[i].dst;
+            tmp_set[_Sample_Set[i].dst].vol = _Sample_Set[i].vol;
+        } else { // if a same dst has been input, calcurate mean.
+            tmp_set[_Sample_Set[i].dst].vol += _Sample_Set[i].vol;
+            tmp_set[_Sample_Set[i].dst].vol /= 2;
+            tmp++;
+        }
+    }
+#ifdef DEBUG
+    g_Serial_Signal.printf("%d\n", _Sample_Num );
+#endif
+    //  substruct tmp from number of sample.
+    _Sample_Num -= tmp;
+
+#ifdef DEBUG
+    g_Serial_Signal.printf("tmp: %d\n", tmp );
+#endif
+    //  apply sort on _Sample_Set
+    tmp = 0;
+    for(int i = 0; i < _ENUM; i++) {
+        if(tmp_set[i].dst != 0xaaaa) {
+            _Sample_Set[i - tmp].dst = tmp_set[i].dst;
+            _Sample_Set[i - tmp].vol = tmp_set[i].vol;
+        } else //  if no data, skip it
+            tmp++;
+    }
+}
+
+//
+//  Function to define _u_spline, specific constants of spline.
+//
+void TRP105FS::_makeSpline()
+{
+    //  x: dst, distance
+    //  y: vol, voltage
+    //
+    //  N: max of index <=> (_Sample_Num - 1)
+    //
+    //  _u_spline[i] === d^2/dx^2(Spline f)[i]
+    //  i:[0,N]
+    //  _u_spline[0] = _u_spline[N] = 0
+    //
+    //  h[i] = x[i+1] - x[i]
+    //  i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
+    double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
+    //unsigned short *h __attribute__((at(0x20080000)));
+    //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
+    //  v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
+    //  i:[0,N-2]
+    double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
+    //unsigned short *v __attribute__((at(0x20080100)));
+    //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
+    //  temporary array whose num of elm equals v array
+    double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
+    //unsigned short *w __attribute__((at(0x20080200)));
+    //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
+    //
+    //  [ 2(h[0]+h[1])  , h[1]          ,                                 O                 ]   [_u[1]  ]   [v[0]  ]
+    //  [ h[1]          , 2(h[1]+h[2])  , h[2]                                              ]   [_u[2]  ]   [v[1]  ]
+    //  [                       ...                                                         ] * [...    ] = [...   ]
+    //  [                   h[j]          , 2(h[j]+h[j+1])  , h[j+1]                        ]   [_u[j+1]]   [v[j]  ]
+    //  [                                   ...                                             ]   [ ...   ]   [ ...  ]
+    //  [                               h[N-3]        , 2(h[N-3]+h[N-2]), h[N-2]            ]   [_u[j+1]]   [v[j]  ]
+    //  [       O                                       h[N-2]          , 2(h[N-2]+h[N-1])  ]   [_u[N-1]]   [v[N-2]]
+    //
+    // For LU decomposition
+    double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
+    //unsigned short *Upper __attribute__((at(0x20080300)));
+    //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
+    double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
+    //unsigned short *Lower __attribute__((at(0x20080400)));
+    //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
+#ifdef DEBUG
+    _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
+#endif
+    for(int i = 0; i < _Sample_Num - 1; i++)
+        h[i] =  (double)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
+    //(unsigned short)(_Sample_Set[i + 1].dst - _Sample_Set[i].dst);
+
+    for(int i = 0; i < _Sample_Num - 2; i++)
+        v[i] = 6 * (
+                   ((double)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol)) / h[i + 1]
+                   //(unsigned short)(_Sample_Set[i + 2].vol - _Sample_Set[i + 1].vol) / h[i + 1]
+                   -
+                   ((double)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol))     / h[i]
+                   //(unsigned short)(_Sample_Set[i + 1].vol - _Sample_Set[i].vol) / h[i]
+               );
+
+    //
+    //  LU decomposition
+    //
+    Upper[0] = 2 * (h[0] + h[1]);
+    Lower[0] = 0;
+    for (int i = 1; i < _Sample_Num - 2; i++) {
+        Lower[i] = h[i] / Upper[i - 1];
+        Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
+    }
+
+
+    //
+    //  forward substitution
+    //
+    w[0] = v[0];
+    for (int i = 1; i < _Sample_Num - 2; i ++) {
+        w[i] = v[i] - Lower[i] * w[i-1];
+    }
+
+
+    //
+    //  backward substitution
+    //
+    _u_spline[_Sample_Num - 2] =  w[_Sample_Num - 3]                         / Upper[_Sample_Num - 3];
+    for(int i = _Sample_Num - 3; i > 0; i--) {
+        _u_spline[i]           = (w[(i - 1)] -  h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
+    }
+
+    // _u_spline[i] === d^2/dx^2(Spline f)[i]
+    _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
+
+#ifdef DEBUG
+    _printOutData(h, _Sample_Num - 1, "h");
+    _printOutData(v, _Sample_Num - 2, "v");
+    _printOutData(w, _Sample_Num - 2, "w");
+    _printOutData(Upper, _Sample_Num - 2, "Upper");
+    _printOutData(Lower, _Sample_Num - 2, "Lower");
+    _printOutData(_u_spline, _Sample_Num, "u");
+#endif
+    free(h);
+    free(v);
+    free(w);
+    free(Upper);
+    free(Lower);
+}
+//
+//  Function to return Voltage for distance.
+//
+unsigned short TRP105FS:: _getSplineYof(
+    double arg_x       //  the argument is supposed as distance [mm]
+)
+{
+    double y;       //  voltage calculated by spline polynomial
+    double a,b,c,d; //  which is specific constant of spline, and can be expressed with _u.
+    int itv = 0;    //  interval(section) of interpolation
+    //  the number of interval is less 1 than the number of sample sets,
+    //  which means the max number of interval is _Sample_num - 2.
+    if((double)(_Sample_Set[0].dst) <= arg_x) {
+        while (!((double)(_Sample_Set[itv].dst) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].dst))) {
+            itv++;
+            if(itv > _Sample_Num - 2) {
+                itv = _Sample_Num - 2;
+                break;
+            }
+        }
+    }
+    a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst);
+    b = (double)(_u_spline[itv]) / 2.0;
+    c = (double)(_Sample_Set[itv + 1].vol - _Sample_Set[itv].vol) / (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst)
+        -
+        (double)(_Sample_Set[itv + 1].dst - _Sample_Set[itv].dst) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
+    d = (double)(_Sample_Set[itv].vol);
+    //  cubic spline expression
+    y = a * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
+        +
+        b * (arg_x - (double)(_Sample_Set[itv].dst)) * (arg_x - (double)(_Sample_Set[itv].dst))
+        +
+        c * (arg_x - (double)(_Sample_Set[itv].dst))
+        +
+        d;
+
+#ifdef DEBUG2
+    g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
+    g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
+    g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
+#endif
+
+    return ((unsigned short)(int)y);
+}
+
+void TRP105FS::calibrateSensor()
+{
+    _sampleData();
+    _makeSpline();
+
+    for(int i = 0; i < _ENUM; i++) {
+        _Set[i].dst     = i;
+        _Set[i].vol     = _getSplineYof((double)(_Set[i].dst));
+        _Threshold[i]   = _getSplineYof((double)(_Set[i].dst) + 0.5);
+#ifdef DEBUG2
+        g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
+#endif
+    }
+}
+
+void TRP105FS::saveSetting()
+{
+    FILE *fp;
+
+    fp = fopen("/local/savedata.log", "wb");
+
+    for(int i = 0; i < _ENUM; i++) {
+        fwrite(&_Set[i].dst,    sizeof(unsigned short), 1, fp);
+        fputc(0x2c, fp);
+        fwrite(&_Set[i].vol,    sizeof(unsigned short), 1, fp);
+        fputc(0x2c, fp);
+        fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
+        fputc(0x3b, fp);
+    }
+    fwrite(&_Sample_Num, sizeof(int), 1, fp);
+    fputc(0x3b, fp);
+    for(int i = 0; i < _Sample_Num; i++) {
+        fwrite(&_Sample_Set[i].dst,    sizeof(unsigned short), 1, fp);
+        fputc(0x2c, fp);
+        fwrite(&_Sample_Set[i].vol,    sizeof(unsigned short), 1, fp);
+        fputc(0x3b, fp);
+    }
+    fclose(fp);
+
+}
+
+void TRP105FS::printThresholds()
+{
+    for(int i = 0; i < _ENUM; i++)
+        g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
+}
+void TRP105FS::loadSetting()
+{
+    FILE *fp;
+    char tmp;
+
+    //sprintf(filepath, "/local/%s", filename);
+    //fp = fopen(filepath, "rb");
+    fp = fopen("/local/savedata.log", "rb");
+    for(int i = 0; i < _ENUM; i++) {
+
+        fread(&_Set[i].dst,     sizeof(unsigned short), 1, fp);
+        fread(&tmp,             sizeof(char),           1, fp);
+#ifdef DEBUG2
+        g_Serial_Signal.printf("%d%c",  _Set[i].dst, tmp);
+#endif
+
+        fread(&_Set[i].vol,     sizeof(unsigned short), 1, fp);
+        fread(&tmp,             sizeof(char), 1, fp);
+#ifdef DEBUG2
+        g_Serial_Signal.printf("%d%c",  _Set[i].vol, tmp);
+#endif
+
+        fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
+        fread(&tmp,             sizeof(char), 1, fp);
+#ifdef DEBUG2
+        g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
+#endif
+    }
+
+    fread(&_Sample_Num, sizeof(unsigned short),  1, fp);
+    fread(&tmp,         sizeof(char), 1, fp);
+
+    for(int i = 0; i < _Sample_Num; i++) {
+        fread(&_Sample_Set[i].dst, sizeof(unsigned short), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+        fread(&_Sample_Set[i].vol, sizeof(unsigned short), 1, fp);
+        fread(&tmp, sizeof(char),1,fp);
+    }
+    fclose(fp);
+}
+
+
+void TRP105FS::saveSetting(
+    const char *filename
+)
+{
+    FILE *fp;
+    char *filepath;
+    int fnnum = 0;
+
+    while (filename[fnnum] != 0) fnnum++;
+    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
+
+    sprintf(filepath, "/local/%s", filename);
+    fp = fopen(filepath, "wb");
+
+    for(int i = 0; i < _ENUM; i++) {
+        fwrite(&_Set[i].dst,    sizeof(unsigned short), 1, fp);
+        fputc(0x2c, fp);
+        fwrite(&_Set[i].vol,    sizeof(unsigned short), 1, fp);
+        fputc(0x2c, fp);
+        fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
+        fputc(0x3b, fp);
+    }
+    fwrite(&_Sample_Num, sizeof(int), 1, fp);
+    fputc(0x3b, fp);
+    for(int i = 0; i < _Sample_Num; i++) {
+        fwrite(&_Sample_Set[i].dst,    sizeof(unsigned short), 1, fp);
+        fputc(0x2c, fp);
+        fwrite(&_Sample_Set[i].vol,    sizeof(unsigned short), 1, fp);
+        fputc(0x3b, fp);
+    }
+    fclose(fp);
+    free(filepath);
+}
+
+void TRP105FS::loadSetting(
+    const char *filename
+)
+{
+    FILE *fp;
+    char *filepath;
+    char tmp;
+    int fnnum = 0;
+
+    while (filename[fnnum] != 0) fnnum++;
+    filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
+
+    sprintf(filepath, "/local/%s", filename);
+    fp = fopen(filepath, "rb");
+    for(int i = 0; i < _ENUM; i++) {
+
+        fread(&_Set[i].dst,     sizeof(unsigned short), 1, fp);
+        fread(&tmp,             sizeof(char), 1, fp);
+#ifdef DEBUG3
+        g_Serial_Signal.printf("%d%c",  _Set[i].dst, tmp);
+#endif
+
+        fread(&_Set[i].vol,     sizeof(unsigned short), 1, fp);
+        fread(&tmp,             sizeof(char), 1, fp);
+#ifdef DEBUG3
+        g_Serial_Signal.printf("%d%c",  _Set[i].vol, tmp);
+#endif
+
+        fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
+        fread(&tmp,             sizeof(char), 1, fp);
+#ifdef DEBUG3
+        g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
+#endif
+    }
+
+    fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
+    fread(&tmp,         sizeof(char),           1, fp);
+#ifdef DEBUG3
+    g_Serial_Signal.printf("%d%c\n",_Sample_Num,      tmp);
+#endif
+
+    for(int i = 0; i < _Sample_Num; i++) {
+        fread(&_Sample_Set[i].dst,  sizeof(unsigned short), 1, fp);
+        fread(&tmp,                 sizeof(char),1,fp);
+#ifdef DEBUG3
+        g_Serial_Signal.printf("%d%c",  _Sample_Set[i].dst, tmp);
+#endif
+
+        fread(&_Sample_Set[i].vol,  sizeof(unsigned short), 1, fp);
+        fread(&tmp,                 sizeof(char),1,fp);
+#ifdef DEBUG3
+        g_Serial_Signal.printf("%d%c",  _Sample_Set[i].vol, tmp);
+#endif
+    }
+    fclose(fp);
+    free(filepath);
+}
+
+void TRP105FS::printOutData()
+{
+    FILE *fp;
+
+    fp = fopen("/local/log.txt", "w");  // open file in writing mode
+    fprintf(fp, "dst, vol,(threshold)\n");
+    for(int i = 0; i < _ENUM; i++) {
+        fprintf(fp, "%d,%d,(%d)\n", _Set[i].dst, _Set[i].vol, _Threshold[i]);
+    }
+    fprintf(fp, "\nSample:dst, vol\n");
+    for(int i = 0; i < _Sample_Num; i++) {
+        fprintf(fp, "%d,%d\n", _Sample_Set[i].dst, _Sample_Set[i].vol);
+    }
+    fclose(fp);
+
+}
+void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
+{
+    FILE *fp;
+    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
+    fprintf(fp, "%10s\n", name);
+    for(int i = 0; i < num; i++) {
+        fprintf(fp, "%d, ", arg[i]);
+    }
+    fprintf(fp, "\n");
+    fclose(fp);
+}
+void TRP105FS::_printOutData(double *arg, int num, char* name)
+{
+    FILE *fp;
+
+    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
+    fprintf(fp, "%10s\n", name);
+    for(int i = 0; i < num; i++) {
+        fprintf(fp, "%.2f, ", arg[i]);
+    }
+    fprintf(fp, "\n");
+    fclose(fp);
+}
+void TRP105FS::_printOutData(VDset *arg, int num, char* name)
+{
+    FILE *fp;
+
+    fp = fopen("/local/varlog.txt", "a");  // open file in add mode
+    fprintf(fp, "%10s\n", name);
+    for(int i = 0; i < num; i++) {
+        fprintf(fp, "%d, ", arg[i].vol);
+    }
+    fprintf(fp, "\n");
+    fclose(fp);
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TRP105F_Spline.h	Fri Feb 12 11:02:15 2016 +0000
@@ -0,0 +1,100 @@
+/**
+ *  TRP105F_Spline.h,.cpp
+ *
+ *  -   This library is for provide a distance from TRP105F, proximity sensor,
+ *  to whatever reflect the flash.
+ *  -   Deistance is derived from a voltage signal of TRP105F.
+ *  -   An object of TRPFS class has an array of sets of
+ *  distance(16 bit-value) and voltage(16bit-value) at the distance.
+ *  -   The range of distances that an array has in sets is [0:1024] by 1.
+ *  -   an array is derived from a spline curve model.
+ *  -   In order to derive spline curve, some value sets should be got
+ *  at the first calibration.
+ *
+ *  LOG:
+ *  ver.1   2015/10/19 - 2015/10/22
+ *  ver.2   2015/10/22 -
+ *      Distance data type has become unsigned short from int.
+ *      Distance data range : [2:20] by 1 -> [0:1024] by 1.
+ *  ver.2.1 2016/02.12 -
+ *      Distance data range : [0:1024] -> [0:256].
+ */
+#ifndef TRP105F_Spline_H
+#define TRP105F_Spline_H
+
+#include "mbed.h"
+
+
+//  Set of voltage-distance
+typedef struct {
+    unsigned short vol; //  voltage
+    unsigned short dst; //  distance
+} VDset;
+
+//  Type of modality to input data   
+enum DataInType{
+    KEYBORD,    //  set this if input data by 0-9 key
+    SYSTEM      //  set this if any other input method, for instance, like GUI.
+} ;
+
+//
+//  TRP105FS Class for get distance from voltage
+/////////////////////////////////////////////////////////////////////////////////
+////    At calibration, at sampling data, first the class send '>' via seral com, 
+////  after which You can send distancedata [2,20][mm]. Then the class measure
+////  the voltage data for 1 sec.
+////    Above procedure is looped for the number you input in the constructor.
+////------------------------------------------
+////    (when TRP105FS object is made)
+////    >               (< you can input distance data after this signal('>' <=> 0x3e)
+////    >1              (< in this version, distance data is limited[2,20] integer.
+////    >14[CR or LF]   (< you can continue to input unless input ERNTER. But if you set [SYSTEM] on constructor you can input it only once. (use this in GUI)
+////    >14             (< then voltage mesuring begins.
+////                    (< after mesuring voltage, CR or LF signal is sent, which means tt is started a new line if console.
+////    >               (< when you shoud input more data, ':'signal is sent again.
+////////////////////////////////////////////////////////////////////////////////
+class TRP105FS
+{
+public:
+    TRP105FS();
+    TRP105FS(unsigned int);
+    TRP105FS(unsigned int arg_num, DataInType arg_dit);
+    ~TRP105FS();
+    unsigned short getDistance();  //  the fuction to get distance.
+    void    calibrateSensor();
+    void    saveSetting();
+    void    saveSetting(const char *filename);//not usable yet
+    void    loadSetting();
+    void    loadSetting(const char *filename);//not usable yet
+    void    printOutData();
+    void    printThresholds();
+
+private:
+    enum _SetConstant {
+        _LIDX = 0,
+        _RIDX = 1024,
+        _ENUM = _RIDX - _LIDX + 1
+    };
+    int     _Sample_Num;    // the number of samples for derive spline
+    VDset*  _Sample_Set;
+    VDset   _Set[_ENUM];
+    unsigned short _Threshold[_ENUM]; //_Threshold[18] is not used virtually.
+    DataInType  _Data_Input_Type;
+    double*     _u_spline;
+    //
+    //  For calibration
+    //
+    void    _sampleData();
+    void    _makeSpline();  //  Cubic spline
+    unsigned short _getSplineYof(double);
+    //
+    //  For get distance
+    //
+    int     _getNearest(int, int, unsigned short);
+    //
+    //For debug
+    void    _printOutData(unsigned short    *arg, int num, char* name);
+    void    _printOutData(VDset             *arg, int num, char* name);
+    void    _printOutData(double            *arg, int num, char* name);
+};
+#endif
\ No newline at end of file