Easy Training / Teach

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.

Dependents:   Teach_demo Easyfit

teach.cpp@4:b0a7cc9ff8d4, 2020-05-13 (annotated)

Committer:
MarcelPortmann
Date:
Wed May 13 09:29:18 2020 +0000
Revision:
4:b0a7cc9ff8d4
Parent:
3:b896dc38386f
Child:
5:658665f56b9c
vbfdbd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MarcelPortmann 0:c203f964c949 1 /*
MarcelPortmann 0:c203f964c949 2 Hear cold stand a discription
MarcelPortmann 0:c203f964c949 3
MarcelPortmann 0:c203f964c949 4 */
MarcelPortmann 0:c203f964c949 5
MarcelPortmann 0:c203f964c949 6
MarcelPortmann 0:c203f964c949 7 #include "teach.h"
MarcelPortmann 0:c203f964c949 8
MarcelPortmann 0:c203f964c949 9
MarcelPortmann 0:c203f964c949 10 teach::teach(void) // constructor
MarcelPortmann 0:c203f964c949 11 {
MarcelPortmann 0:c203f964c949 12
MarcelPortmann 0:c203f964c949 13 lowcount = 0, highcount = 0; //sets startvalue
MarcelPortmann 0:c203f964c949 14 data_vec.clear();
xenjamo 3:b896dc38386f 15 store.init();
MarcelPortmann 0:c203f964c949 16 }
MarcelPortmann 0:c203f964c949 17
MarcelPortmann 0:c203f964c949 18 void teach::setstart(float posx,float posy,float posz, int time)
MarcelPortmann 0:c203f964c949 19 {
MarcelPortmann 0:c203f964c949 20 lowcount = 0;
MarcelPortmann 0:c203f964c949 21
MarcelPortmann 0:c203f964c949 22 temp.start = true;
MarcelPortmann 0:c203f964c949 23 temp.end = false;
MarcelPortmann 0:c203f964c949 24 temp.hi = highcount;
MarcelPortmann 0:c203f964c949 25 temp.low = lowcount;
MarcelPortmann 0:c203f964c949 26 temp.time = time;
MarcelPortmann 0:c203f964c949 27 temp.pos[0] = posx;
MarcelPortmann 0:c203f964c949 28 temp.pos[1] = posy;
MarcelPortmann 0:c203f964c949 29 temp.pos[2] = posz;
MarcelPortmann 0:c203f964c949 30 data_vec.push_back(temp);
MarcelPortmann 0:c203f964c949 31
MarcelPortmann 0:c203f964c949 32 lowcount++;
MarcelPortmann 0:c203f964c949 33 highcount++;
MarcelPortmann 0:c203f964c949 34
MarcelPortmann 0:c203f964c949 35 contr_sice();
MarcelPortmann 0:c203f964c949 36 }
MarcelPortmann 0:c203f964c949 37
MarcelPortmann 0:c203f964c949 38 void teach::setpoint(float posx,float posy,float posz, int time)
MarcelPortmann 0:c203f964c949 39 {
MarcelPortmann 0:c203f964c949 40 lowcount++;
MarcelPortmann 0:c203f964c949 41
MarcelPortmann 0:c203f964c949 42 temp.start = false;
MarcelPortmann 0:c203f964c949 43 temp.end = false;
MarcelPortmann 0:c203f964c949 44 temp.hi = highcount;
MarcelPortmann 0:c203f964c949 45 temp.low = lowcount;
MarcelPortmann 0:c203f964c949 46 temp.time = time;
MarcelPortmann 0:c203f964c949 47 temp.pos[0] = posx;
MarcelPortmann 0:c203f964c949 48 temp.pos[1] = posy;
MarcelPortmann 0:c203f964c949 49 temp.pos[2] = posz;
MarcelPortmann 0:c203f964c949 50 data_vec.push_back(temp);
MarcelPortmann 0:c203f964c949 51
MarcelPortmann 0:c203f964c949 52 contr_sice();
MarcelPortmann 0:c203f964c949 53 }
MarcelPortmann 0:c203f964c949 54
MarcelPortmann 0:c203f964c949 55 void teach::setend(float posx,float posy,float posz, int time)
MarcelPortmann 0:c203f964c949 56 {
MarcelPortmann 0:c203f964c949 57 temp.start = false;
MarcelPortmann 0:c203f964c949 58 temp.end = true;
MarcelPortmann 0:c203f964c949 59 temp.hi = highcount;
MarcelPortmann 0:c203f964c949 60 temp.low = lowcount;
MarcelPortmann 0:c203f964c949 61 temp.time = time;
MarcelPortmann 0:c203f964c949 62 temp.pos[0] = posx;
MarcelPortmann 0:c203f964c949 63 temp.pos[1] = posy;
MarcelPortmann 0:c203f964c949 64 temp.pos[2] = posz;
MarcelPortmann 0:c203f964c949 65 data_vec.push_back(temp);
MarcelPortmann 0:c203f964c949 66
MarcelPortmann 0:c203f964c949 67 lowcount = 0;
MarcelPortmann 0:c203f964c949 68 highcount++;
MarcelPortmann 0:c203f964c949 69
MarcelPortmann 0:c203f964c949 70 contr_sice();
MarcelPortmann 0:c203f964c949 71 }
MarcelPortmann 0:c203f964c949 72
MarcelPortmann 0:c203f964c949 73 void teach::save(string name)
MarcelPortmann 0:c203f964c949 74 {
MarcelPortmann 0:c203f964c949 75 tr_name = name;
MarcelPortmann 0:c203f964c949 76 store.ErstellenDatenblock(tr_name);
MarcelPortmann 4:b0a7cc9ff8d4 77
MarcelPortmann 4:b0a7cc9ff8d4 78
MarcelPortmann 4:b0a7cc9ff8d4 79
MarcelPortmann 0:c203f964c949 80 process();
MarcelPortmann 0:c203f964c949 81
MarcelPortmann 0:c203f964c949 82 }
MarcelPortmann 0:c203f964c949 83
MarcelPortmann 0:c203f964c949 84 void teach::stop(void)
MarcelPortmann 0:c203f964c949 85 {
MarcelPortmann 0:c203f964c949 86 data_vec.clear();
MarcelPortmann 0:c203f964c949 87 highcount = 0; lowcount = 0;
MarcelPortmann 0:c203f964c949 88 // memory lösch funktion;
MarcelPortmann 0:c203f964c949 89 temp.start = false;
MarcelPortmann 0:c203f964c949 90 temp.end = false;
MarcelPortmann 0:c203f964c949 91 temp.hi = 0;
MarcelPortmann 0:c203f964c949 92 temp.low = 0;
MarcelPortmann 0:c203f964c949 93 temp.time = 0;
MarcelPortmann 0:c203f964c949 94 temp.pos[0] = 0;
MarcelPortmann 0:c203f964c949 95 temp.pos[1] = 0;
MarcelPortmann 0:c203f964c949 96 temp.pos[2] = 0;
MarcelPortmann 0:c203f964c949 97
MarcelPortmann 0:c203f964c949 98 }
MarcelPortmann 0:c203f964c949 99
MarcelPortmann 0:c203f964c949 100 void teach::process(void)
MarcelPortmann 0:c203f964c949 101 {
MarcelPortmann 0:c203f964c949 102 int i=0, k=0, p=0, count=0,forward = 100000, backward = 100000;
MarcelPortmann 0:c203f964c949 103 float tempsum[3] = {};
MarcelPortmann 0:c203f964c949 104 float empty[3] = {};
MarcelPortmann 0:c203f964c949 105 int max = data_vec.size();
MarcelPortmann 4:b0a7cc9ff8d4 106 printf("max sice %d \n", max);
MarcelPortmann 0:c203f964c949 107
MarcelPortmann 4:b0a7cc9ff8d4 108 for(i=0; i<max; i++) { // avrage starting Point
MarcelPortmann 0:c203f964c949 109 if(data_vec[i].start) {
MarcelPortmann 0:c203f964c949 110 tempsum[0] += data_vec[i].pos[0];
MarcelPortmann 0:c203f964c949 111 tempsum[1] += data_vec[i].pos[1];
MarcelPortmann 0:c203f964c949 112 tempsum[2] += data_vec[i].pos[2];
MarcelPortmann 0:c203f964c949 113 k++;
MarcelPortmann 0:c203f964c949 114 }
MarcelPortmann 0:c203f964c949 115 }
MarcelPortmann 0:c203f964c949 116 tempsum[0] = tempsum[0]/k;
MarcelPortmann 0:c203f964c949 117 tempsum[1] = tempsum[1]/k;
MarcelPortmann 0:c203f964c949 118 tempsum[2] = tempsum[2]/k;
MarcelPortmann 0:c203f964c949 119
MarcelPortmann 0:c203f964c949 120 empty[0] = 1; // status: 000 standart point
MarcelPortmann 0:c203f964c949 121 empty[1] = 0; // 001 start point
MarcelPortmann 0:c203f964c949 122 empty[2] = 0; // 010 end point
MarcelPortmann 0:c203f964c949 123 store.SpeichernPosition(tr_name,0,tempsum,empty,empty); // save startpoint to memory
MarcelPortmann 0:c203f964c949 124
MarcelPortmann 0:c203f964c949 125
MarcelPortmann 0:c203f964c949 126 i = 0, k = 0, tempsum[0] = 0, tempsum[1] = 0, tempsum[2] = 0; // reset counters
MarcelPortmann 0:c203f964c949 127
MarcelPortmann 0:c203f964c949 128
MarcelPortmann 0:c203f964c949 129
MarcelPortmann 0:c203f964c949 130 for(i=0; i<=max; i++) { // forward point number
MarcelPortmann 0:c203f964c949 131
MarcelPortmann 0:c203f964c949 132 while(!data_vec[k].end) {
MarcelPortmann 0:c203f964c949 133 count++;
MarcelPortmann 0:c203f964c949 134 k++;
MarcelPortmann 0:c203f964c949 135 }
MarcelPortmann 0:c203f964c949 136
MarcelPortmann 0:c203f964c949 137 if(count<forward) {
MarcelPortmann 0:c203f964c949 138 forward = count;
MarcelPortmann 0:c203f964c949 139 }
MarcelPortmann 0:c203f964c949 140 while(!data_vec[k].start) {
MarcelPortmann 0:c203f964c949 141 k++;
MarcelPortmann 0:c203f964c949 142 }
MarcelPortmann 0:c203f964c949 143 i = k;
MarcelPortmann 0:c203f964c949 144 count = 0;
MarcelPortmann 0:c203f964c949 145
MarcelPortmann 0:c203f964c949 146 }
MarcelPortmann 4:b0a7cc9ff8d4 147
MarcelPortmann 4:b0a7cc9ff8d4 148
MarcelPortmann 0:c203f964c949 149
MarcelPortmann 0:c203f964c949 150 k = 0;
MarcelPortmann 0:c203f964c949 151 for(i=0; i<=max; i++) { // backward point number
MarcelPortmann 0:c203f964c949 152
MarcelPortmann 0:c203f964c949 153 while(!data_vec[k].end) {
MarcelPortmann 0:c203f964c949 154 k++;
MarcelPortmann 0:c203f964c949 155 }
MarcelPortmann 0:c203f964c949 156
MarcelPortmann 0:c203f964c949 157 while(!data_vec[k].start) {
MarcelPortmann 0:c203f964c949 158 count++;
MarcelPortmann 0:c203f964c949 159 k++;
MarcelPortmann 0:c203f964c949 160 }
MarcelPortmann 0:c203f964c949 161 if(count<backward) {
MarcelPortmann 0:c203f964c949 162 backward = count;
MarcelPortmann 0:c203f964c949 163 }
MarcelPortmann 0:c203f964c949 164 i = k;
MarcelPortmann 0:c203f964c949 165 count = 0;
MarcelPortmann 0:c203f964c949 166
MarcelPortmann 0:c203f964c949 167 }
MarcelPortmann 4:b0a7cc9ff8d4 168
MarcelPortmann 4:b0a7cc9ff8d4 169 printf("forward points %d backward points %d \n", forward, backward);
MarcelPortmann 4:b0a7cc9ff8d4 170
MarcelPortmann 4:b0a7cc9ff8d4 171 /*
MarcelPortmann 0:c203f964c949 172
MarcelPortmann 0:c203f964c949 173 k = 0;
MarcelPortmann 0:c203f964c949 174 struct temp_arry forw[forward]; // temp array initialise and zerowing
MarcelPortmann 0:c203f964c949 175 for(i=0; i<forward; i++) {
MarcelPortmann 0:c203f964c949 176 forw[i].time=0;
MarcelPortmann 0:c203f964c949 177 forw[i].pos[0]=0;
MarcelPortmann 0:c203f964c949 178 forw[i].pos[1]=0;
MarcelPortmann 0:c203f964c949 179 forw[i].pos[2]=0;
MarcelPortmann 0:c203f964c949 180 }
MarcelPortmann 0:c203f964c949 181
MarcelPortmann 0:c203f964c949 182 for(i=0; i<=max; i++) { // avrage forward points
MarcelPortmann 0:c203f964c949 183 for(p=0; p<=forward; p++) {
MarcelPortmann 0:c203f964c949 184 forw[p].time += data_vec[k].time;
MarcelPortmann 0:c203f964c949 185 forw[p].pos[0] += data_vec[k].pos[0];
MarcelPortmann 0:c203f964c949 186 forw[p].pos[1] += data_vec[k].pos[1];
MarcelPortmann 0:c203f964c949 187 forw[p].pos[2] += data_vec[k].pos[2];
MarcelPortmann 0:c203f964c949 188 k++;
MarcelPortmann 0:c203f964c949 189 }
MarcelPortmann 0:c203f964c949 190
MarcelPortmann 0:c203f964c949 191 while(!data_vec[k].start) {
MarcelPortmann 0:c203f964c949 192 k++;
MarcelPortmann 0:c203f964c949 193 }
MarcelPortmann 0:c203f964c949 194 i = k;
MarcelPortmann 0:c203f964c949 195 count++;
MarcelPortmann 0:c203f964c949 196
MarcelPortmann 0:c203f964c949 197 }
MarcelPortmann 0:c203f964c949 198 for(i=0; i<=forward; i++) {
MarcelPortmann 0:c203f964c949 199 forw[i].time = (forw[i].time)/count;
MarcelPortmann 0:c203f964c949 200 forw[i].pos[0] = (forw[i].pos[0])/count;
MarcelPortmann 0:c203f964c949 201 forw[i].pos[1] = (forw[i].pos[1])/count;
MarcelPortmann 0:c203f964c949 202 forw[i].pos[2] = (forw[i].pos[2])/count;
MarcelPortmann 0:c203f964c949 203 }
MarcelPortmann 0:c203f964c949 204 count = 0;
MarcelPortmann 0:c203f964c949 205 k = 0, i = 0, p = 0; // reset counters
MarcelPortmann 0:c203f964c949 206
MarcelPortmann 0:c203f964c949 207 // Save forward way to memory
MarcelPortmann 0:c203f964c949 208 empty[0] = 0; // status: 000 standart point
MarcelPortmann 0:c203f964c949 209 empty[1] = 0; // 001 start point
MarcelPortmann 0:c203f964c949 210 empty[2] = 0; // 010 end point
MarcelPortmann 0:c203f964c949 211 for(i=0; i<=forward; i++) {
MarcelPortmann 0:c203f964c949 212 store.SpeichernPosition(tr_name,forw[i].time,forw[i].pos,empty,empty);
MarcelPortmann 0:c203f964c949 213 }
MarcelPortmann 0:c203f964c949 214
MarcelPortmann 0:c203f964c949 215 for(i=0; i<=max; i++) { // avrage end Point
MarcelPortmann 0:c203f964c949 216 if(data_vec[i].end) {
MarcelPortmann 0:c203f964c949 217 tempsum[0] += data_vec[i].pos[0];
MarcelPortmann 0:c203f964c949 218 tempsum[1] += data_vec[i].pos[1];
MarcelPortmann 0:c203f964c949 219 tempsum[2] += data_vec[i].pos[2];
MarcelPortmann 0:c203f964c949 220 k++;
MarcelPortmann 0:c203f964c949 221 }
MarcelPortmann 0:c203f964c949 222 }
MarcelPortmann 0:c203f964c949 223 tempsum[0] = tempsum[0]/k;
MarcelPortmann 0:c203f964c949 224 tempsum[1] = tempsum[1]/k;
MarcelPortmann 0:c203f964c949 225 tempsum[2] = tempsum[2]/k;
MarcelPortmann 0:c203f964c949 226
MarcelPortmann 0:c203f964c949 227 // save endpoint to memory
MarcelPortmann 0:c203f964c949 228 empty[0] = 0; // status: 000 standart point
MarcelPortmann 0:c203f964c949 229 empty[1] = 1; // 001 start point
MarcelPortmann 0:c203f964c949 230 empty[2] = 0; // 010 end point
MarcelPortmann 0:c203f964c949 231 store.SpeichernPosition(tr_name,0,tempsum,empty,empty); // save endpoint to memory
MarcelPortmann 0:c203f964c949 232
MarcelPortmann 0:c203f964c949 233 i = 0, k = 0, tempsum[0] = 0, tempsum[1] = 0, tempsum[2] = 0; // reset counters
MarcelPortmann 0:c203f964c949 234
MarcelPortmann 0:c203f964c949 235 struct temp_arry backw[backward]; // temp array initialise and zerowing
MarcelPortmann 0:c203f964c949 236 for(i=0; i<backward; i++) {
MarcelPortmann 0:c203f964c949 237 backw[i].time=0;
MarcelPortmann 0:c203f964c949 238 backw[i].pos[0]=0;
MarcelPortmann 0:c203f964c949 239 backw[i].pos[1]=0;
MarcelPortmann 0:c203f964c949 240 backw[i].pos[2]=0;
MarcelPortmann 0:c203f964c949 241 }
MarcelPortmann 0:c203f964c949 242
MarcelPortmann 0:c203f964c949 243 for(i=0; i<=max; i++) { // avrage backward points
MarcelPortmann 0:c203f964c949 244
MarcelPortmann 0:c203f964c949 245 while(!data_vec[k].end) {
MarcelPortmann 0:c203f964c949 246 k++;
MarcelPortmann 0:c203f964c949 247 }
MarcelPortmann 0:c203f964c949 248
MarcelPortmann 0:c203f964c949 249 for(p=0; p<=backward; p++) {
MarcelPortmann 0:c203f964c949 250 backw[p].time += data_vec[k].time;
MarcelPortmann 0:c203f964c949 251 backw[p].pos[0] += data_vec[k].pos[0];
MarcelPortmann 0:c203f964c949 252 backw[p].pos[1] += data_vec[k].pos[1];
MarcelPortmann 0:c203f964c949 253 backw[p].pos[2] += data_vec[k].pos[2];
MarcelPortmann 0:c203f964c949 254 k++;
MarcelPortmann 0:c203f964c949 255 }
MarcelPortmann 0:c203f964c949 256 i = k;
MarcelPortmann 0:c203f964c949 257 count++;
MarcelPortmann 0:c203f964c949 258 }
MarcelPortmann 0:c203f964c949 259
MarcelPortmann 0:c203f964c949 260 for(i=0; i<=backward; i++) {
MarcelPortmann 0:c203f964c949 261 backw[i].time = (backw[i].time)/count;
MarcelPortmann 0:c203f964c949 262 backw[i].pos[0] = (backw[i].pos[0])/count;
MarcelPortmann 0:c203f964c949 263 backw[i].pos[1] = (backw[i].pos[1])/count;
MarcelPortmann 0:c203f964c949 264 backw[i].pos[2] = (backw[i].pos[2])/count;
MarcelPortmann 0:c203f964c949 265 }
MarcelPortmann 0:c203f964c949 266 count = 0;
MarcelPortmann 0:c203f964c949 267 k = 0, i = 0, p = 0; // reset counters
MarcelPortmann 0:c203f964c949 268
MarcelPortmann 0:c203f964c949 269 // save backward way to memory
MarcelPortmann 0:c203f964c949 270 empty[0] = 0; // status: 000 standart point
MarcelPortmann 0:c203f964c949 271 empty[1] = 0; // 001 start point
MarcelPortmann 0:c203f964c949 272 empty[2] = 0; // 010 end point
MarcelPortmann 0:c203f964c949 273 for(i=0; i<=backward; i++) {
MarcelPortmann 0:c203f964c949 274 store.SpeichernPosition(tr_name,backw[i].time,backw[i].pos,empty,empty);
MarcelPortmann 0:c203f964c949 275 }
MarcelPortmann 4:b0a7cc9ff8d4 276 */
MarcelPortmann 0:c203f964c949 277 }
MarcelPortmann 0:c203f964c949 278
MarcelPortmann 0:c203f964c949 279 void teach::temp_store()
MarcelPortmann 0:c203f964c949 280 {
MarcelPortmann 0:c203f964c949 281
MarcelPortmann 0:c203f964c949 282
MarcelPortmann 0:c203f964c949 283 }
MarcelPortmann 0:c203f964c949 284
MarcelPortmann 0:c203f964c949 285 void teach::contr_sice()
MarcelPortmann 0:c203f964c949 286 {
MarcelPortmann 0:c203f964c949 287 if((data_vec.size()*sizeof(posdata)) >= MAX_SIZE) {
MarcelPortmann 0:c203f964c949 288 // fehlermeldung oder speichern bei zu grossem temp speicher
MarcelPortmann 0:c203f964c949 289 }
MarcelPortmann 0:c203f964c949 290 }