春ロボ　ロケット団 / Mbed 2 deprecated Spring_motor_test2

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.

Dependencies:   mbed SpeedController Encoder CruizCore_R1370P

Diff: main.cpp

Revision:
19:0c1c3b2e009f
Parent:
18:1d89ec4148ec
Child:
20:acb13e232678
--- a/main.cpp	Thu Mar 05 12:46:31 2020 +0000
+++ b/main.cpp	Thu Mar 05 13:09:29 2020 +0000
@@ -1,4 +1,3 @@
-
 #include "mbed.h"
 #include "EC.h"
 #include "SpeedController.h"
@@ -146,11 +145,13 @@
 
 WheelOmega omega;
 //パラメタ処理
-double pControl(int dx_,int dy_,double time_)
+double pControl(int dx_,int dy_,double time_,double diftime_)
 {
-    double distance,z,zMax;
+    double distance,z,zMax,olddistance;
+    
+    //double pid=Kp_*diff+Kd_*(diff-pre_diff)/(t-ptsc_);
     distance = sqrt((double)dx_*dx_+dy_*dy_);
-    z=0.004*distance;
+    z=0.004*distance - 0.004*(olddistance-distance)/diftime_;
     zMax=10;
     if(z>zMax) {
         z=zMax;
@@ -158,6 +159,7 @@
     if(time_<1) {
         z=z*time_;
     }
+    olddistance = distance;
     return z;
 }
 
@@ -203,7 +205,7 @@
     }
 
     int n=1,dx,dy,aimX,aimY;
-    double vx,vy;
+    double vx,vy,oldtime,newtime,diftime;
     Location location;
     Timer time;
     time.start();
@@ -225,7 +227,8 @@
         vx=dx/sqrt((double)dx*dx+dy*dy);
         vy=dy/sqrt((double)dx*dx+dy*dy);
         //四輪の出力計算
-        z=pControl(dx,dy,time.read());
+        diftime = newtime - oldtime;
+        z=pControl(dx,dy,time.read(),diftime);
         omega.setOmega(z);
         omega.setVxy(vx,vy,theta);
         omega.calOmega();