Diff: sample02.cpp

Revision:

1:85da508f8d7e

Parent:

0:0b1d25a1682d

Child:

2:b51bb221438a

diff -r 0b1d25a1682d -r 85da508f8d7e sample02.cpp
--- a/sample02.cpp	Mon Jul 08 07:17:33 2019 +0000
+++ b/sample02.cpp	Tue Jul 09 08:20:47 2019 +0000
@@ -1,10 +1,10 @@
 #include "sample02.h"
 
-Sample::Sample(double accdis_, double decdis_, double maxspeed_):
-    accdis(accdis_),
-    decdis(decdis_),
-    maxspeed(maxspeed_)
+Sample::Sample(double accdis_, double decdis_, double maxspeed_)
 {
+    accdis  =accdis_;
+    decdis  =decdis_;
+    maxspeed=maxspeed_;
 }
 
 void Sample::terget_xy(int tergetx_, int tergety_, int startx_, int starty_)
@@ -18,7 +18,7 @@
         accsec    = 2.0 / maxspeed * accdis;
         decsec    = 2.0 / maxspeed * decdis;
         s_vector  = 0.1;
-        middledis = tergetDis - (accsec + decdis);
+        middledis = tergetDis - accdis - decdis;
     }
 }
 
@@ -26,17 +26,21 @@
 {
     nowx = now_x;
     nowy = now_y;
-    nowDis = hypot(nowx,nowy);
+    nowDis = hypot((startx-nowx),(starty-nowy));
     tergetRad = atan2((double)(tergety-nowy), (double)(tergetx-nowx));
     /***/
     if (nowDis < accdis) {
+        
         counter = sqrt(2.0*accsec/maxspeed*nowDis);
-        vector = s_vector + (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / (2.0*PI/(maxspeed-s_vector));
-    } else if (nowDis > accdis && nowDis < accdis+middledis) {
+        vector = s_vector + (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
+        
+    } else if (nowDis >= accdis && nowDis < accdis+middledis) {
         vector = maxspeed;
-    } else if (nowDis > accdis+middledis) {
-        counter = sqrt(2.0*accsec/maxspeed*(tergetDis - nowDis));
-        vector = (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter / (2.0*PI/maxspeed));
+    } else if (nowDis > (accdis+middledis)) {
+        
+        counter = sqrt(2.0*accsec/maxspeed*fabs(tergetDis-nowDis));
+        vector = (-1.0 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / (2.0*PI/maxspeed);
+        
     }
     /***/
 }