From b7ce588b0713ba6bfe67f9967423df8813d9b73a Mon Sep 17 00:00:00 2001
From: Oleg Kalachev <okalachev@gmail.com>
Date: Tue, 13 Oct 2020 15:22:13 +0300
Subject: [PATCH] blocks: add yaw_tolerance parameter

---
 clover_blocks/README.md     | 1 +
 clover_blocks/www/main.js   | 1 +
 clover_blocks/www/python.js | 3 +--
 3 files changed, 3 insertions(+), 2 deletions(-)

diff --git a/clover_blocks/README.md b/clover_blocks/README.md
index 2711a833..4ff7c825 100644
--- a/clover_blocks/README.md
+++ b/clover_blocks/README.md
@@ -30,6 +30,7 @@ The frontend files are located in [`www`](./www/) subdirectory. The frontend app
 Parameters read by frontend:
 
 * `~navigate_tolerance` (*float*) – distance tolerance in meters, used for navigate-like blocks (default: 0.2).
+* `~yaw_tolerance` (*float*) – yaw angle tolerance in degrees, used in set_yaw block (default: 20).
 * `~sleep_time` (*float*) – duration of sleep in loop cycles, used for navigate-like blocks (default: 0.2).
 * `~confirm_run` (*bool*) – enable confirmation to run the program (default: true).
 
diff --git a/clover_blocks/www/main.js b/clover_blocks/www/main.js
index b0000cd7..9f68fd2f 100644
--- a/clover_blocks/www/main.js
+++ b/clover_blocks/www/main.js
@@ -39,6 +39,7 @@ var workspace = Blockly.inject('blockly', {
 function readParams() {
 	return Promise.all([
 		ros.readParam('navigate_tolerance', true, 0.2),
+		ros.readParam('yaw_tolerance', true, 20),
 		ros.readParam('sleep_time', true, 0.2),
 		ros.readParam('confirm_run', true, true),
 	]);
diff --git a/clover_blocks/www/python.js b/clover_blocks/www/python.js
index 0d9667d4..7135da19 100644
--- a/clover_blocks/www/python.js
+++ b/clover_blocks/www/python.js
@@ -38,11 +38,10 @@ const LAND_WAIT = () => `\ndef land_wait():
     while get_telemetry().armed:
         rospy.sleep(${params.sleep_time})\n`;
 
-// TODO: tolerance to parameters
 const WAIT_YAW = () => `\ndef wait_yaw():
     while not rospy.is_shutdown():
         telem = get_telemetry(frame_id='navigate_target')
-        if abs(telem.yaw) < math.radians(20):
+        if abs(telem.yaw) < math.radians(${params.yaw_tolerance}):
             return
         rospy.sleep(${params.sleep_time})\n`;