Diff: teach.cpp

Revision:

5:658665f56b9c

Parent:

4:b0a7cc9ff8d4

Child:

6:6c3d0a838b6c

--- a/teach.cpp	Wed May 13 09:29:18 2020 +0000
+++ b/teach.cpp	Wed May 13 19:57:21 2020 +0000
@@ -168,7 +168,7 @@
     
     printf("forward points %d backward points %d \n", forward, backward);
     
-    /*
+    
 
     k = 0;
     struct temp_arry forw[forward];                                            // temp array initialise and zerowing
@@ -178,16 +178,17 @@
         forw[i].pos[1]=0;
         forw[i].pos[2]=0;
     }
-
-    for(i=0; i<=max; i++) {                                                    // avrage forward points
-        for(p=0; p<=forward; p++) {
+    
+    for(i=0; i<max-10; i++) {                                                    // avrage forward points
+        for(p=0; p<forward; p++) {
             forw[p].time += data_vec[k].time;
             forw[p].pos[0] += data_vec[k].pos[0];
             forw[p].pos[1] += data_vec[k].pos[1];
             forw[p].pos[2] += data_vec[k].pos[2];
             k++;
+            //printf("data_vec[k].pos[0]: %f,data_vec[k].time: %d, k:%d \n",data_vec[k].pos[0],data_vec[k].time,k);
         }
-
+        
         while(!data_vec[k].start) {
             k++;
         }
@@ -195,7 +196,8 @@
         count++;
 
     }
-    for(i=0; i<=forward; i++) {
+    
+    for(i=0; i<forward; i++) {
         forw[i].time = (forw[i].time)/count;
         forw[i].pos[0] = (forw[i].pos[0])/count;
         forw[i].pos[1] = (forw[i].pos[1])/count;
@@ -203,23 +205,26 @@
     }
     count = 0;
     k = 0, i = 0, p = 0;                                                       // reset counters
-
+    
     // Save forward way to memory
     empty[0] = 0;                                                              // status: 000 standart point
     empty[1] = 0;                                                              //         001 start point
     empty[2] = 0;                                                              //         010 end point
-    for(i=0; i<=forward; i++) {
+    for(i=0; i<forward; i++) {
+        empty[1] = i;
         store.SpeichernPosition(tr_name,forw[i].time,forw[i].pos,empty,empty);
     }
-
-    for(i=0; i<=max; i++) {                                                // avrage end Point
+    
+    for(i=0; i<max; i++) {                                                // avrage end Point
         if(data_vec[i].end) {
             tempsum[0] += data_vec[i].pos[0];
             tempsum[1] += data_vec[i].pos[1];
             tempsum[2] += data_vec[i].pos[2];
             k++;
         }
+        
     }
+    printf("\n\n k: %d", k);
     tempsum[0] = tempsum[0]/k;
     tempsum[1] = tempsum[1]/k;
     tempsum[2] = tempsum[2]/k;
@@ -240,24 +245,25 @@
         backw[i].pos[2]=0;
     }
 
-    for(i=0; i<=max; i++) {                                                 // avrage backward points
+    for(i=0; i<max-10; i++) {                                                 // avrage backward points
 
         while(!data_vec[k].end) {
             k++;
         }
 
-        for(p=0; p<=backward; p++) {
+        for(p=0; p<backward; p++) {
             backw[p].time += data_vec[k].time;
             backw[p].pos[0] += data_vec[k].pos[0];
             backw[p].pos[1] += data_vec[k].pos[1];
             backw[p].pos[2] += data_vec[k].pos[2];
             k++;
+            printf("data_vec[k].pos[0]: %f,data_vec[k].time: %d, k:%d \n",data_vec[k].pos[0],data_vec[k].time,k);
         }
         i = k;
         count++;
     }
-
-    for(i=0; i<=backward; i++) {
+    printf("count: %d \n", count);
+    for(i=0; i<backward; i++) {
         backw[i].time = (backw[i].time)/count;
         backw[i].pos[0] = (backw[i].pos[0])/count;
         backw[i].pos[1] = (backw[i].pos[1])/count;
@@ -265,15 +271,15 @@
     }
     count = 0;
     k = 0, i = 0, p = 0;                                                       // reset counters
-
+    
     // save backward way to memory
     empty[0] = 0;                                                              // status: 000 standart point
     empty[1] = 0;                                                              //         001 start point
     empty[2] = 0;                                                              //         010 end point
-    for(i=0; i<=backward; i++) {
+    for(i=0; i<backward; i++) {
         store.SpeichernPosition(tr_name,backw[i].time,backw[i].pos,empty,empty);
     }
-*/
+
 }
 
 void teach::temp_store()