TRP105F_Spline.cpp

00001 #include "TRP105F_Spline.h"
00002 
00003 //
00004 //  For Platform Variation
00005 //
00006 #if defined(TARGET_LPC1114)
00007 const PinName DEFALT_AI_PIN = dp4;
00008 #elif defined (TARGET_LPC1768)
00009 const PinName DEFALT_AI_PIN = p16;
00010 LocalFileSystem local("local");  // define Mount Point(which becomes Direcory Path)
00011 //  To get sample distance via seral com
00012 Serial      g_Serial_Signal(USBTX, USBRX);
00013 //  To get ytage of TRP105F
00014 //AnalogIn*   g_Sensor_Voltage;
00015 #endif
00016 
00017 
00018 
00019 //
00020 //  For debug
00021 //
00022 //#define DEBUG
00023 //#define DEBUG2
00024 //#define DEBUG3
00025 #ifdef DEBUG
00026 DigitalOut led1(LED1);
00027 DigitalOut led2(LED2);
00028 DigitalOut led3(LED3);
00029 DigitalOut led4(LED4);
00030 #endif
00031 
00032 //  Constructor
00033 TRP105FS::TRP105FS()
00034     :_useType(AsMODULE)
00035     ,_ai(AnalogIn(DEFALT_AI_PIN))
00036 {
00037     _snum = 0;
00038     _Sample_Num = 5;
00039     _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
00040     _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
00041 
00042     for(int i = 0; i < _Sample_Num; i++) {
00043         _Sample_Set[i].x = _Sample_Set[i].y = 0;
00044         _u_spline[i] = 0.0;
00045     }
00046 }
00047 
00048 TRP105FS::TRP105FS(
00049     unsigned short arg_num
00050 )
00051     :_useType(AsMODULE)
00052     ,_ai(AnalogIn(DEFALT_AI_PIN))
00053 {
00054     _snum = 0;
00055     if(arg_num > _ENUM) _Sample_Num = _ENUM;
00056     else _Sample_Num = arg_num;
00057 
00058     _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
00059     _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
00060 
00061     for(int i = 0; i < _Sample_Num; i++) {
00062         _Sample_Set[i].x = _Sample_Set[i].y = 0;
00063         _u_spline[i] = 0.0;
00064     }
00065 }
00066 
00067 TRP105FS::TRP105FS(
00068     unsigned short arg_num,
00069     UseType arg_type
00070 )
00071     :_useType(arg_type)
00072     ,_ai(AnalogIn(DEFALT_AI_PIN))
00073 {
00074     _snum = 0;
00075     if(arg_num > _ENUM) _Sample_Num = _ENUM;
00076     else _Sample_Num = arg_num;
00077 
00078     _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
00079     _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
00080 
00081     for(int i = 0; i < _Sample_Num; i++) {
00082         _Sample_Set[i].x = _Sample_Set[i].y = 0;
00083         _u_spline[i] = 0.0;
00084     }
00085 }
00086 
00087 TRP105FS::TRP105FS(
00088     unsigned short arg_num,
00089     PinName pin
00090 )
00091     :_useType(AsMODULE)
00092     ,_ai(AnalogIn(pin))
00093 
00094 {
00095     _snum = 0;
00096     if(arg_num > _ENUM) _Sample_Num = _ENUM;
00097     else _Sample_Num = arg_num;
00098 
00099     _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
00100     _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
00101 
00102     for(int i = 0; i < _Sample_Num; i++) {
00103         _Sample_Set[i].x = _Sample_Set[i].y = 0;
00104         _u_spline[i] = 0.0;
00105     }
00106 }
00107 
00108 TRP105FS::TRP105FS(
00109     unsigned short arg_num,
00110     UseType arg_type,
00111     PinName pin
00112 )
00113     :_useType(arg_type)
00114     ,_ai(AnalogIn(pin))
00115 {
00116     _snum = 0;
00117     if(arg_num > _ENUM) _Sample_Num = _ENUM;
00118     else _Sample_Num = arg_num;
00119 
00120     _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
00121     _u_spline   = (double*)malloc(_Sample_Num * sizeof(double));
00122 
00123     for(int i = 0; i < _Sample_Num; i++) {
00124         _Sample_Set[i].x = _Sample_Set[i].y = 0;
00125         _u_spline[i] = 0.0;
00126     }
00127 }
00128 
00129 //  Destructor
00130 TRP105FS::~TRP105FS()
00131 {
00132     free(_Sample_Set);
00133     free(_u_spline);
00134     //delete g_Sensor_Voltage;
00135 }
00136 
00137 unsigned short TRP105FS::getSampleNum()
00138 {
00139     return _Sample_Num;
00140 }
00141 
00142 /*
00143 unsigned short getVoltage()   // get voltage
00144 {
00145     return(AnalogIn(pin).read_u16());
00146 }
00147 */
00148 
00149 unsigned short TRP105FS::getX(unsigned short arg_y)
00150 {
00151     int idx;
00152     unsigned short pv = 0;
00153 
00154     pv = arg_y;
00155 
00156     idx = _getNearest(_LIDX, _RIDX, pv);
00157 
00158     if (idx != 0xFFFF)    //  unless occuring error
00159         return _Set[idx].x;
00160     else
00161         return 0xFFFF;
00162 }
00163 
00164 /*
00165     Function to find a set whose y member is nearest a ytage from the sensor, recursively.
00166 
00167                    SHORT                                                          LONG
00168     +------------>  HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
00169     |(if ytage form sensor < threshold[cidx])   |||   (if threshold[cidx] < ytage form sensor)|
00170     |               HIGH[lidx , ... , cidx]LOW    |||    HIGH[cidx+1 , ... , ridx]LOW             |
00171     |                |                                                            |               |
00172     +----------------+                                                            +---------------+
00173 */
00174 int TRP105FS::_getNearest(
00175     int arg_lidx,
00176     int arg_ridx,
00177     unsigned short arg_y
00178 )
00179 {
00180     int cidx = (arg_lidx + arg_ridx) / 2;
00181 
00182     //  When the number of element to compare is only one, return it as result.
00183     if(arg_lidx == arg_ridx)
00184         return cidx;
00185     //  If the ytage from the sensor is lower than the center threshold
00186     //  (_set[cidx] > _threshold[cidx] > _set[cidx+1])
00187     else if(arg_y > _Threshold[cidx])
00188         return _getNearest(arg_lidx, cidx,     arg_y);
00189     //  If the ytage from the sensor is higher than the center threshold
00190     else if(arg_y < _Threshold[cidx])
00191         return _getNearest(cidx + 1, arg_ridx, arg_y);
00192     //  If the ytage from the sensor eauals the center threshold
00193     else //(arg_y == _Treshold[cidx].y)
00194         return cidx;
00195 }
00196 
00197 void TRP105FS::setSample(unsigned short arg_x, unsigned short arg_y)
00198 {
00199     _setSample(arg_x, arg_y);
00200 }
00201 
00202 void TRP105FS::_setSample(unsigned short arg_x, unsigned short arg_y)
00203 {
00204     unsigned int num;
00205     int     tmp;
00206     VDset tmp_set[_ENUM];   // for bucket sort
00207 
00208     // Increment it if this function called.
00209     _snum++;
00210 #ifdef DEBUG
00211     g_Serial_Signal.printf("_snum       : %d\n", _snum);
00212     g_Serial_Signal.printf("(%d,%d)\n",arg_x, arg_y);
00213 #endif
00214 
00215     //  fit to smaller
00216     if (_snum < _Sample_Num) {
00217         num = _snum;
00218     } else {
00219         //  To reclloc memories if _snum is bigger than _Sample_Num.
00220         //  When realloc is failed, _snum is back to porevius.
00221         VDset*  tmp_Set = (VDset *)realloc(_Sample_Set, _snum * sizeof(VDset));
00222         double* tmp__u_ = (double*)realloc(_u_spline,   _snum * sizeof(double));
00223         if (tmp_Set != NULL && tmp__u_ != NULL) {
00224             _Sample_Set = tmp_Set;
00225             _u_spline   = tmp__u_;
00226             num = _Sample_Num = _snum;
00227         } else {
00228             _snum--;
00229             num = _snum = _Sample_Num ;
00230 #ifdef DEBUG
00231             g_Serial_Signal.printf("failed to realloc\n", _snum);
00232 #endif
00233         }
00234     }
00235 
00236     _Sample_Set[num - 1].x = arg_x;
00237     _Sample_Set[num - 1].y = arg_y;
00238     if((unsigned short)_RIDX < _Sample_Set[num - 1].x)
00239         _Sample_Set[num - 1].x = (unsigned short)_RIDX;
00240 
00241     //
00242     //  Sort set data array in distanceAscending order
00243     //
00244     //  Bucket sort
00245     for(int i = 0; i < _ENUM; i++)
00246         tmp_set[i].x = 0xaaaa;
00247     tmp = 0;
00248     for(int i = 0; i < num; i++) {
00249         //  use x as index for x range [_LIDX,_RIDX]
00250         if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
00251             tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
00252             tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
00253         } else { // if a same x has been input, calcurate mean.
00254             tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
00255             tmp_set[_Sample_Set[i].x].y /= 2;
00256             tmp++;
00257         }
00258     }
00259 #ifdef DEBUG
00260     g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
00261     g_Serial_Signal.printf("-)        tmp: %d\n", tmp );
00262 #endif
00263     //  substruct tmp from number of sample.
00264     _Sample_Num -= tmp;
00265     _snum -= tmp;
00266 #ifdef DEBUG
00267     g_Serial_Signal.printf("-----------------\n");
00268     g_Serial_Signal.printf("  _Sample_num: %d\n", _Sample_Num );
00269 #endif
00270     //  apply sort on _Sample_Set
00271     tmp = 0;
00272     for(int i = 0; i < _ENUM; i++) {
00273         if(tmp_set[i].x != 0xaaaa) {
00274             _Sample_Set[i - tmp].x = tmp_set[i].x;
00275             _Sample_Set[i - tmp].y = tmp_set[i].y;
00276         } else //  if no data, skip it
00277             tmp++;
00278     }
00279 }
00280 
00281 
00282 #ifdef TARGET_LPC1768
00283 void TRP105FS::_sampleData()
00284 {
00285     int     tmp;
00286     char    sig;
00287     unsigned short tmp_y;
00288     VDset tmp_set[_ENUM];   // for bucket sort
00289 
00290     //  For evry set,
00291     //  1, get x data via serai com,
00292     //  2, get y data,
00293     //  and then do same for next index set.
00294     for(int i = 0; i < _Sample_Num; i++) {
00295         //
00296         //  Recieve a Distance datus and store it into member
00297         //
00298         if(_useType == AsDEBUG) {
00299             g_Serial_Signal.putc('>');
00300             _Sample_Set[i].x = 0;
00301             do {
00302                 sig = g_Serial_Signal.getc();
00303                 if('0' <= sig && sig <= '9') {
00304                     _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
00305                     g_Serial_Signal.putc(char(sig));
00306                 } else if(sig == 0x08) {
00307                     _Sample_Set[i].x = 0;
00308                     g_Serial_Signal.printf("[canseled!]");
00309                     g_Serial_Signal.putc('\n');
00310                     g_Serial_Signal.putc('>');
00311                 }
00312             } while (!(sig == 0x0a || sig == 0x0d));
00313             g_Serial_Signal.putc('\n');
00314         } else {
00315             _Sample_Set[i].x = g_Serial_Signal.getc();
00316         }
00317 
00318         //  if input data is over the bound calibrate it below
00319         if (_Sample_Set[i].x < (unsigned short)_LIDX)
00320             _Sample_Set[i].x = (unsigned short)_LIDX;
00321         else if ((unsigned short)_RIDX < _Sample_Set[i].x)
00322             _Sample_Set[i].x = (unsigned short)_RIDX;
00323         //
00324         //  Recieve a Voltage datus and store it into member
00325         //
00326         //  LOW PASS FILTERED
00327         //  Get 10 data and store mean as a sample.
00328         //  After get one original sample, system waits for 0.1 sec,
00329         //  thus it takes 1 sec evry sampling.
00330         _Sample_Set[i].y = 0;
00331         for(int j = 0; j < 10; j++) {
00332             //unsigned short 's range [0 , 65535]
00333             //the Number of significant figures of read ytage is 3 or 4.
00334             tmp_y = _ai.read_u16();
00335 
00336 #ifdef DEBUG
00337             g_Serial_Signal.printf("%d,",tmp_y);
00338 #endif
00339             _Sample_Set[i].y += (tmp_y / 10);
00340             wait(0.1);
00341         }
00342 #ifdef DEBUG
00343         g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
00344 #endif
00345     }
00346     //
00347     //  Sort set data array in distanceAscending order
00348     //
00349     //  Bucket sort
00350     for(int i = 0; i < _ENUM; i++)
00351         tmp_set[i].x = 0xaaaa;
00352     tmp = 0;
00353     for(int i = 0; i < _Sample_Num; i++) {
00354         //  use x as index for x range [LIDX,RIDX]
00355         if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
00356             tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
00357             tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
00358         } else { // if a same x has been input, calcurate mean.
00359             tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
00360             tmp_set[_Sample_Set[i].x].y /= 2;
00361             tmp++;
00362         }
00363     }
00364 #ifdef DEBUG
00365     g_Serial_Signal.printf("%d\n", _Sample_Num );
00366 #endif
00367     //  substruct tmp from number of sample.
00368     _Sample_Num -= tmp;
00369 
00370 #ifdef DEBUG
00371     g_Serial_Signal.printf("tmp: %d\n", tmp );
00372 #endif
00373     //  apply sort on _Sample_Set
00374     tmp = 0;
00375     for(int i = 0; i < _ENUM; i++) {
00376         if(tmp_set[i].x != 0xaaaa) {
00377             _Sample_Set[i - tmp].x = tmp_set[i].x;
00378             _Sample_Set[i - tmp].y = tmp_set[i].y;
00379         } else //  if no data, skip it
00380             tmp++;
00381     }
00382 }
00383 #endif
00384 
00385 //
00386 //  Function to define _u_spline, specific constants of spline.
00387 //
00388 void TRP105FS::_makeSpline()
00389 {
00390     //  x: x, distance
00391     //  y: y, ytage
00392     //
00393     //  N: max of index <=> (_Sample_Num - 1)
00394     //
00395     //  _u_spline[i] === d^2/dx^2(Spline f)[i]
00396     //  i:[0,N]
00397     //  _u_spline[0] = _u_spline[N] = 0
00398     //
00399     //  h[i] = x[i+1] - x[i]
00400     //  i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
00401     double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
00402     //unsigned short *h __attribute__((at(0x20080000)));
00403     //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
00404     //  v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
00405     //  i:[0,N-2]
00406     double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
00407     //unsigned short *v __attribute__((at(0x20080100)));
00408     //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
00409     //  temporary array whose num of elm equals v array
00410     double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
00411     //unsigned short *w __attribute__((at(0x20080200)));
00412     //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
00413     //
00414     //  [ 2(h[0]+h[1])  , h[1]          ,                                 O                 ]   [_u[1]  ]   [v[0]  ]
00415     //  [ h[1]          , 2(h[1]+h[2])  , h[2]                                              ]   [_u[2]  ]   [v[1]  ]
00416     //  [                       ...                                                         ] * [...    ] = [...   ]
00417     //  [                   h[j]          , 2(h[j]+h[j+1])  , h[j+1]                        ]   [_u[j+1]]   [v[j]  ]
00418     //  [                                   ...                                             ]   [ ...   ]   [ ...  ]
00419     //  [                               h[N-3]        , 2(h[N-3]+h[N-2]), h[N-2]            ]   [_u[j+1]]   [v[j]  ]
00420     //  [       O                                       h[N-2]          , 2(h[N-2]+h[N-1])  ]   [_u[N-1]]   [v[N-2]]
00421     //
00422     // For LU decomposition
00423     double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
00424     //unsigned short *Upper __attribute__((at(0x20080300)));
00425     //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
00426     double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
00427     //unsigned short *Lower __attribute__((at(0x20080400)));
00428     //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
00429 #ifdef DEBUG
00430     _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
00431 #endif
00432     for(int i = 0; i < _Sample_Num - 1; i++)
00433         h[i] =  (double)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
00434     //(unsigned short)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
00435 
00436     for(int i = 0; i < _Sample_Num - 2; i++)
00437         v[i] = 6 * (
00438                    ((double)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y)) / h[i + 1]
00439                    //(unsigned short)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y) / h[i + 1]
00440                    -
00441                    ((double)(_Sample_Set[i + 1].y - _Sample_Set[i].y))     / h[i]
00442                    //(unsigned short)(_Sample_Set[i + 1].y - _Sample_Set[i].y) / h[i]
00443                );
00444 
00445     //
00446     //  LU decomposition
00447     //
00448     Upper[0] = 2 * (h[0] + h[1]);
00449     Lower[0] = 0;
00450     for (int i = 1; i < _Sample_Num - 2; i++) {
00451         Lower[i] = h[i] / Upper[i - 1];
00452         Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
00453     }
00454 
00455 
00456     //
00457     //  forward substitution
00458     //
00459     w[0] = v[0];
00460     for (int i = 1; i < _Sample_Num - 2; i ++) {
00461         w[i] = v[i] - Lower[i] * w[i-1];
00462     }
00463 
00464 
00465     //
00466     //  backward substitution
00467     //
00468     _u_spline[_Sample_Num - 2] =  w[_Sample_Num - 3]                         / Upper[_Sample_Num - 3];
00469     for(int i = _Sample_Num - 3; i > 0; i--) {
00470         _u_spline[i]           = (w[(i - 1)] -  h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
00471     }
00472 
00473     // _u_spline[i] === d^2/dx^2(Spline f)[i]
00474     _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
00475 
00476 #ifdef DEBUG
00477     _printOutData(h, _Sample_Num - 1, "h");
00478     _printOutData(v, _Sample_Num - 2, "v");
00479     _printOutData(w, _Sample_Num - 2, "w");
00480     _printOutData(Upper, _Sample_Num - 2, "Upper");
00481     _printOutData(Lower, _Sample_Num - 2, "Lower");
00482     _printOutData(_u_spline, _Sample_Num, "u");
00483 #endif
00484     free(h);
00485     free(v);
00486     free(w);
00487     free(Upper);
00488     free(Lower);
00489 }
00490 //
00491 //  Function to return Voltage for distance.
00492 //
00493 unsigned short TRP105FS:: _getSplineYof(
00494     double arg_x       //  the argument is supposed as distance [mm]
00495 )
00496 {
00497     double y;       //  ytage calculated by spline polynomial
00498     double a,b,c,d; //  which is specific constant of spline, and can be expressed with _u.
00499     int itv = 0;    //  interval(section) of interpolation
00500     //  the number of interval is less 1 than the number of sample sets,
00501     //  which means the max number of interval is _Sample_num - 2.
00502     if((double)(_Sample_Set[0].x) <= arg_x) {
00503         while (!((double)(_Sample_Set[itv].x) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].x))) {
00504             itv++;
00505             if(itv > _Sample_Num - 2) {
00506                 itv = _Sample_Num - 2;
00507                 break;
00508             }
00509         }
00510     }
00511     a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x);
00512     b = (double)(_u_spline[itv]) / 2.0;
00513     c = (double)(_Sample_Set[itv + 1].y - _Sample_Set[itv].y) / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x)
00514         -
00515         (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
00516     d = (double)(_Sample_Set[itv].y);
00517     //  cubic spline expression
00518     y = a * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
00519         +
00520         b * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
00521         +
00522         c * (arg_x - (double)(_Sample_Set[itv].x))
00523         +
00524         d;
00525 
00526 #ifdef DEBUG2
00527     g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
00528     g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
00529     g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
00530 #endif
00531 
00532     return ((unsigned short)(int)y);
00533 }
00534 
00535 #if defined(HAS_COM_TO_CONSOLE)
00536 void TRP105FS::calibrateSensor()
00537 {
00538     _sampleData();
00539     _makeSpline();
00540 
00541     for(int i = 0; i < _ENUM; i++) {
00542         _Set[i].x     = i;
00543         _Set[i].y     = _getSplineYof((double)(_Set[i].x));
00544         _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
00545 #ifdef DEBUG2
00546         g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
00547 #endif
00548     }
00549 }
00550 #endif
00551 
00552 void TRP105FS::calibrate()
00553 {
00554 #ifdef DEBUG2
00555     g_Serial_Signal.printf("Sample Data Set\n");
00556     for(int i = 0; i < _Sample_Num; i++)
00557         g_Serial_Signal.printf("(%d,%d)\n", _Sample_Set[i].x, _Sample_Set[i].y);
00558 #endif
00559 
00560     _makeSpline();
00561 
00562     for(int i = 0; i < _ENUM; i++) {
00563         _Set[i].x     = i;
00564         _Set[i].y     = _getSplineYof((double)(_Set[i].x));
00565         _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
00566 
00567 #ifdef DEBUG2
00568         g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
00569 #endif
00570     }
00571 }
00572 
00573 
00574 #ifdef HAS_COM_TO_CONSOLE
00575 void TRP105FS::printThresholds()
00576 {
00577     for(int i = 0; i < _ENUM; i++)
00578         g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
00579 }
00580 #endif
00581 
00582 
00583 #ifdef HAS_LOCAL_FILE_SYSTEM
00584 void TRP105FS::printOutData(
00585     const char *filename)
00586 {
00587     FILE *fp;
00588     char *filepath;
00589     int fnnum = 0;
00590 
00591     while (filename[fnnum] != 0) fnnum++;
00592     filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
00593     sprintf(filepath, "/local/%s", filename);
00594     fp = fopen(filepath, "w");
00595     //fp = fopen("/local/log.txt", "w");  // open file in writing mode
00596 
00597     fprintf(fp, "x, y,(threshold)\n");
00598     for(int i = 0; i < _ENUM; i++) {
00599         fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]);
00600     }
00601     fprintf(fp, "\nSample:x, y\n");
00602     for(int i = 0; i < _Sample_Num; i++) {
00603         fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y);
00604     }
00605 
00606     free(filepath);
00607     fclose(fp);
00608 
00609 }
00610 
00611 void TRP105FS::saveSetting()
00612 {
00613     FILE *fp;
00614 
00615     fp = fopen("/local/savedata.log", "wb");
00616 
00617     for(int i = 0; i < _ENUM; i++) {
00618         fwrite(&_Set[i].x,    sizeof(unsigned short), 1, fp);
00619         fputc(0x2c, fp);
00620         fwrite(&_Set[i].y,    sizeof(unsigned short), 1, fp);
00621         fputc(0x2c, fp);
00622         fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
00623         fputc(0x3b, fp);
00624     }
00625     fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
00626     fputc(0x3b, fp);
00627     for(int i = 0; i < _Sample_Num; i++) {
00628         fwrite(&_Sample_Set[i].x,    sizeof(unsigned short), 1, fp);
00629         fputc(0x2c, fp);
00630         fwrite(&_Sample_Set[i].y,    sizeof(unsigned short), 1, fp);
00631         fputc(0x3b, fp);
00632     }
00633     fclose(fp);
00634 
00635 }
00636 
00637 void TRP105FS::loadSetting()
00638 {
00639     FILE *fp;
00640     char tmp;
00641 
00642     //sprintf(filepath, "/local/%s", filename);
00643     //fp = fopen(filepath, "rb");
00644     fp = fopen("/local/savedata.log", "rb");
00645     for(int i = 0; i < _ENUM; i++) {
00646 
00647         fread(&_Set[i].x,     sizeof(unsigned short), 1, fp);
00648         fread(&tmp,             sizeof(char),           1, fp);
00649 #ifdef DEBUG2
00650         g_Serial_Signal.printf("%d%c",  _Set[i].x, tmp);
00651 #endif
00652 
00653         fread(&_Set[i].y,     sizeof(unsigned short), 1, fp);
00654         fread(&tmp,             sizeof(char), 1, fp);
00655 #ifdef DEBUG2
00656         g_Serial_Signal.printf("%d%c",  _Set[i].y, tmp);
00657 #endif
00658 
00659         fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
00660         fread(&tmp,             sizeof(char), 1, fp);
00661 #ifdef DEBUG2
00662         g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
00663 #endif
00664     }
00665 
00666     fread(&_Sample_Num, sizeof(unsigned short),  1, fp);
00667     fread(&tmp,         sizeof(char), 1, fp);
00668 
00669     for(int i = 0; i < _Sample_Num; i++) {
00670         fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
00671         fread(&tmp, sizeof(char),1,fp);
00672         fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
00673         fread(&tmp, sizeof(char),1,fp);
00674     }
00675     fclose(fp);
00676 }
00677 
00678 
00679 void TRP105FS::saveSetting(
00680     const char *filename
00681 )
00682 {
00683     FILE *fp;
00684     char *filepath;
00685     int fnnum = 0;
00686 
00687     while (filename[fnnum] != 0) fnnum++;
00688     filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
00689 
00690     sprintf(filepath, "/local/%s", filename);
00691     fp = fopen(filepath, "wb");
00692 
00693     for(int i = 0; i < _ENUM; i++) {
00694         fwrite(&_Set[i].x,    sizeof(unsigned short), 1, fp);
00695         fputc(0x2c, fp);
00696         fwrite(&_Set[i].y,    sizeof(unsigned short), 1, fp);
00697         fputc(0x2c, fp);
00698         fwrite(&_Threshold[i],  sizeof(unsigned short), 1, fp);
00699         fputc(0x3b, fp);
00700     }
00701     fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
00702     fputc(0x3b, fp);
00703     for(int i = 0; i < _Sample_Num; i++) {
00704         fwrite(&_Sample_Set[i].x,    sizeof(unsigned short), 1, fp);
00705         fputc(0x2c, fp);
00706         fwrite(&_Sample_Set[i].y,    sizeof(unsigned short), 1, fp);
00707         fputc(0x3b, fp);
00708     }
00709     fclose(fp);
00710     free(filepath);
00711 }
00712 
00713 void TRP105FS::loadSetting(
00714     const char *filename
00715 )
00716 {
00717     FILE *fp;
00718     char *filepath;
00719     char tmp;
00720     int fnnum = 0;
00721 
00722     while (filename[fnnum] != 0) fnnum++;
00723     filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
00724 
00725     sprintf(filepath, "/local/%s", filename);
00726     fp = fopen(filepath, "rb");
00727 
00728     for(int i = 0; i < _ENUM; i++) {
00729 
00730         fread(&_Set[i].x,     sizeof(unsigned short), 1, fp);
00731         fread(&tmp,             sizeof(char), 1, fp);
00732 #ifdef DEBUG3
00733         g_Serial_Signal.printf("%d%c",  _Set[i].x, tmp);
00734 #endif
00735 
00736         fread(&_Set[i].y,     sizeof(unsigned short), 1, fp);
00737         fread(&tmp,             sizeof(char), 1, fp);
00738 #ifdef DEBUG3
00739         g_Serial_Signal.printf("%d%c",  _Set[i].y, tmp);
00740 #endif
00741 
00742         fread(&_Threshold[i],   sizeof(unsigned short), 1, fp);
00743         fread(&tmp,             sizeof(char), 1, fp);
00744 #ifdef DEBUG3
00745         g_Serial_Signal.printf("%d%c\n",_Threshold[i],      tmp);
00746 #endif
00747     }
00748 
00749     fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
00750     fread(&tmp,         sizeof(char),           1, fp);
00751 #ifdef DEBUG3
00752     g_Serial_Signal.printf("%d%c\n",_Sample_Num,      tmp);
00753 #endif
00754 
00755     for(int i = 0; i < _Sample_Num; i++) {
00756         fread(&_Sample_Set[i].x,  sizeof(unsigned short), 1, fp);
00757         fread(&tmp,                 sizeof(char),1,fp);
00758 #ifdef DEBUG3
00759         g_Serial_Signal.printf("%d%c",  _Sample_Set[i].x, tmp);
00760 #endif
00761 
00762         fread(&_Sample_Set[i].y,  sizeof(unsigned short), 1, fp);
00763         fread(&tmp,                 sizeof(char),1,fp);
00764 #ifdef DEBUG3
00765         g_Serial_Signal.printf("%d%c",  _Sample_Set[i].y, tmp);
00766 #endif
00767     }
00768     fclose(fp);
00769     free(filepath);
00770 }
00771 #endif// HAS_LOCAL_FILE_SYSTEM
00772 
00773 
00774 void TRP105FS::saveSetting_intoSerial(
00775     Serial* com
00776 )
00777 {
00778     char buffer[3];
00779 
00780     for(int i = 0; i < _ENUM; i++) {
00781         //_Set.X
00782         buffer[0] = 0xFF & (_Set[i].x >> 8);
00783         buffer[1] = 0xFF & (_Set[i].x);
00784         buffer[2] = 0x2c;
00785         for(int j = 0; j < 3; j++) com->putc(buffer[j]);
00786         while(1) {
00787             if(com->getc() == 0x06) break;    //wait ACK
00788         }
00789 
00790         //Set Y
00791         buffer[0] = 0xFF & (_Set[i].y >> 8);
00792         buffer[1] = 0xFF & (_Set[i].y);
00793         buffer[2] = 0x2c;
00794         for(int j = 0; j < 3; j++) com->putc(buffer[j]);
00795         while(1) {
00796             if(com->getc() == 0x06) break;    //wait ACK
00797         }
00798 
00799         //Threshold
00800         buffer[0] = 0xFF & (_Threshold[i] >> 8);
00801         buffer[1] = 0xFF & (_Threshold[i]);
00802         buffer[2] = 0x3b;
00803         for(int j = 0; j < 3; j++) com->putc(buffer[j]);
00804         while(1) {
00805             if(com->getc() == 0x06) break;    //wait ACK
00806         }
00807     }
00808 
00809     //Sample Num
00810     buffer[0] = 0xFF & (_Sample_Num >> 8);
00811     buffer[1] = 0xFF & (_Sample_Num);
00812     buffer[2] = 0x3b;
00813     for(int j = 0; j < 3; j++) com->putc(buffer[j]);
00814     while(1) {
00815         if(com->getc() == 0x06) break;    //wait ACK
00816     }
00817 
00818     for(int i = 0; i < _Sample_Num; i++) {
00819         //Sample Set X
00820         buffer[0] = 0xFF & (_Sample_Set[i].x >> 8);
00821         buffer[1] = 0xFF & (_Sample_Set[i].x);
00822         buffer[2] = 0x2c;
00823         for(int j = 0; j < 3; j++) com->putc(buffer[j]);
00824         while(1) {
00825             if(com->getc() == 0x06) break;    //wait ACK
00826         }
00827 
00828         //Sample Set Y
00829         buffer[0] = 0xFF & (_Sample_Set[i].x >> 8);
00830         buffer[1] = 0xFF & (_Sample_Set[i].x);
00831         buffer[2] = 0x2c;
00832         for(int j = 0; j < 3; j++) com->putc(buffer[j]);
00833         while(1) {
00834             if(com->getc() == 0x06) break;    //wait ACK
00835         }
00836     }
00837     com->putc(0x04);    //send End of Transmission
00838 }
00839 
00840 void TRP105FS::loadSetting_fromSerial(
00841     Serial* com
00842 )
00843 {
00844     char buffer[3];
00845 
00846 
00847     for(int i = 0; i < _ENUM; i++) {
00848         //_Set.X
00849         for(int j = 0; j < 3; j++) {
00850             buffer[j] = com->getc();
00851         }
00852         com->putc(0x06);        //return ACK
00853         _Set[i].x       = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
00854 
00855         //_Set/Y
00856         for(int j = 0; j < 3; j++) {
00857             buffer[j] = com->getc();
00858         }
00859         com->putc(0x06);        //return ACK
00860         _Set[i].y       = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
00861 
00862         //_Threshold
00863         for(int j = 0; j < 3; j++) {
00864             buffer[j] = com->getc();
00865         }
00866         com->putc(0x06);        //return ACK
00867         _Threshold[i]   = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
00868     }
00869 
00870     //_Sample_Num
00871     for(int j = 0; j < 3; j++) {
00872         buffer[j] = com->getc();
00873     }
00874     com->putc(0x06);        //return ACK
00875     _Sample_Num         = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
00876 
00877     for(int i = 0; i < _Sample_Num; i++) {
00878         //Sample Set X
00879         for(int j = 0; j < 3; j++) {
00880             buffer[j] = com->getc();
00881         }
00882         com->putc(0x06);        //return ACK
00883         _Sample_Set[i].x= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
00884 
00885         //Sample Set Y
00886         for(int j = 0; j < 3; j++) {
00887             buffer[j] = com->getc();
00888         }
00889         if(i != _Sample_Num - 1)
00890             com->putc(0x06);        //return ACK
00891         else
00892             com->putc(0x04);        //return EOT
00893         _Sample_Set[i].y= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
00894     }
00895 }
00896 
00897 
00898 #ifdef HAS_LOCAL_FILE_SYSTEM
00899 void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
00900 {
00901     FILE *fp;
00902     fp = fopen("/local/varlog.txt", "a");  // open file in add mode
00903     fprintf(fp, "%10s\n", name);
00904     for(int i = 0; i < num; i++) {
00905         fprintf(fp, "%d, ", arg[i]);
00906     }
00907     fprintf(fp, "\n");
00908     fclose(fp);
00909 }
00910 void TRP105FS::_printOutData(double *arg, int num, char* name)
00911 {
00912     FILE *fp;
00913 
00914     fp = fopen("/local/varlog.txt", "a");  // open file in add mode
00915     fprintf(fp, "%10s\n", name);
00916     for(int i = 0; i < num; i++) {
00917         fprintf(fp, "%.2f, ", arg[i]);
00918     }
00919     fprintf(fp, "\n");
00920     fclose(fp);
00921 }
00922 void TRP105FS::_printOutData(VDset *arg, int num, char* name)
00923 {
00924     FILE *fp;
00925 
00926     fp = fopen("/local/varlog.txt", "a");  // open file in add mode
00927     fprintf(fp, "%10s\n", name);
00928     for(int i = 0; i < num; i++) {
00929         fprintf(fp, "%d, ", arg[i].y);
00930     }
00931     fprintf(fp, "\n");
00932     fclose(fp);
00933 }
00934 #endif// HAS_LOCAL_FILE_SYSTEM