Revision 9:4a72ba990137, committed 2019-10-04

Comitter:

ec30109b

Date:

Fri Oct 04 08:21:13 2019 +0000

Parent:

7:c9707baaf007

Commit message:

a

Changed in this revision

--- a/proto01.cpp	Mon Sep 23 23:28:45 2019 +0000
+++ b/proto01.cpp	Fri Oct 04 08:21:13 2019 +0000
@@ -1,16 +1,16 @@
 #include "proto01.h"
-
+ 
 Proto1::Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_)
 {
     accdis    = accdis_;
-    accdis2   = accdis_;
+    accdis2   = accdis;
     decdis    = decdis_;
-    decdis2   = decdis_;
+    decdis2   = decdis;
     maxspeed  = maxspeed_;
-    maxspeed2 = maxspeed_;
+    maxspeed2 = maxspeed;
     s_vector  = s_vector_;
 }
-
+ 
 void Proto1::target(double target_, double start_)
 {
     targetDis = target_;
@@ -22,15 +22,15 @@
         consdis   = targetDis - accdis - decdis;
     } else {
         maxspeed  = maxspeed2 / 2.0;
-        accdis    = targetDis / 2.0;
-        decdis    = targetDis / 2.0;
-        consdis   = 0.0;
+        accdis    = targetDis / 2;
+        decdis    = targetDis / 2;
+        consdis   = 0;
     }
     /*秘伝のコード*/ /*いじるな*/
     accsec = 2.0 / maxspeed * accdis;
     decsec = 2.0 / maxspeed * decdis;
 }
-
+ 
 void Proto1::targetXY(int targetx_, int targety_, int startx_, int starty_)
 {
     targetDis = hypot((float)(startx_-targetx_), (float)(starty_-targety_));
@@ -44,22 +44,22 @@
         decdis    = decdis2;
         consdis   = targetDis - accdis - decdis;
     } else {
-        maxspeed  = maxspeed2 / 2.0;
-        accdis    = targetDis / 2.0;
-        decdis    = targetDis / 2.0;
-        consdis   = 0.0;
+        maxspeed =  maxspeed / 2.0;
+        accdis    = targetDis / 2;
+        decdis    = targetDis / 2;
+        consdis   = 0;
     }
     /*秘伝のコード*/ /*いじるな*/
     accsec = 2.0 / maxspeed * accdis;
     decsec = 2.0 / maxspeed * decdis;
 }
-
+ 
 void Proto1::Input_now(double now_)
 {
     now    = now_;
     nowDis = now - start;
 }
-
+ 
 void Proto1::Input_nowXY(int now_x, int now_y)
 {
     nowx      = now_x;
@@ -67,11 +67,11 @@
     nowDis    = hypot((startx-nowx),(starty-nowy));
     targetRad = atan2((double)(targety-nowy), (double)(targetx-nowx));
 }
-
+ 
 void Proto1::calculate()
 {
     if (nowDis < accdis) {
-        counter = sqrt(2.0*accsec/maxspeed*fabs(nowDis));
+        counter = sqrt(2.0*accsec/maxspeed*nowDis);
         vector = s_vector + (-1 * sin((2.0*PI/accsec)*counter) + (2.0*PI/accsec)*counter) / ((2.0*PI)/(maxspeed-s_vector));
     } else if (nowDis >= accdis && nowDis < accdis+consdis) {
         vector = maxspeed;
@@ -82,23 +82,18 @@
         vector = 0;
     }
 }
-
+ 
 double Proto1::getvalue()
 {
     return vector;
 }
-
+ 
 double Proto1::getvalue_x()
 {
     return vector * cos(targetRad);
 }
-
+ 
 double Proto1::getvalue_y()
 {
     return vector * sin(targetRad);
-}
-
-double Proto1::getTheta()
-{
-    return targetRad;
 }
\ No newline at end of file

--- a/proto01.h	Mon Sep 23 23:28:45 2019 +0000
+++ b/proto01.h	Fri Oct 04 08:21:13 2019 +0000
@@ -1,15 +1,15 @@
 #ifndef PROTO01_H
 #define PROTO01_H
 #define PI 3.141592653589793238462643383219502884
-
+ 
 #include "mbed.h"
-
+ 
 /**
  * S字制御クラス
  */
 class Proto1 {
 public:
-
+ 
     /**
      * コンストラクタ
      * @param accdis_   加速距離
@@ -17,14 +17,14 @@
      * @param maxspeed_ 最大速度(?)
      */
     Proto1(double accdis_, double decdis_, double maxspeed_, double s_vector_);
-
+ 
     /**
      * 目標点を設定する
      * @param target_ 目標点
      * @param start_  開始地点
      */
     void target(double target_, double start_);
-
+ 
     /**
      * xy座標で目標点を設定する
      * @param targetx_ 目標点のx座標
@@ -33,47 +33,40 @@
      * @param starty_  開始地点のy座標
      */
     void targetXY(int targetx_, int targety_, int startx_, int starty_);
-
+ 
     /**
      * 現在地点を入力
      * @param now_ 現在地点
      */
     void Input_now(double now_);
-
+ 
     /**
      * 現在地点を入力
      * @param now_x 現在地点のx座標
      * @param now_y 現在地点のy座標
      */
     void Input_nowXY(int now_x, int now_y);
-
+ 
     /**
      * S字に変化する速度を計算
      */
     void calculate();
-
+ 
     /**
      * x方向のベクトルを返す
      */
     double getvalue_x();
-
+ 
     /**
      * y方向のベクトルを返す
      */
     double getvalue_y();
-
+ 
     /**
      * そのまま返す
      */
     double getvalue();
-    
-    /**
-     * 角度を返す
-     */
-    double getTheta();
-    
-
-
+ 
     double accdis;
     double decdis;
     double accdis2;
@@ -82,25 +75,26 @@
     double decsec;
     double maxspeed;
     double maxspeed2;
-
+ 
     double s_vector;
     double vector;
-
+ 
     double startx;
     double starty;
     double start;
     double nowx;
     double nowy;
     double now;
-
+ 
     double targetDis;
     double targetRad;
     double targetx;
     double targety;
     double nowDis;
+    //consdis : 一定速度で走るところ
     double consdis;
     double counter;
-
+ 
 };
  
 #endif
\ No newline at end of file