Diff: pathfollowing/PathFollowing.cpp

Revision:

2:820dcd23c8e3

Parent:

0:b87fd8dd4322

diff -r 26fc1b2f1c42 -r 820dcd23c8e3 pathfollowing/PathFollowing.cpp
--- a/pathfollowing/PathFollowing.cpp	Sat Nov 16 06:26:57 2019 +0000
+++ b/pathfollowing/PathFollowing.cpp	Wed Dec 11 04:59:36 2019 +0000
@@ -2,7 +2,7 @@
 #include "mbed.h"
 #include "math.h"
 
-double p_out,r_out_max;
+double p_out,r_out_max, q_out_max;
 double Kvq_p,Kvq_d,Kvr_p,Kvr_d;
 double diff_old,diffangle,diffangle_old;
 double out_dutyQ,out_dutyR;
@@ -37,8 +37,8 @@
     out_dutyQ=Kvq_p*diff+Kvq_d*(diff-diff_old)/(now_timeQ-old_timeQ); //ベクトルABに垂直方向の出力を決定
     diff_old=diff;
 
-    if(out_dutyQ>500)out_dutyQ=500;
-    if(out_dutyQ<-500)out_dutyQ=-500;
+    if(out_dutyQ>q_out_max)out_dutyQ=q_out_max;
+    if(out_dutyQ<-q_out_max)out_dutyQ=-q_out_max;
 
     old_timeQ=now_timeQ;
 
@@ -144,6 +144,10 @@
     r_out_max = r;
 }
 
+void set_q_out(double q){
+    q_out_max = q;   
+}
+
 void set_target_angle(double t)  //機体の目標角度設定関数
 {
     target_angle = t;