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