diff --git a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp index f5b3ccf479a84d40aada967b4558cd0a412e9ef3..678c87ec457c8265c9ff55d63592f5181e6c0a7a 100644 --- a/catkin_ws/src/asclinic_pkg/src/Kalman.cpp +++ b/catkin_ws/src/asclinic_pkg/src/Kalman.cpp @@ -19,7 +19,9 @@ float x = -1, y = -3, phi = 0.0;///////////////////////////////////////////// int left_p = 1, right_p = 1; ros::Time time_previous, time_now; // Timing variables ros::Duration elapsed_time; // Duration to calculate elapsed time - +ros::Time constrain_time; +ros::Time begin_time; +ros::Duration constrain_duration; double time_float; // Floating-point representation of elapsed time @@ -122,7 +124,12 @@ void answer(const asclinic_pkg::LeftRightInt32& msg) { void ImageAnswer(const asclinic_pkg::position& msg) { - + constrain_time = ros::Time::now(); + constrain_duration = constrain_time - begin_time; + float temp_value = constrain_duration.toSec(); + if (temp_value<30) { + return; + } // Check if elapsed time is less than 0.5 seconds if (authority ==0) { return; @@ -218,7 +225,7 @@ int main(int argc, char **argv) { ros::init(argc, argv, "Kalman"); std::string ns_for_group = ros::this_node::getNamespace(); ros::NodeHandle nh(ns_for_group); - + begin_time = ros::Time::now(); // Initialize time_now to current time time_now = ros::Time::now(); diff --git a/catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp b/catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp index 60daafa68cf4c2e5b067b9f3d9f5ab6467103c51..376db272304de8be1ef51c7bb1829cef3e3d6d56 100644 --- a/catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp +++ b/catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp @@ -58,7 +58,7 @@ float w_now = 0; float v_now = 0; int state_value = 2; // 0 is forward, 1 is rotate , 2 is pending -float initial_pwm = 24; +float initial_pwm = 25; //initial 26.5 void straightline(const asclinic_pkg::vw& vw_msg) {