diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md index 75ddcebf7..7d44985e1 100644 --- a/docs/user-guides/sbs_motion_commander.md +++ b/docs/user-guides/sbs_motion_commander.md @@ -19,16 +19,22 @@ Since you should have installed cflib in the previous step by step tutorial, you ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper -URI = 'radio://0/80/2M/E7E7E7E7E7' -logging.basicConfig(level=logging.ERROR) +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') + +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 if __name__ == '__main__': @@ -58,24 +64,22 @@ We want to know if the deck is correctly attached before flying, therefore we wi Above `__main__`, start a parameter callback function: ```python -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') ``` -The `is_deck_attached` is a global variable which should be defined under `URI`. Note that the value type that the `param_deck_flow()` is a string type, so you will need to convert it to a number first before you can do any operations with it. +The `deck_attached_event` is a global variable which should be defined under `URI`. Note that the value type that the `param_deck_flow()` is a string type, so you will need to convert it to a number first before you can do any operations with it. ```python ... -URI = 'radio://0/80/2M/E7E7E7E7E7' -is_deck_attached = False +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') +deck_attached_event = Event() ``` Try to run the script now, and try to see if it is able to detect that the flowdeck (or any other positioning deck), is correctly attached. Also try to remove it to see if it can detect it missing as well. @@ -83,28 +87,30 @@ Try to run the script now, and try to see if it is able to detect that the flowd This is the full script as we are: ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper -URI = 'radio://0/80/2M/E7E7E7E7E7' +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') @@ -126,17 +132,21 @@ So now we are going to start up the SyncCrazyflie and start a function in the `_ ```python with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: - if is_deck_attached: - take_off_simple(scf) + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + take_off_simple(scf) ``` -See that we are now using `is_deck_attached`? If this is false, the function will not be called and the crazyflie will not take off. +See that we are now using `deck_attached_event.wait()`? If this returns false, the function will not be called and the crazyflie will not take off. Now make the function `take_off_simple(..)` above `__main__`, which will contain the motion commander instance. ```python def take_off_simple(scf): - with MotionCommander(scf) as mc: + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) + mc.stop() ``` If you run the python script, you will see the crazyflie connect and immediately take off. After flying for 3 seconds it will land again. @@ -152,6 +162,7 @@ Change the following line in `take_off_simple(...)`: with MotionCommander(scf) as mc: mc.up(0.3) time.sleep(3) + mc.stop() ``` Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. @@ -172,24 +183,29 @@ Double check if your script is the same as beneath and run it again to check ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) def take_off_simple(scf): with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: time.sleep(3) + mc.stop() + def param_deck_flow(name, value_str): ... @@ -203,8 +219,11 @@ if __name__ == '__main__': cb=param_deck_flow) time.sleep(1) - if is_deck_attached: - take_off_simple(scf) + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + take_off_simple(scf) ``` @@ -244,19 +263,21 @@ Double check if your code code is still correct: ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -is_deck_attached = False - +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -287,8 +308,11 @@ if __name__ == '__main__': cb=param_deck_flow) time.sleep(1) - if is_deck_attached: - move_linear_simple(scf) + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + move_linear_simple(scf) ``` # Step 4: Logging while flying @@ -302,16 +326,18 @@ Let's integrate some logging to this as well. Add the following log config right logconf = LogConfig(name='Position', period_in_ms=10) logconf.add_variable('stateEstimate.x', 'float') logconf.add_variable('stateEstimate.y', 'float') - cf = scf.cf - cf.log.add_config(logconf) + scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) - move_linear_simple(scf) + logconf.start() - logconf.stop() + move_linear_simple(scf) + + logconf.stop() ``` Don't forget to add `from cflib.crazyflie.log import LogConfig` at the imports (we don't need the sync logger since we are going to use the callback). Make the function `log_pos_callback` above param_deck_flow: @@ -330,19 +356,23 @@ Just double check that everything has been implemented correctly and then run th *You can replace the print function in the callback with a plotter if you would like to try that out, like with the python lib matplotlib :)* ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper + +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -387,11 +417,14 @@ if __name__ == '__main__': scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) - move_linear_simple(scf) - logconf.stop() + logconf.start() + + move_linear_simple(scf) + logconf.stop() ``` @@ -439,20 +472,23 @@ You probably also noticed that we are using `mc.start_back()` and `mc.start_forw ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -494,19 +530,22 @@ if __name__ == '__main__': scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() - move_box_limit(scf) - logconf.stop() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + logconf.start() + move_box_limit(scf) + logconf.stop() ``` ## Bouncing in a bounding box Let's take it up a notch! Replace the content in the while loop with the following: ```python - body_x_cmd = 0.2; - body_y_cmd = 0.1; - max_vel = 0.2; + body_x_cmd = 0.2 + body_y_cmd = 0.1 + max_vel = 0.2 while (1): if position_estimate[0] > BOX_LIMIT: @@ -531,20 +570,23 @@ This will now start a linear motion into a certain direction, and makes the Craz Check out if your code still matches the full code and run the script! ```python import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie from cflib.crazyflie.log import LogConfig from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.positioning.motion_commander import MotionCommander +from cflib.utils import uri_helper +URI = uri_helper.uri_from_env(default='radio://0/80/2M/E7E7E7E7E7') -URI = 'radio://0/80/2M/E7E7E7E7E7' DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -590,15 +632,13 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') @@ -617,10 +657,13 @@ if __name__ == '__main__': scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() - move_box_limit(scf) - logconf.stop() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) + + logconf.start() + move_box_limit(scf) + logconf.stop() ``` You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py index d93507f37..9e3b420cb 100644 --- a/examples/step-by-step/sbs_motion_commander.py +++ b/examples/step-by-step/sbs_motion_commander.py @@ -22,7 +22,9 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import logging +import sys import time +from threading import Event import cflib.crtp from cflib.crazyflie import Crazyflie @@ -36,7 +38,7 @@ DEFAULT_HEIGHT = 0.5 BOX_LIMIT = 0.5 -is_deck_attached = False +deck_attached_event = Event() logging.basicConfig(level=logging.ERROR) @@ -94,15 +96,13 @@ def log_pos_callback(timestamp, data, logconf): position_estimate[1] = data['stateEstimate.y'] -def param_deck_flow(name, value_str): +def param_deck_flow(_, value_str): value = int(value_str) print(value) - global is_deck_attached if value: - is_deck_attached = True + deck_attached_event.set() print('Deck is attached!') else: - is_deck_attached = False print('Deck is NOT attached!') @@ -121,10 +121,13 @@ def param_deck_flow(name, value_str): scf.cf.log.add_config(logconf) logconf.data_received_cb.add_callback(log_pos_callback) - if is_deck_attached: - logconf.start() + if not deck_attached_event.wait(timeout=5): + print('No flow deck detected!') + sys.exit(1) - # take_off_simple(scf) - # move_linear_simple(scf) - # move_box_limit(scf) - # logconf.stop() + logconf.start() + + # take_off_simple(scf) + # move_linear_simple(scf) + # move_box_limit(scf) + # logconf.stop() diff --git a/examples/tuning/PID_controller_tuner.py b/examples/tuning/PID_controller_tuner.py index 5e575402f..449d68639 100644 --- a/examples/tuning/PID_controller_tuner.py +++ b/examples/tuning/PID_controller_tuner.py @@ -152,7 +152,10 @@ def __init__(self, pid_gui, scf): self.cf.param.add_update_callback( group='posCtlPid', name='zKd', cb=self.param_updated_callback_Kd) self.cf.param.add_update_callback( - group='posCtlPid', name='xyVelMax', + group='posCtlPid', name='xVelMax', + cb=self.param_updated_callback_vMax) + self.cf.param.add_update_callback( + group='posCtlPid', name='yVelMax', cb=self.param_updated_callback_vMax) self.current_value_kp = 0 @@ -163,7 +166,8 @@ def __init__(self, pid_gui, scf): self.cf.param.request_param_update('posCtlPid.zKp') self.cf.param.request_param_update('posCtlPid.zKi') self.cf.param.request_param_update('posCtlPid.zKd') - self.cf.param.request_param_update('posCtlPid.xyVelMax') + self.cf.param.request_param_update('posCtlPid.xVelMax') + self.cf.param.request_param_update('posCtlPid.yVelMax') time.sleep(0.1) @@ -194,7 +198,8 @@ def send_pid_gains(self): 'Ki', self.pid_gui.scale_Ki.get()) cf.param.set_value(self.unit_choice+'CtlPid.'+self.axis_choice + 'Kd', self.pid_gui.scale_Kd.get()) - cf.param.set_value('posCtlPid.xyVelMax', self.pid_gui.scale_vMax.get()) + cf.param.set_value('posCtlPid.xVelMax', self.pid_gui.scale_vMax.get()) + cf.param.set_value('posCtlPid.yVelMax', self.pid_gui.scale_vMax.get()) time.sleep(0.1)