Kenta Tanabe / TRP105F_Spline_for_child

for child

Fork of TRP105F_Spline by Akifumi Takahashi

TRP105F_Spline.cpp@13:16fc61b41eff, 2016-06-07 (annotated)

Committer:
aktk
Date:
Tue Jun 07 16:23:19 2016 +0000
Revision:
13:16fc61b41eff
Parent:
12:db5110d9d494
Child:
14:a99bf22b919d
type of _Sample_Num was modified: int -> unsigned short.; in saveSetting(any), size of _Sample_num to write into savedata file was modified: int -> unsigned short.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
aktk 0:e85788b14028 1 #include "TRP105F_Spline.h"
aktk 0:e85788b14028 2
aktk 11:d60fb729eacf 3 //
aktk 11:d60fb729eacf 4 // For Platform Variation
aktk 11:d60fb729eacf 5 //
aktk 11:d60fb729eacf 6 #if defined(TARGET_LPC1114)
aktk 11:d60fb729eacf 7 const PinName DEFALT_AI_PIN = dp4;
aktk 11:d60fb729eacf 8 #elif defined (TARGET_LPC1768)
aktk 11:d60fb729eacf 9 const PinName DEFALT_AI_PIN = p16;
aktk 11:d60fb729eacf 10 LocalFileSystem local("local"); // define Mount Point(which becomes Direcory Path)
aktk 11:d60fb729eacf 11 // To get sample distance via seral com
aktk 11:d60fb729eacf 12 Serial g_Serial_Signal(USBTX, USBRX);
aktk 3:b56e933bebc2 13 // To get ytage of TRP105F
aktk 9:ec1ee4a6b6a4 14 //AnalogIn* g_Sensor_Voltage;
aktk 11:d60fb729eacf 15 #endif
aktk 11:d60fb729eacf 16
aktk 11:d60fb729eacf 17
aktk 0:e85788b14028 18
aktk 11:d60fb729eacf 19 //
aktk 11:d60fb729eacf 20 // For debug
aktk 11:d60fb729eacf 21 //
aktk 11:d60fb729eacf 22 //#define DEBUG
aktk 11:d60fb729eacf 23 //#define DEBUG2
aktk 0:e85788b14028 24 #ifdef DEBUG
aktk 0:e85788b14028 25 DigitalOut led1(LED1);
aktk 0:e85788b14028 26 DigitalOut led2(LED2);
aktk 0:e85788b14028 27 DigitalOut led3(LED3);
aktk 0:e85788b14028 28 DigitalOut led4(LED4);
aktk 0:e85788b14028 29 #endif
aktk 0:e85788b14028 30
aktk 3:b56e933bebc2 31 // Constructor
aktk 0:e85788b14028 32 TRP105FS::TRP105FS()
aktk 3:b56e933bebc2 33 :_useType(AsMODULE)
aktk 9:ec1ee4a6b6a4 34 ,_ai(AnalogIn(DEFALT_AI_PIN))
aktk 0:e85788b14028 35 {
aktk 0:e85788b14028 36 _Sample_Num = 5;
aktk 0:e85788b14028 37 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 38 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 39
aktk 4:701f958d137a 40 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 41 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 42 _u_spline[i] = 0.0;
aktk 4:701f958d137a 43 }
aktk 0:e85788b14028 44 }
aktk 0:e85788b14028 45
aktk 0:e85788b14028 46 TRP105FS::TRP105FS(
aktk 0:e85788b14028 47 unsigned int arg_num
aktk 0:e85788b14028 48 )
aktk 3:b56e933bebc2 49 :_useType(AsMODULE)
aktk 9:ec1ee4a6b6a4 50 ,_ai(AnalogIn(DEFALT_AI_PIN))
aktk 3:b56e933bebc2 51 {
aktk 3:b56e933bebc2 52 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 3:b56e933bebc2 53 else _Sample_Num = arg_num;
aktk 3:b56e933bebc2 54
aktk 3:b56e933bebc2 55 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 3:b56e933bebc2 56 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 57
aktk 4:701f958d137a 58 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 59 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 60 _u_spline[i] = 0.0;
aktk 4:701f958d137a 61 }
aktk 3:b56e933bebc2 62 }
aktk 3:b56e933bebc2 63
aktk 3:b56e933bebc2 64 TRP105FS::TRP105FS(
aktk 3:b56e933bebc2 65 unsigned int arg_num,
aktk 3:b56e933bebc2 66 UseType arg_type
aktk 3:b56e933bebc2 67 )
aktk 3:b56e933bebc2 68 :_useType(arg_type)
aktk 9:ec1ee4a6b6a4 69 ,_ai(AnalogIn(DEFALT_AI_PIN))
aktk 0:e85788b14028 70 {
aktk 0:e85788b14028 71 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 72 else _Sample_Num = arg_num;
aktk 0:e85788b14028 73
aktk 0:e85788b14028 74 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 75 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 76
aktk 4:701f958d137a 77 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 78 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 79 _u_spline[i] = 0.0;
aktk 4:701f958d137a 80 }
aktk 0:e85788b14028 81 }
aktk 0:e85788b14028 82
aktk 0:e85788b14028 83 TRP105FS::TRP105FS(
aktk 0:e85788b14028 84 unsigned int arg_num,
aktk 10:b50e4bb40571 85 PinName pin
aktk 10:b50e4bb40571 86 )
aktk 10:b50e4bb40571 87 :_ai(AnalogIn(pin))
aktk 10:b50e4bb40571 88
aktk 10:b50e4bb40571 89 {
aktk 10:b50e4bb40571 90 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 10:b50e4bb40571 91 else _Sample_Num = arg_num;
aktk 10:b50e4bb40571 92
aktk 10:b50e4bb40571 93 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 10:b50e4bb40571 94 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 10:b50e4bb40571 95
aktk 10:b50e4bb40571 96 for(int i = 0; i < _Sample_Num; i++) {
aktk 10:b50e4bb40571 97 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 10:b50e4bb40571 98 _u_spline[i] = 0.0;
aktk 10:b50e4bb40571 99 }
aktk 10:b50e4bb40571 100 }
aktk 10:b50e4bb40571 101
aktk 10:b50e4bb40571 102 TRP105FS::TRP105FS(
aktk 10:b50e4bb40571 103 unsigned int arg_num,
aktk 3:b56e933bebc2 104 UseType arg_type,
aktk 3:b56e933bebc2 105 PinName pin
aktk 0:e85788b14028 106 )
aktk 3:b56e933bebc2 107 :_useType(arg_type)
aktk 9:ec1ee4a6b6a4 108 ,_ai(AnalogIn(pin))
aktk 0:e85788b14028 109 {
aktk 0:e85788b14028 110 if(arg_num > _ENUM) _Sample_Num = _ENUM;
aktk 0:e85788b14028 111 else _Sample_Num = arg_num;
aktk 0:e85788b14028 112
aktk 0:e85788b14028 113 _Sample_Set = (VDset *)malloc(_Sample_Num * sizeof(VDset));
aktk 0:e85788b14028 114 _u_spline = (double*)malloc(_Sample_Num * sizeof(double));
aktk 4:701f958d137a 115
aktk 4:701f958d137a 116 for(int i = 0; i < _Sample_Num; i++) {
aktk 4:701f958d137a 117 _Sample_Set[i].x = _Sample_Set[i].y = 0;
aktk 4:701f958d137a 118 _u_spline[i] = 0.0;
aktk 4:701f958d137a 119 }
aktk 0:e85788b14028 120 }
aktk 0:e85788b14028 121
aktk 3:b56e933bebc2 122 // Destructor
aktk 0:e85788b14028 123 TRP105FS::~TRP105FS()
aktk 0:e85788b14028 124 {
aktk 0:e85788b14028 125 free(_Sample_Set);
aktk 0:e85788b14028 126 free(_u_spline);
aktk 9:ec1ee4a6b6a4 127 //delete g_Sensor_Voltage;
aktk 0:e85788b14028 128 }
aktk 0:e85788b14028 129
aktk 3:b56e933bebc2 130
aktk 3:b56e933bebc2 131 unsigned short TRP105FS::getX(unsigned short arg_y)
aktk 0:e85788b14028 132 {
aktk 0:e85788b14028 133 int idx;
aktk 0:e85788b14028 134 unsigned short pv = 0;
aktk 0:e85788b14028 135
aktk 3:b56e933bebc2 136 pv = arg_y;
aktk 0:e85788b14028 137
aktk 0:e85788b14028 138 idx = _getNearest(_LIDX, _RIDX, pv);
aktk 0:e85788b14028 139
aktk 0:e85788b14028 140 if (idx != 0xFFFF) // unless occuring error
aktk 3:b56e933bebc2 141 return _Set[idx].x;
aktk 0:e85788b14028 142 else
aktk 0:e85788b14028 143 return 0xFFFF;
aktk 0:e85788b14028 144 }
aktk 0:e85788b14028 145
aktk 3:b56e933bebc2 146 unsigned short TRP105FS::getDistance(unsigned short arg_y)
aktk 3:b56e933bebc2 147 {
aktk 3:b56e933bebc2 148 return getX(arg_y);
aktk 3:b56e933bebc2 149 }
aktk 3:b56e933bebc2 150
aktk 3:b56e933bebc2 151 unsigned short TRP105FS::getDistance()
aktk 3:b56e933bebc2 152 {
aktk 9:ec1ee4a6b6a4 153 return getX(_ai.read_u16());
aktk 3:b56e933bebc2 154 }
aktk 3:b56e933bebc2 155
aktk 0:e85788b14028 156 /*
aktk 3:b56e933bebc2 157 Function to find a set whose y member is nearest a ytage from the sensor, recursively.
aktk 0:e85788b14028 158
aktk 0:e85788b14028 159 SHORT LONG
aktk 0:e85788b14028 160 +------------> HIGH[lidx , ... , cidx , threshold[cidx], cidx+1 , ... , ridx]LOW <-----------+
aktk 3:b56e933bebc2 161 |(if ytage form sensor < threshold[cidx]) ||| (if threshold[cidx] < ytage form sensor)|
aktk 0:e85788b14028 162 | HIGH[lidx , ... , cidx]LOW ||| HIGH[cidx+1 , ... , ridx]LOW |
aktk 0:e85788b14028 163 | | | |
aktk 0:e85788b14028 164 +----------------+ +---------------+
aktk 0:e85788b14028 165 */
aktk 0:e85788b14028 166 int TRP105FS::_getNearest(
aktk 0:e85788b14028 167 int arg_lidx,
aktk 0:e85788b14028 168 int arg_ridx,
aktk 3:b56e933bebc2 169 unsigned short arg_y
aktk 0:e85788b14028 170 )
aktk 0:e85788b14028 171 {
aktk 0:e85788b14028 172 int cidx = (arg_lidx + arg_ridx) / 2;
aktk 0:e85788b14028 173
aktk 0:e85788b14028 174 // When the number of element to compare is only one, return it as result.
aktk 0:e85788b14028 175 if(arg_lidx == arg_ridx)
aktk 0:e85788b14028 176 return cidx;
aktk 3:b56e933bebc2 177 // If the ytage from the sensor is lower than the center threshold
aktk 0:e85788b14028 178 // (_set[cidx] > _threshold[cidx] > _set[cidx+1])
aktk 3:b56e933bebc2 179 else if(arg_y > _Threshold[cidx])
aktk 3:b56e933bebc2 180 return _getNearest(arg_lidx, cidx, arg_y);
aktk 3:b56e933bebc2 181 // If the ytage from the sensor is higher than the center threshold
aktk 3:b56e933bebc2 182 else if(arg_y < _Threshold[cidx])
aktk 3:b56e933bebc2 183 return _getNearest(cidx + 1, arg_ridx, arg_y);
aktk 3:b56e933bebc2 184 // If the ytage from the sensor eauals the center threshold
aktk 3:b56e933bebc2 185 else //(arg_y == _Treshold[cidx].y)
aktk 0:e85788b14028 186 return cidx;
aktk 0:e85788b14028 187 }
aktk 0:e85788b14028 188
aktk 4:701f958d137a 189 void TRP105FS::setSample(unsigned short arg_x, unsigned short arg_y)
aktk 4:701f958d137a 190 {
aktk 4:701f958d137a 191 _setSample(arg_x, arg_y);
aktk 3:b56e933bebc2 192 }
aktk 3:b56e933bebc2 193
aktk 3:b56e933bebc2 194 void TRP105FS::_setSample(unsigned short arg_x, unsigned short arg_y)
aktk 3:b56e933bebc2 195 {
aktk 3:b56e933bebc2 196 static unsigned int snum = 0;
aktk 3:b56e933bebc2 197 unsigned int num;
aktk 3:b56e933bebc2 198 int tmp;
aktk 3:b56e933bebc2 199 VDset tmp_set[_ENUM]; // for bucket sort
aktk 3:b56e933bebc2 200
aktk 4:701f958d137a 201 // Increment it if this function called.
aktk 3:b56e933bebc2 202 snum++;
aktk 4:701f958d137a 203 #ifdef DEBUG
aktk 4:701f958d137a 204 g_Serial_Signal.printf("snum : %d\n", snum);
aktk 4:701f958d137a 205 g_Serial_Signal.printf("(%d,%d)\n",arg_x, arg_y);
aktk 4:701f958d137a 206 #endif
aktk 3:b56e933bebc2 207
aktk 3:b56e933bebc2 208 // fit to smaller
aktk 4:701f958d137a 209 if (snum < _Sample_Num) {
aktk 4:701f958d137a 210 num = snum;
aktk 3:b56e933bebc2 211 } else {
aktk 3:b56e933bebc2 212 // To reclloc memories if snum is bigger than _Sample_Num.
aktk 3:b56e933bebc2 213 // When realloc is failed, snum is back to porevius.
aktk 3:b56e933bebc2 214 VDset* tmp_Set = (VDset *)realloc(_Sample_Set, snum * sizeof(VDset));
aktk 3:b56e933bebc2 215 double* tmp__u_ = (double*)realloc(_u_spline, snum * sizeof(double));
aktk 3:b56e933bebc2 216 if (tmp_set != NULL && tmp__u_ != NULL) {
aktk 3:b56e933bebc2 217 _Sample_Set = tmp_Set;
aktk 3:b56e933bebc2 218 _u_spline = tmp__u_;
aktk 3:b56e933bebc2 219 num = _Sample_Num = snum;
aktk 3:b56e933bebc2 220 } else {
aktk 3:b56e933bebc2 221 snum--;
aktk 3:b56e933bebc2 222 num = snum = _Sample_Num ;
aktk 4:701f958d137a 223 #ifdef DEBUG
aktk 4:701f958d137a 224 g_Serial_Signal.printf("failed to realloc\n", snum);
aktk 4:701f958d137a 225 #endif
aktk 3:b56e933bebc2 226 }
aktk 3:b56e933bebc2 227 }
aktk 3:b56e933bebc2 228
aktk 3:b56e933bebc2 229 _Sample_Set[num - 1].x = arg_x;
aktk 3:b56e933bebc2 230 _Sample_Set[num - 1].y = arg_y;
aktk 4:701f958d137a 231 if((unsigned short)_RIDX < _Sample_Set[num - 1].x)
aktk 4:701f958d137a 232 _Sample_Set[num - 1].x = (unsigned short)_RIDX;
aktk 3:b56e933bebc2 233
aktk 3:b56e933bebc2 234 //
aktk 3:b56e933bebc2 235 // Sort set data array in distanceAscending order
aktk 3:b56e933bebc2 236 //
aktk 3:b56e933bebc2 237 // Bucket sort
aktk 3:b56e933bebc2 238 for(int i = 0; i < _ENUM; i++)
aktk 3:b56e933bebc2 239 tmp_set[i].x = 0xaaaa;
aktk 3:b56e933bebc2 240 tmp = 0;
aktk 4:701f958d137a 241 for(int i = 0; i < num; i++) {
aktk 3:b56e933bebc2 242 // use x as index for x range [_LIDX,_RIDX]
aktk 3:b56e933bebc2 243 if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
aktk 3:b56e933bebc2 244 tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
aktk 3:b56e933bebc2 245 tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
aktk 3:b56e933bebc2 246 } else { // if a same x has been input, calcurate mean.
aktk 3:b56e933bebc2 247 tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
aktk 3:b56e933bebc2 248 tmp_set[_Sample_Set[i].x].y /= 2;
aktk 3:b56e933bebc2 249 tmp++;
aktk 3:b56e933bebc2 250 }
aktk 3:b56e933bebc2 251 }
aktk 3:b56e933bebc2 252 #ifdef DEBUG
aktk 3:b56e933bebc2 253 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:b56e933bebc2 254 g_Serial_Signal.printf("-) tmp: %d\n", tmp );
aktk 3:b56e933bebc2 255 #endif
aktk 3:b56e933bebc2 256 // substruct tmp from number of sample.
aktk 3:b56e933bebc2 257 _Sample_Num -= tmp;
aktk 4:701f958d137a 258 snum -= tmp;
aktk 3:b56e933bebc2 259 #ifdef DEBUG
aktk 3:b56e933bebc2 260 g_Serial_Signal.printf("-----------------\n");
aktk 3:b56e933bebc2 261 g_Serial_Signal.printf(" _Sample_num: %d\n", _Sample_Num );
aktk 3:b56e933bebc2 262 #endif
aktk 3:b56e933bebc2 263 // apply sort on _Sample_Set
aktk 3:b56e933bebc2 264 tmp = 0;
aktk 3:b56e933bebc2 265 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 266 if(tmp_set[i].x != 0xaaaa) {
aktk 3:b56e933bebc2 267 _Sample_Set[i - tmp].x = tmp_set[i].x;
aktk 3:b56e933bebc2 268 _Sample_Set[i - tmp].y = tmp_set[i].y;
aktk 3:b56e933bebc2 269 } else // if no data, skip it
aktk 3:b56e933bebc2 270 tmp++;
aktk 3:b56e933bebc2 271 }
aktk 3:b56e933bebc2 272 }
aktk 11:d60fb729eacf 273
aktk 11:d60fb729eacf 274
aktk 11:d60fb729eacf 275 #ifdef TARGET_LPC1768
aktk 0:e85788b14028 276 void TRP105FS::_sampleData()
aktk 0:e85788b14028 277 {
aktk 0:e85788b14028 278 int tmp;
aktk 0:e85788b14028 279 char sig;
aktk 3:b56e933bebc2 280 unsigned short tmp_y;
aktk 0:e85788b14028 281 VDset tmp_set[_ENUM]; // for bucket sort
aktk 0:e85788b14028 282
aktk 3:b56e933bebc2 283 // For evry set,
aktk 3:b56e933bebc2 284 // 1, get x data via serai com,
aktk 3:b56e933bebc2 285 // 2, get y data,
aktk 0:e85788b14028 286 // and then do same for next index set.
aktk 0:e85788b14028 287 for(int i = 0; i < _Sample_Num; i++) {
aktk 0:e85788b14028 288 //
aktk 0:e85788b14028 289 // Recieve a Distance datus and store it into member
aktk 0:e85788b14028 290 //
aktk 3:b56e933bebc2 291 if(_useType == AsDEBUG) {
aktk 0:e85788b14028 292 g_Serial_Signal.putc('>');
aktk 3:b56e933bebc2 293 _Sample_Set[i].x = 0;
aktk 0:e85788b14028 294 do {
aktk 0:e85788b14028 295 sig = g_Serial_Signal.getc();
aktk 0:e85788b14028 296 if('0' <= sig && sig <= '9') {
aktk 3:b56e933bebc2 297 _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
aktk 0:e85788b14028 298 g_Serial_Signal.putc(char(sig));
aktk 0:e85788b14028 299 } else if(sig == 0x08) {
aktk 3:b56e933bebc2 300 _Sample_Set[i].x = 0;
aktk 0:e85788b14028 301 g_Serial_Signal.printf("[canseled!]");
aktk 0:e85788b14028 302 g_Serial_Signal.putc('\n');
aktk 0:e85788b14028 303 g_Serial_Signal.putc('>');
aktk 0:e85788b14028 304 }
aktk 0:e85788b14028 305 } while (!(sig == 0x0a || sig == 0x0d));
aktk 0:e85788b14028 306 g_Serial_Signal.putc('\n');
aktk 4:701f958d137a 307 } else {
aktk 3:b56e933bebc2 308 _Sample_Set[i].x = g_Serial_Signal.getc();
aktk 0:e85788b14028 309 }
aktk 0:e85788b14028 310
aktk 0:e85788b14028 311 // if input data is over the bound calibrate it below
aktk 3:b56e933bebc2 312 if (_Sample_Set[i].x < (unsigned short)_LIDX)
aktk 3:b56e933bebc2 313 _Sample_Set[i].x = (unsigned short)_LIDX;
aktk 3:b56e933bebc2 314 else if ((unsigned short)_RIDX < _Sample_Set[i].x)
aktk 3:b56e933bebc2 315 _Sample_Set[i].x = (unsigned short)_RIDX;
aktk 0:e85788b14028 316 //
aktk 0:e85788b14028 317 // Recieve a Voltage datus and store it into member
aktk 0:e85788b14028 318 //
aktk 0:e85788b14028 319 // LOW PASS FILTERED
aktk 0:e85788b14028 320 // Get 10 data and store mean as a sample.
aktk 0:e85788b14028 321 // After get one original sample, system waits for 0.1 sec,
aktk 0:e85788b14028 322 // thus it takes 1 sec evry sampling.
aktk 3:b56e933bebc2 323 _Sample_Set[i].y = 0;
aktk 0:e85788b14028 324 for(int j = 0; j < 10; j++) {
aktk 0:e85788b14028 325 //unsigned short 's range [0 , 65535]
aktk 3:b56e933bebc2 326 //the Number of significant figures of read ytage is 3 or 4.
aktk 9:ec1ee4a6b6a4 327 tmp_y = _ai.read_u16();
aktk 3:b56e933bebc2 328
aktk 0:e85788b14028 329 #ifdef DEBUG
aktk 3:b56e933bebc2 330 g_Serial_Signal.printf("%d,",tmp_y);
aktk 0:e85788b14028 331 #endif
aktk 3:b56e933bebc2 332 _Sample_Set[i].y += (tmp_y / 10);
aktk 0:e85788b14028 333 wait(0.1);
aktk 0:e85788b14028 334 }
aktk 0:e85788b14028 335 #ifdef DEBUG
aktk 3:b56e933bebc2 336 g_Serial_Signal.printf("(%d)\n",_Sample_Set[i].y);
aktk 0:e85788b14028 337 #endif
aktk 0:e85788b14028 338 }
aktk 0:e85788b14028 339 //
aktk 0:e85788b14028 340 // Sort set data array in distanceAscending order
aktk 0:e85788b14028 341 //
aktk 0:e85788b14028 342 // Bucket sort
aktk 0:e85788b14028 343 for(int i = 0; i < _ENUM; i++)
aktk 3:b56e933bebc2 344 tmp_set[i].x = 0xaaaa;
aktk 0:e85788b14028 345 tmp = 0;
aktk 0:e85788b14028 346 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 347 // use x as index for x range [LIDX,RIDX]
aktk 3:b56e933bebc2 348 if (tmp_set[_Sample_Set[i].x].x == 0xaaaa) {
aktk 3:b56e933bebc2 349 tmp_set[_Sample_Set[i].x].x = _Sample_Set[i].x;
aktk 3:b56e933bebc2 350 tmp_set[_Sample_Set[i].x].y = _Sample_Set[i].y;
aktk 3:b56e933bebc2 351 } else { // if a same x has been input, calcurate mean.
aktk 3:b56e933bebc2 352 tmp_set[_Sample_Set[i].x].y += _Sample_Set[i].y;
aktk 3:b56e933bebc2 353 tmp_set[_Sample_Set[i].x].y /= 2;
aktk 0:e85788b14028 354 tmp++;
aktk 0:e85788b14028 355 }
aktk 0:e85788b14028 356 }
aktk 0:e85788b14028 357 #ifdef DEBUG
aktk 0:e85788b14028 358 g_Serial_Signal.printf("%d\n", _Sample_Num );
aktk 0:e85788b14028 359 #endif
aktk 0:e85788b14028 360 // substruct tmp from number of sample.
aktk 0:e85788b14028 361 _Sample_Num -= tmp;
aktk 0:e85788b14028 362
aktk 0:e85788b14028 363 #ifdef DEBUG
aktk 0:e85788b14028 364 g_Serial_Signal.printf("tmp: %d\n", tmp );
aktk 0:e85788b14028 365 #endif
aktk 0:e85788b14028 366 // apply sort on _Sample_Set
aktk 0:e85788b14028 367 tmp = 0;
aktk 0:e85788b14028 368 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 369 if(tmp_set[i].x != 0xaaaa) {
aktk 3:b56e933bebc2 370 _Sample_Set[i - tmp].x = tmp_set[i].x;
aktk 3:b56e933bebc2 371 _Sample_Set[i - tmp].y = tmp_set[i].y;
aktk 0:e85788b14028 372 } else // if no data, skip it
aktk 0:e85788b14028 373 tmp++;
aktk 0:e85788b14028 374 }
aktk 0:e85788b14028 375 }
aktk 11:d60fb729eacf 376 #endif
aktk 0:e85788b14028 377
aktk 0:e85788b14028 378 //
aktk 0:e85788b14028 379 // Function to define _u_spline, specific constants of spline.
aktk 0:e85788b14028 380 //
aktk 0:e85788b14028 381 void TRP105FS::_makeSpline()
aktk 0:e85788b14028 382 {
aktk 3:b56e933bebc2 383 // x: x, distance
aktk 3:b56e933bebc2 384 // y: y, ytage
aktk 0:e85788b14028 385 //
aktk 0:e85788b14028 386 // N: max of index <=> (_Sample_Num - 1)
aktk 0:e85788b14028 387 //
aktk 0:e85788b14028 388 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 389 // i:[0,N]
aktk 0:e85788b14028 390 // _u_spline[0] = _u_spline[N] = 0
aktk 0:e85788b14028 391 //
aktk 0:e85788b14028 392 // h[i] = x[i+1] - x[i]
aktk 0:e85788b14028 393 // i:[0,N-1]; num of elm: N<=>_Sample_Num - 1
aktk 0:e85788b14028 394 double *h = (double*)malloc((_Sample_Num - 1) * sizeof(double));
aktk 0:e85788b14028 395 //unsigned short *h __attribute__((at(0x20080000)));
aktk 0:e85788b14028 396 //h = (unsigned short*)malloc((_Sample_Num - 1) * sizeof(unsigned short));
aktk 0:e85788b14028 397 // v[i] = 6*((y[i+2]-y[i+1])/h[i+1] + (y[i+1]-y[i])/h[i])
aktk 0:e85788b14028 398 // i:[0,N-2]
aktk 0:e85788b14028 399 double *v = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 400 //unsigned short *v __attribute__((at(0x20080100)));
aktk 0:e85788b14028 401 //v = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 402 // temporary array whose num of elm equals v array
aktk 0:e85788b14028 403 double *w = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 404 //unsigned short *w __attribute__((at(0x20080200)));
aktk 0:e85788b14028 405 //w = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 406 //
aktk 0:e85788b14028 407 // [ 2(h[0]+h[1]) , h[1] , O ] [_u[1] ] [v[0] ]
aktk 0:e85788b14028 408 // [ h[1] , 2(h[1]+h[2]) , h[2] ] [_u[2] ] [v[1] ]
aktk 0:e85788b14028 409 // [ ... ] * [... ] = [... ]
aktk 0:e85788b14028 410 // [ h[j] , 2(h[j]+h[j+1]) , h[j+1] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 411 // [ ... ] [ ... ] [ ... ]
aktk 0:e85788b14028 412 // [ h[N-3] , 2(h[N-3]+h[N-2]), h[N-2] ] [_u[j+1]] [v[j] ]
aktk 0:e85788b14028 413 // [ O h[N-2] , 2(h[N-2]+h[N-1]) ] [_u[N-1]] [v[N-2]]
aktk 0:e85788b14028 414 //
aktk 0:e85788b14028 415 // For LU decomposition
aktk 0:e85788b14028 416 double *Upper = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 417 //unsigned short *Upper __attribute__((at(0x20080300)));
aktk 0:e85788b14028 418 //Upper = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 419 double *Lower = (double*)malloc((_Sample_Num - 2) * sizeof(double));
aktk 0:e85788b14028 420 //unsigned short *Lower __attribute__((at(0x20080400)));
aktk 0:e85788b14028 421 //Lower = (unsigned short*)malloc((_Sample_Num - 2) * sizeof(unsigned short));
aktk 0:e85788b14028 422 #ifdef DEBUG
aktk 0:e85788b14028 423 _printOutData(_Sample_Set, _Sample_Num, "\nSampleSet");
aktk 0:e85788b14028 424 #endif
aktk 0:e85788b14028 425 for(int i = 0; i < _Sample_Num - 1; i++)
aktk 3:b56e933bebc2 426 h[i] = (double)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
aktk 3:b56e933bebc2 427 //(unsigned short)(_Sample_Set[i + 1].x - _Sample_Set[i].x);
aktk 0:e85788b14028 428
aktk 0:e85788b14028 429 for(int i = 0; i < _Sample_Num - 2; i++)
aktk 0:e85788b14028 430 v[i] = 6 * (
aktk 3:b56e933bebc2 431 ((double)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y)) / h[i + 1]
aktk 3:b56e933bebc2 432 //(unsigned short)(_Sample_Set[i + 2].y - _Sample_Set[i + 1].y) / h[i + 1]
aktk 0:e85788b14028 433 -
aktk 3:b56e933bebc2 434 ((double)(_Sample_Set[i + 1].y - _Sample_Set[i].y)) / h[i]
aktk 3:b56e933bebc2 435 //(unsigned short)(_Sample_Set[i + 1].y - _Sample_Set[i].y) / h[i]
aktk 0:e85788b14028 436 );
aktk 0:e85788b14028 437
aktk 0:e85788b14028 438 //
aktk 0:e85788b14028 439 // LU decomposition
aktk 0:e85788b14028 440 //
aktk 0:e85788b14028 441 Upper[0] = 2 * (h[0] + h[1]);
aktk 0:e85788b14028 442 Lower[0] = 0;
aktk 0:e85788b14028 443 for (int i = 1; i < _Sample_Num - 2; i++) {
aktk 0:e85788b14028 444 Lower[i] = h[i] / Upper[i - 1];
aktk 0:e85788b14028 445 Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
aktk 0:e85788b14028 446 }
aktk 0:e85788b14028 447
aktk 0:e85788b14028 448
aktk 0:e85788b14028 449 //
aktk 0:e85788b14028 450 // forward substitution
aktk 0:e85788b14028 451 //
aktk 0:e85788b14028 452 w[0] = v[0];
aktk 0:e85788b14028 453 for (int i = 1; i < _Sample_Num - 2; i ++) {
aktk 0:e85788b14028 454 w[i] = v[i] - Lower[i] * w[i-1];
aktk 0:e85788b14028 455 }
aktk 0:e85788b14028 456
aktk 0:e85788b14028 457
aktk 0:e85788b14028 458 //
aktk 0:e85788b14028 459 // backward substitution
aktk 0:e85788b14028 460 //
aktk 0:e85788b14028 461 _u_spline[_Sample_Num - 2] = w[_Sample_Num - 3] / Upper[_Sample_Num - 3];
aktk 0:e85788b14028 462 for(int i = _Sample_Num - 3; i > 0; i--) {
aktk 0:e85788b14028 463 _u_spline[i] = (w[(i - 1)] - h[(i)] * _u_spline[(i) + 1]) / Upper[(i - 1)];
aktk 0:e85788b14028 464 }
aktk 0:e85788b14028 465
aktk 0:e85788b14028 466 // _u_spline[i] === d^2/dx^2(Spline f)[i]
aktk 0:e85788b14028 467 _u_spline[0] = _u_spline[_Sample_Num - 1] = 0.0;
aktk 0:e85788b14028 468
aktk 0:e85788b14028 469 #ifdef DEBUG
aktk 0:e85788b14028 470 _printOutData(h, _Sample_Num - 1, "h");
aktk 0:e85788b14028 471 _printOutData(v, _Sample_Num - 2, "v");
aktk 0:e85788b14028 472 _printOutData(w, _Sample_Num - 2, "w");
aktk 0:e85788b14028 473 _printOutData(Upper, _Sample_Num - 2, "Upper");
aktk 0:e85788b14028 474 _printOutData(Lower, _Sample_Num - 2, "Lower");
aktk 0:e85788b14028 475 _printOutData(_u_spline, _Sample_Num, "u");
aktk 0:e85788b14028 476 #endif
aktk 0:e85788b14028 477 free(h);
aktk 0:e85788b14028 478 free(v);
aktk 0:e85788b14028 479 free(w);
aktk 0:e85788b14028 480 free(Upper);
aktk 0:e85788b14028 481 free(Lower);
aktk 0:e85788b14028 482 }
aktk 0:e85788b14028 483 //
aktk 0:e85788b14028 484 // Function to return Voltage for distance.
aktk 0:e85788b14028 485 //
aktk 0:e85788b14028 486 unsigned short TRP105FS:: _getSplineYof(
aktk 0:e85788b14028 487 double arg_x // the argument is supposed as distance [mm]
aktk 0:e85788b14028 488 )
aktk 0:e85788b14028 489 {
aktk 3:b56e933bebc2 490 double y; // ytage calculated by spline polynomial
aktk 0:e85788b14028 491 double a,b,c,d; // which is specific constant of spline, and can be expressed with _u.
aktk 0:e85788b14028 492 int itv = 0; // interval(section) of interpolation
aktk 0:e85788b14028 493 // the number of interval is less 1 than the number of sample sets,
aktk 0:e85788b14028 494 // which means the max number of interval is _Sample_num - 2.
aktk 3:b56e933bebc2 495 if((double)(_Sample_Set[0].x) <= arg_x) {
aktk 3:b56e933bebc2 496 while (!((double)(_Sample_Set[itv].x) <= arg_x && arg_x < (double)(_Sample_Set[itv + 1].x))) {
aktk 0:e85788b14028 497 itv++;
aktk 0:e85788b14028 498 if(itv > _Sample_Num - 2) {
aktk 0:e85788b14028 499 itv = _Sample_Num - 2;
aktk 0:e85788b14028 500 break;
aktk 0:e85788b14028 501 }
aktk 0:e85788b14028 502 }
aktk 0:e85788b14028 503 }
aktk 3:b56e933bebc2 504 a = (double)(_u_spline[itv + 1] - _u_spline[itv]) / 6.0 / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x);
aktk 0:e85788b14028 505 b = (double)(_u_spline[itv]) / 2.0;
aktk 3:b56e933bebc2 506 c = (double)(_Sample_Set[itv + 1].y - _Sample_Set[itv].y) / (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x)
aktk 0:e85788b14028 507 -
aktk 3:b56e933bebc2 508 (double)(_Sample_Set[itv + 1].x - _Sample_Set[itv].x) * (double)(_u_spline[itv + 1] + 2.0 * _u_spline[itv]) / 6.0;
aktk 3:b56e933bebc2 509 d = (double)(_Sample_Set[itv].y);
aktk 0:e85788b14028 510 // cubic spline expression
aktk 3:b56e933bebc2 511 y = a * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 512 +
aktk 3:b56e933bebc2 513 b * (arg_x - (double)(_Sample_Set[itv].x)) * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 514 +
aktk 3:b56e933bebc2 515 c * (arg_x - (double)(_Sample_Set[itv].x))
aktk 0:e85788b14028 516 +
aktk 0:e85788b14028 517 d;
aktk 0:e85788b14028 518
aktk 0:e85788b14028 519 #ifdef DEBUG2
aktk 0:e85788b14028 520 g_Serial_Signal.printf("%f(interval: %d)", arg_x, itv);
aktk 0:e85788b14028 521 g_Serial_Signal.printf("a:%f, b:%f, c:%f, d:%f, ", a,b,c,d);
aktk 0:e85788b14028 522 g_Serial_Signal.printf("(y:%f -> %d)\n", y, (unsigned short)y);
aktk 0:e85788b14028 523 #endif
aktk 0:e85788b14028 524
aktk 0:e85788b14028 525 return ((unsigned short)(int)y);
aktk 0:e85788b14028 526 }
aktk 0:e85788b14028 527
aktk 11:d60fb729eacf 528 #if defined(HAS_COM_TO_CONSOLE)
aktk 0:e85788b14028 529 void TRP105FS::calibrateSensor()
aktk 0:e85788b14028 530 {
aktk 0:e85788b14028 531 _sampleData();
aktk 0:e85788b14028 532 _makeSpline();
aktk 0:e85788b14028 533
aktk 0:e85788b14028 534 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 535 _Set[i].x = i;
aktk 4:701f958d137a 536 _Set[i].y = _getSplineYof((double)(_Set[i].x));
aktk 4:701f958d137a 537 _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
aktk 3:b56e933bebc2 538 #ifdef DEBUG2
aktk 3:b56e933bebc2 539 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 3:b56e933bebc2 540 #endif
aktk 3:b56e933bebc2 541 }
aktk 3:b56e933bebc2 542 }
aktk 11:d60fb729eacf 543 #endif
aktk 3:b56e933bebc2 544
aktk 3:b56e933bebc2 545 void TRP105FS::calibrate()
aktk 3:b56e933bebc2 546 {
aktk 4:701f958d137a 547 #ifdef DEBUG2
aktk 4:701f958d137a 548 g_Serial_Signal.printf("Sample Data Set\n");
aktk 4:701f958d137a 549 for(int i = 0; i < _Sample_Num; i++)
aktk 4:701f958d137a 550 g_Serial_Signal.printf("(%d,%d)\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 4:701f958d137a 551 #endif
aktk 4:701f958d137a 552
aktk 3:b56e933bebc2 553 _makeSpline();
aktk 3:b56e933bebc2 554
aktk 3:b56e933bebc2 555 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 556 _Set[i].x = i;
aktk 4:701f958d137a 557 _Set[i].y = _getSplineYof((double)(_Set[i].x));
aktk 4:701f958d137a 558 _Threshold[i] = _getSplineYof((double)(_Set[i].x) + 0.5);
aktk 4:701f958d137a 559
aktk 0:e85788b14028 560 #ifdef DEBUG2
aktk 0:e85788b14028 561 g_Serial_Signal.printf("(get...threashold:%d)\n", _Threshold[i]);
aktk 0:e85788b14028 562 #endif
aktk 0:e85788b14028 563 }
aktk 0:e85788b14028 564 }
aktk 0:e85788b14028 565
aktk 0:e85788b14028 566 void TRP105FS::saveSetting()
aktk 0:e85788b14028 567 {
aktk 0:e85788b14028 568 FILE *fp;
aktk 0:e85788b14028 569
aktk 0:e85788b14028 570 fp = fopen("/local/savedata.log", "wb");
aktk 0:e85788b14028 571
aktk 0:e85788b14028 572 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 573 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 574 fputc(0x2c, fp);
aktk 3:b56e933bebc2 575 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 576 fputc(0x2c, fp);
aktk 0:e85788b14028 577 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 578 fputc(0x3b, fp);
aktk 0:e85788b14028 579 }
aktk 13:16fc61b41eff 580 fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 581 fputc(0x3b, fp);
aktk 0:e85788b14028 582 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 583 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 584 fputc(0x2c, fp);
aktk 3:b56e933bebc2 585 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 586 fputc(0x3b, fp);
aktk 0:e85788b14028 587 }
aktk 0:e85788b14028 588 fclose(fp);
aktk 0:e85788b14028 589
aktk 0:e85788b14028 590 }
aktk 0:e85788b14028 591
aktk 11:d60fb729eacf 592 #ifdef TARGET_LPC1768
aktk 0:e85788b14028 593 void TRP105FS::printThresholds()
aktk 0:e85788b14028 594 {
aktk 0:e85788b14028 595 for(int i = 0; i < _ENUM; i++)
aktk 0:e85788b14028 596 g_Serial_Signal.printf("Threshold[%d]%d\n",i,_Threshold[i]);
aktk 0:e85788b14028 597 }
aktk 11:d60fb729eacf 598 #endif
aktk 11:d60fb729eacf 599
aktk 0:e85788b14028 600 void TRP105FS::loadSetting()
aktk 0:e85788b14028 601 {
aktk 0:e85788b14028 602 FILE *fp;
aktk 0:e85788b14028 603 char tmp;
aktk 0:e85788b14028 604
aktk 0:e85788b14028 605 //sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 606 //fp = fopen(filepath, "rb");
aktk 0:e85788b14028 607 fp = fopen("/local/savedata.log", "rb");
aktk 0:e85788b14028 608 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 609
aktk 3:b56e933bebc2 610 fread(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 611 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 612 #ifdef DEBUG2
aktk 3:b56e933bebc2 613 g_Serial_Signal.printf("%d%c", _Set[i].x, tmp);
aktk 0:e85788b14028 614 #endif
aktk 0:e85788b14028 615
aktk 3:b56e933bebc2 616 fread(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 617 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 618 #ifdef DEBUG2
aktk 3:b56e933bebc2 619 g_Serial_Signal.printf("%d%c", _Set[i].y, tmp);
aktk 0:e85788b14028 620 #endif
aktk 0:e85788b14028 621
aktk 0:e85788b14028 622 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 623 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 624 #ifdef DEBUG2
aktk 0:e85788b14028 625 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 626 #endif
aktk 0:e85788b14028 627 }
aktk 0:e85788b14028 628
aktk 0:e85788b14028 629 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 630 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 631
aktk 0:e85788b14028 632 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 633 fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 634 fread(&tmp, sizeof(char),1,fp);
aktk 3:b56e933bebc2 635 fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 636 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 637 }
aktk 0:e85788b14028 638 fclose(fp);
aktk 0:e85788b14028 639 }
aktk 0:e85788b14028 640
aktk 0:e85788b14028 641
aktk 0:e85788b14028 642 void TRP105FS::saveSetting(
aktk 0:e85788b14028 643 const char *filename
aktk 0:e85788b14028 644 )
aktk 0:e85788b14028 645 {
aktk 0:e85788b14028 646 FILE *fp;
aktk 0:e85788b14028 647 char *filepath;
aktk 0:e85788b14028 648 int fnnum = 0;
aktk 0:e85788b14028 649
aktk 0:e85788b14028 650 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 651 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 652
aktk 0:e85788b14028 653 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 654 fp = fopen(filepath, "wb");
aktk 0:e85788b14028 655
aktk 0:e85788b14028 656 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 657 fwrite(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 658 fputc(0x2c, fp);
aktk 3:b56e933bebc2 659 fwrite(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 660 fputc(0x2c, fp);
aktk 0:e85788b14028 661 fwrite(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 662 fputc(0x3b, fp);
aktk 0:e85788b14028 663 }
aktk 13:16fc61b41eff 664 fwrite(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 665 fputc(0x3b, fp);
aktk 0:e85788b14028 666 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 667 fwrite(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 668 fputc(0x2c, fp);
aktk 3:b56e933bebc2 669 fwrite(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 670 fputc(0x3b, fp);
aktk 0:e85788b14028 671 }
aktk 0:e85788b14028 672 fclose(fp);
aktk 0:e85788b14028 673 free(filepath);
aktk 0:e85788b14028 674 }
aktk 0:e85788b14028 675
aktk 0:e85788b14028 676 void TRP105FS::loadSetting(
aktk 0:e85788b14028 677 const char *filename
aktk 0:e85788b14028 678 )
aktk 0:e85788b14028 679 {
aktk 0:e85788b14028 680 FILE *fp;
aktk 0:e85788b14028 681 char *filepath;
aktk 0:e85788b14028 682 char tmp;
aktk 0:e85788b14028 683 int fnnum = 0;
aktk 0:e85788b14028 684
aktk 0:e85788b14028 685 while (filename[fnnum] != 0) fnnum++;
aktk 0:e85788b14028 686 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 0:e85788b14028 687
aktk 0:e85788b14028 688 sprintf(filepath, "/local/%s", filename);
aktk 0:e85788b14028 689 fp = fopen(filepath, "rb");
aktk 10:b50e4bb40571 690
aktk 0:e85788b14028 691 for(int i = 0; i < _ENUM; i++) {
aktk 0:e85788b14028 692
aktk 3:b56e933bebc2 693 fread(&_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 694 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 695 #ifdef DEBUG3
aktk 3:b56e933bebc2 696 g_Serial_Signal.printf("%d%c", _Set[i].x, tmp);
aktk 0:e85788b14028 697 #endif
aktk 0:e85788b14028 698
aktk 3:b56e933bebc2 699 fread(&_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 700 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 701 #ifdef DEBUG3
aktk 3:b56e933bebc2 702 g_Serial_Signal.printf("%d%c", _Set[i].y, tmp);
aktk 0:e85788b14028 703 #endif
aktk 0:e85788b14028 704
aktk 0:e85788b14028 705 fread(&_Threshold[i], sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 706 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 707 #ifdef DEBUG3
aktk 0:e85788b14028 708 g_Serial_Signal.printf("%d%c\n",_Threshold[i], tmp);
aktk 0:e85788b14028 709 #endif
aktk 0:e85788b14028 710 }
aktk 0:e85788b14028 711
aktk 0:e85788b14028 712 fread(&_Sample_Num, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 713 fread(&tmp, sizeof(char), 1, fp);
aktk 0:e85788b14028 714 #ifdef DEBUG3
aktk 0:e85788b14028 715 g_Serial_Signal.printf("%d%c\n",_Sample_Num, tmp);
aktk 0:e85788b14028 716 #endif
aktk 0:e85788b14028 717
aktk 0:e85788b14028 718 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 719 fread(&_Sample_Set[i].x, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 720 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 721 #ifdef DEBUG3
aktk 3:b56e933bebc2 722 g_Serial_Signal.printf("%d%c", _Sample_Set[i].x, tmp);
aktk 0:e85788b14028 723 #endif
aktk 0:e85788b14028 724
aktk 3:b56e933bebc2 725 fread(&_Sample_Set[i].y, sizeof(unsigned short), 1, fp);
aktk 0:e85788b14028 726 fread(&tmp, sizeof(char),1,fp);
aktk 0:e85788b14028 727 #ifdef DEBUG3
aktk 3:b56e933bebc2 728 g_Serial_Signal.printf("%d%c", _Sample_Set[i].y, tmp);
aktk 0:e85788b14028 729 #endif
aktk 0:e85788b14028 730 }
aktk 0:e85788b14028 731 fclose(fp);
aktk 0:e85788b14028 732 free(filepath);
aktk 0:e85788b14028 733 }
aktk 0:e85788b14028 734
aktk 12:db5110d9d494 735 void TRP105FS::loadSetting_fromSerial(
aktk 12:db5110d9d494 736 Serial* com
aktk 12:db5110d9d494 737 )
aktk 12:db5110d9d494 738 {
aktk 12:db5110d9d494 739 char buffer[3];
aktk 12:db5110d9d494 740
aktk 12:db5110d9d494 741 for(int i = 0; i < _ENUM; i++) {
aktk 13:16fc61b41eff 742 //Set X
aktk 12:db5110d9d494 743 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 744 buffer[j] = com->getc();
aktk 12:db5110d9d494 745 }
aktk 13:16fc61b41eff 746 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 747 _Set[i].x = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 748
aktk 13:16fc61b41eff 749 //Set Y
aktk 12:db5110d9d494 750 for(int j = 0; j < 3; j++) {
aktk 13:16fc61b41eff 751 buffer[j] = com->getc();
aktk 13:16fc61b41eff 752 }
aktk 13:16fc61b41eff 753 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 754 _Set[i].y = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 13:16fc61b41eff 755
aktk 13:16fc61b41eff 756 //Threshold
aktk 13:16fc61b41eff 757 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 758 buffer[j] = com->getc();
aktk 12:db5110d9d494 759 }
aktk 13:16fc61b41eff 760 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 761 _Threshold[i] = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 762 }
aktk 12:db5110d9d494 763
aktk 13:16fc61b41eff 764 //Sample Num
aktk 12:db5110d9d494 765 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 766 buffer[j] = com->getc();
aktk 12:db5110d9d494 767 }
aktk 13:16fc61b41eff 768 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 769 _Sample_Num = static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 770
aktk 12:db5110d9d494 771 for(int i = 0; i < _Sample_Num; i++) {
aktk 13:16fc61b41eff 772 //Sample Set X
aktk 12:db5110d9d494 773 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 774 buffer[j] = com->getc();
aktk 12:db5110d9d494 775 }
aktk 13:16fc61b41eff 776 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 777 _Sample_Set[i].x= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 13:16fc61b41eff 778
aktk 13:16fc61b41eff 779 //Sample Set Y
aktk 12:db5110d9d494 780 for(int j = 0; j < 3; j++) {
aktk 12:db5110d9d494 781 buffer[j] = com->getc();
aktk 12:db5110d9d494 782 }
aktk 13:16fc61b41eff 783 if(i != _Sample_Num - 1)
aktk 13:16fc61b41eff 784 com->putc(0x06); //return ACK
aktk 13:16fc61b41eff 785 else
aktk 13:16fc61b41eff 786 com->putc(0x04); //return EOT
aktk 13:16fc61b41eff 787 _Sample_Set[i].y= static_cast<unsigned short>(0xFFFF & (buffer[1] << 8 | buffer[0]));
aktk 12:db5110d9d494 788 }
aktk 12:db5110d9d494 789 }
aktk 12:db5110d9d494 790
aktk 4:701f958d137a 791 void TRP105FS::printOutData(
aktk 4:701f958d137a 792 const char *filename)
aktk 0:e85788b14028 793 {
aktk 0:e85788b14028 794 FILE *fp;
aktk 4:701f958d137a 795 char *filepath;
aktk 4:701f958d137a 796 int fnnum = 0;
aktk 4:701f958d137a 797
aktk 4:701f958d137a 798 while (filename[fnnum] != 0) fnnum++;
aktk 4:701f958d137a 799 filepath = (char *)malloc((fnnum + 8) * sizeof(char)); // "/local/" are 7 char and \0 is 1 char.
aktk 4:701f958d137a 800 sprintf(filepath, "/local/%s", filename);
aktk 6:d2363b50aeaf 801 fp = fopen(filepath, "w");
aktk 6:d2363b50aeaf 802 //fp = fopen("/local/log.txt", "w"); // open file in writing mode
aktk 10:b50e4bb40571 803
aktk 3:b56e933bebc2 804 fprintf(fp, "x, y,(threshold)\n");
aktk 0:e85788b14028 805 for(int i = 0; i < _ENUM; i++) {
aktk 3:b56e933bebc2 806 fprintf(fp, "%d,%d,(%d)\n", _Set[i].x, _Set[i].y, _Threshold[i]);
aktk 0:e85788b14028 807 }
aktk 3:b56e933bebc2 808 fprintf(fp, "\nSample:x, y\n");
aktk 0:e85788b14028 809 for(int i = 0; i < _Sample_Num; i++) {
aktk 3:b56e933bebc2 810 fprintf(fp, "%d,%d\n", _Sample_Set[i].x, _Sample_Set[i].y);
aktk 0:e85788b14028 811 }
aktk 10:b50e4bb40571 812
aktk 4:701f958d137a 813 free(filepath);
aktk 0:e85788b14028 814 fclose(fp);
aktk 0:e85788b14028 815
aktk 0:e85788b14028 816 }
aktk 4:701f958d137a 817
aktk 0:e85788b14028 818 void TRP105FS::_printOutData(unsigned short *arg, int num, char* name)
aktk 0:e85788b14028 819 {
aktk 0:e85788b14028 820 FILE *fp;
aktk 0:e85788b14028 821 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 822 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 823 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 824 fprintf(fp, "%d, ", arg[i]);
aktk 0:e85788b14028 825 }
aktk 0:e85788b14028 826 fprintf(fp, "\n");
aktk 0:e85788b14028 827 fclose(fp);
aktk 0:e85788b14028 828 }
aktk 0:e85788b14028 829 void TRP105FS::_printOutData(double *arg, int num, char* name)
aktk 0:e85788b14028 830 {
aktk 0:e85788b14028 831 FILE *fp;
aktk 0:e85788b14028 832
aktk 0:e85788b14028 833 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 834 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 835 for(int i = 0; i < num; i++) {
aktk 0:e85788b14028 836 fprintf(fp, "%.2f, ", arg[i]);
aktk 0:e85788b14028 837 }
aktk 0:e85788b14028 838 fprintf(fp, "\n");
aktk 0:e85788b14028 839 fclose(fp);
aktk 0:e85788b14028 840 }
aktk 0:e85788b14028 841 void TRP105FS::_printOutData(VDset *arg, int num, char* name)
aktk 0:e85788b14028 842 {
aktk 0:e85788b14028 843 FILE *fp;
aktk 0:e85788b14028 844
aktk 0:e85788b14028 845 fp = fopen("/local/varlog.txt", "a"); // open file in add mode
aktk 0:e85788b14028 846 fprintf(fp, "%10s\n", name);
aktk 0:e85788b14028 847 for(int i = 0; i < num; i++) {
aktk 3:b56e933bebc2 848 fprintf(fp, "%d, ", arg[i].y);
aktk 0:e85788b14028 849 }
aktk 0:e85788b14028 850 fprintf(fp, "\n");
aktk 0:e85788b14028 851 fclose(fp);
aktk 0:e85788b14028 852 }