diff --git a/docs/_data/menu.yml b/docs/_data/menu.yml index 8cc9f204e..fff7eef72 100644 --- a/docs/_data/menu.yml +++ b/docs/_data/menu.yml @@ -6,6 +6,8 @@ - title: User guides subs: - {page_id: python_api} + - {page_id: sbs_connect_log_param} + - {page_id: sbs_motion_commander} - title: Functional areas subs: - {page_id: crazyradio_lib} diff --git a/docs/user-guides/sbs_connect_log_param.md b/docs/user-guides/sbs_connect_log_param.md index bf48de01c..b1d53fac8 100644 --- a/docs/user-guides/sbs_connect_log_param.md +++ b/docs/user-guides/sbs_connect_log_param.md @@ -1,9 +1,10 @@ --- -title: Connecting, logging and parameters +title: "Step-by-Step: Connecting, logging and parameters" page_id: sbs_connect_log_param +redirect: /step-by-step/connect_log_param/ --- -On this step by step guide we will show you how to connect to your crazyflie through the crazyflie python library by a python script. This is the starting point for developing for the crazyflie for off-board enabled flight. +On this step by step guide we will show you how to connect to your Crazyflie through the Crazyflie python library by a python script. This is the starting point for developing for the Crazyflie for off-board enabled flight. # Prerequisites @@ -48,7 +49,7 @@ from cflib.crazyflie import Crazyflie from cflib.crazyflie.syncCrazyflie import SyncCrazyflie ``` -* The cflib.crtp module is for scanning for crazyflies instances. +* The cflib.crtp module is for scanning for Crazyflies instances. * The Crazyflie class is used to easily connect/send/receive data from a Crazyflie. * The synCrazyflie class is a wrapper around the "normal" Crazyflie @@ -64,7 +65,7 @@ After these imports, start the script with: uri = 'radio://0/80/2M/E7E7E7E7E7' ``` -This is the radio uri of the crazyflie, and currently displaying the default. It should be probably fine but if you do not know what the uri of your crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) +This is the radio uri of the crazyflie, and currently displaying the default. It should be probably fine but if you do not know what the uri of your Crazyfie is you can check that with an usb cable and looking at the config ([here](https://www.bitcraze.io/documentation/repository/crazyflie-clients-python/master/userguides/userguide_client/#firmware-configuration) are the instructions) ## Main @@ -111,12 +112,12 @@ Now I will disconnect :'( ``` -The script connected with your crazyflie, synced and disconnected after a few seconds. You can see that the M3 LED is flashing yellow, which means that the crazyflie is connected to the script, but as soon as it leaves the `simple_connect()` function, the LED turns of. The Crazyflie is no longer connected +The script connected with your Crazyflie, synced and disconnected after a few seconds. You can see that the M4 LED is flashing yellow, which means that the Crazyflie is connected to the script, but as soon as it leaves the `simple_connect()` function, the LED turns of. The Crazyflie is no longer connected Not super exciting stuff yet but it is a great start! It is also a good test if everything is correctly configured on your system. -If you are getting an error, retrace your steps or check if your code matches the entire code underneath here. Also make sure your crazyflie is on and your crazyradio PA connected to you computer, and that the crazyflie is not connected to anything else (like the cfclient). If everything is peachy, please continue to the next part! +If you are getting an error, retrace your steps or check if your code matches the entire code underneath here. Also make sure your Crazyflie is on and your crazyradio PA connected to you computer, and that the Crazyflie is not connected to anything else (like the cfclient). If everything is peachy, please continue to the next part! ``` import logging @@ -191,7 +192,7 @@ Now we are going to define the logging configuration. So add `lg_stab` in the `_ ``` -Here you will add the logs variables you would like to read out. If you are unsure how your variable is called, this can be checked by connecting to crazyflie to the cfclient and look at the log TOC tab. If the variables don't match, you get a `KeyError` (more on that later.) +Here you will add the logs variables you would like to read out. If you are unsure how your variable is called, this can be checked by connecting to Crazyflie to the cfclient and look at the log TOC tab. If the variables don't match, you get a `KeyError` (more on that later.) ## Make the logging function @@ -288,7 +289,7 @@ Here we will explain how this asynchronous logging can be set up in the script. ## Start a new function -Above `simple_log(..)`, begin a new function: +Above `def simple_log(..)`, begin a new function: ``` def simple_log_async(scf, logconf): @@ -296,7 +297,7 @@ def simple_log_async(scf, logconf): cf.log.add_config(logconf) ``` -Here you add the logging configuration to to the logging framework of the crazyflie. It will check if the log configuration is part of the TOC, which is a list of all the logging variables defined in the crazyflie. You can test this out by changing one of the `lg_stab` variables to a completely bogus name like `'not.real'`. In this case you would receive the following message: +Here you add the logging configuration to to the logging framework of the Crazyflie. It will check if the log configuration is part of the TOC, which is a list of all the logging variables defined in the Crazyflie. You can test this out by changing one of the `lg_stab` variables to a completely bogus name like `'not.real'`. In this case you would receive the following message: `KeyError: 'Variable not.real not in TOC'` @@ -408,9 +409,12 @@ Then add the following to the `def simple_param_async(...)` function: ``` cf.param.add_update_callback(group=groupstr, name=namestr, cb=param_stab_est_callback) + time.sleep(1) + ``` +The sleep function is to give the script a bit more time to wait for the Crazyflies response and not lose the connection immediatly. -If you would like to test out the script now already, replace `simple_log_async(...)` with `simple_param_async(group,name)` and runt the script. You can see that it will print out the variable name and value: +If you would like to test out the script now already, replace `simple_log_async(...)` with `simple_param_async(scf, group, name)` and run the script. You can see that it will print out the variable name and value: `The crazyflie has parameter stabilizer.estimator set at number: 1` @@ -419,14 +423,13 @@ If you would like to test out the script now already, replace `simple_log_async( Now we will set a variable in a parameter. Add the following to the `simple_param_async(...)` function: ``` - time.sleep(1) cf.param.set_value(full_name,2) ``` If you would run the script now you will also get this message: `The crazyflie has parameter stabilizer.estimator set at number: 2` -This means that the crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. +This means that the Crazyflie has changed the parameter value to 2, which is another methods it uses for state estimation. This can also be done to change the color on the ledring, or to initiate the highlevel commander. What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) parameters, which can be checked by the parameter TOC in the CFclient. You can check this by changing the parameter name to group `'CPU' ` and name `flash'`. Then you will get the following error: @@ -434,15 +437,77 @@ What it can't do is to set a Read Only (RO) parameter, only Read Write (RW) para ## Finishing and running the script -It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the crazyflie, the parameter will still be set. Only after physcially restarting the crazyflie the parameter will reset to its default setting as defined in the firmware. +It is usually good practice to put the parameter setting back to where it came from, since after disconnecting the Crazyflie, the parameter will still be set. Only after physcially restarting the Crazyflie the parameter will reset to its default setting as defined in the firmware. So finish the `simple_param_async(...)` function by adding the next few lines: ``` - time.sleep(1) cf.param.set_value(full_name,1) time.sleep(1) ``` -Make sure the right function is in `__main__`. Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: +Make sure the right function is in `__main__`. Check if your script corresponds with the code: + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.log import LogConfig +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.crazyflie.syncLogger import SyncLogger + +# URI to the Crazyflie to connect to +uri = 'radio://0/80/2M/E7E7E7E7E7' + +# Only output errors from the logging framework +logging.basicConfig(level=logging.ERROR) + + +def param_stab_est_callback(name, value): + print('The crazyflie has parameter ' + name + ' set at number: ' + value) + + +def simple_param_async(scf, groupstr, namestr): + cf = scf.cf + full_name = groupstr + '.' + namestr + + cf.param.add_update_callback(group=groupstr, name=namestr, + cb=param_stab_est_callback) + time.sleep(1) + cf.param.set_value(full_name, 2) + time.sleep(1) + cf.param.set_value(full_name, 1) + time.sleep(1) + + +def log_stab_callback(timestamp, data, logconf): + ... +def simple_log_async(scf, logconf): + ... +def simple_log(scf, logconf): + ... +def simple_connect(): + ... + +if __name__ == '__main__': + # Initialize the low-level drivers (don't list the debug drivers) + cflib.crtp.init_drivers(enable_debug_driver=False) + + lg_stab = LogConfig(name='Stabilizer', period_in_ms=10) + lg_stab.add_variable('stabilizer.roll', 'float') + lg_stab.add_variable('stabilizer.pitch', 'float') + lg_stab.add_variable('stabilizer.yaw', 'float') + + group = 'stabilizer' + name = 'estimator' + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + simple_param_async(scf, group, name) +``` + + + +Run the script with `python3 connect_log_param.py` in a terminal and you should see the following: ``` Connecting to radio://0/80/2M/E7E7E7E7E7 @@ -452,6 +517,12 @@ The crazyflie has parameter stabilizer.estimator set at number: 2 The crazyflie has parameter stabilizer.estimator set at number: 1 ``` -# Finished and What's next? +You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. + + +# What's next? + + + Now you know how to connect to the Crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the Crazyflie fly by giving it setpoints which is one step closer to making your own application! -You're done and now you know how to connect to the crazyflie and how to retrieve the parameters and logging variables through a python script. The next step is to make the crazyflie fly by giving it setpoints which is one step closer to making your own application! + Go to the [next tutorial](/user-guides/sbs_motion_commander/) about the motion commander. diff --git a/docs/user-guides/sbs_motion_commander.md b/docs/user-guides/sbs_motion_commander.md new file mode 100644 index 000000000..d2b573f30 --- /dev/null +++ b/docs/user-guides/sbs_motion_commander.md @@ -0,0 +1,516 @@ +--- +title: "Step-by-Step: Motion Commander" +page_id: sbs_motion_commander +--- + +Here we will go through step-by-step how to make your crazyflie move based on a motion script. For the first part of this tutorial, you just need the crazyflie and the flowdeck. For the second part, it would be handy to have the multiranger present. + +# Prerequisites + +We will assume that you already know this before you start with the tutorial: + +* Some basic experience with python +* Followed the [crazyflie getting started guide](https://www.bitcraze.io/documentation/tutorials/getting-started-with-crazyflie-2-x/) and [the connecting, logging and parameters tutorial](/user-guides/sbs_connect_log_param/). + + +# Get the script started + +Since you should have installed cflib in the previous step by step tutorial, you are all ready to got now. Open up an new python script called `motion_flying.py`. First you will start by adding the following import to the script: + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + +URI = 'radio://0/80/2M/E7E7E7E7E7' + +logging.basicConfig(level=logging.ERROR) + +if __name__ == '__main__': + + cflib.crtp.init_drivers(enable_debug_driver=False) + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: +``` + +This probably all looks pretty familiar, except for one thing line, namely: + +`from cflib.positioning.motion_commander import MotionCommander` + +This imports the motion commander, which is pretty much a wrapper around the position setpoint frame work of the crazyflie. You probably have unknowingly experienced this a when trying out the assist modes in this [tutorial with the flowdeck in the cfclient](https://www.bitcraze.io/documentation/tutorials/getting-started-with-flow-deck/) + +# Step 1: Security before flying + +Since this tutorial won't be a table top tutorial like last time, but an actual flying one, we need to put some securities in place. The flowdeck or any other positioning deck that you are using, should be correctly attached to the crazyflie. If it is not, it will try to fly anyway without a good position estimate and for sure is going to crash. + +We want to know if the deck is correctly attached before flying, therefore we will add a callback for the `"deck.bcFlow2"` parameter. Add the following line after the `...init_drivers(...)` in `__main__` +``` + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) +``` + +Above `__main__`, start a parameter callback function: +``` +def param_deck_flow(name, value): + if value == 1: + is_deck_attached = True + 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` + +``` +... +URI = 'radio://0/80/2M/E7E7E7E7E7' +is_deck_attached = False +``` + +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. + +This is the full script as we are: +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +def param_deck_flow(name, value): + global is_deck_attached + + print(value) + + if value: + is_deck_attached = True + print("Deck is attached!") + else: + is_deck_attached = False + print("Deck is NOT attached!") + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: +``` + +# Step 2: Take off function + +So now we are going to start up the SyncCrazyflie and start a function in the `__main__` function: + +``` + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + if is_deck_attached: + 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. + +Now make the function `take_off_simple(..)` above `__main__`, which will contain the motion commander instance. + +``` +def take_off_simple(scf): + with MotionCommander(scf) as mc: + time.sleep(3) +``` + +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. + +The reason for the crazyflie to immediately take off, is that the motion commander if intialized with a take off function that will already start sending position setpoints to the crazyflie. Once the script goes out of the instance, the motion commander instance will close with a land function. + +## Changing the height + +Currently the motion commander had 0.3 meters height as default but this can ofcourse be changed. + +Change the following line in `take_off_simple(...)`: +``` + with MotionCommander(scf) as mc: + mc.up(0.3) + time.sleep(3) +``` + +Run the script again. The crazyflie will first take off to 0.3 meters and then goes up for another 0.3 meters. + +The same can be achieved by adjusting the default_height of the motion_commander, which is what we will do for now on in this tutorial. Remove the `mc.up(0.3)` and replace the motion commander line with +``` + with MotionCommander(scf, default_height = DEFAULT_HEIGHT) as mc: +``` + +Add the variable underneath `URI`: +``` +DEFAULT_HEIGHT = 0.6 +``` + + +Double check if your script is the same as beneath and run it again to check + + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 + +is_deck_attached = False +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): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + if is_deck_attached: + take_off_simple(scf) +``` + +# Step 3 Go Forward, Turn and Go back + +So now we know how to take off, so the second step is to move in a direction! Start a new function above `def take_off_simple(scf)`: + +``` +def move_linear_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.back(0.5) + time.sleep(1) +``` + +If you replace `take_off_simple(scf)` in `__main__` with `move_linear_simple(scf)`, try to run the script. + +You will see the crazyflie take off, fly 0.5 m forward, fly backwards and land again. + +Now we are going to add a turn into it. Replace the content under motion commander in `move_linear_simple(..)` with the following: + +``` + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) +``` + +Try to run the script again. Now you can see the crazyflie take off, go forward, turn 180 degrees and go forward again to its initial position. The `mc.back()` needed to be replaced with the forward since the motion commander sends the velocity setpoints in the __body fixed coordinated__ system. This means that the commands forward will go forward to whereever the current heading (the front) of the crazyflie points to. + +Double check if your code code is still correct: + +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +is_deck_attached = False +DEFAULT_HEIGHT = 0.5 +logging.basicConfig(level=logging.ERROR) + +def move_linear_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + +def take_off_simple(scf): + ... +def param_deck_flow(name, value): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + if is_deck_attached: + move_linear_simple(scf) +``` + +# Step 4: Logging while flying + +When the motion commander commands have been executed, the script stops and the crazyflie lands... however that is a bit boring. Maybe you would like for it to keep flying and responding certain elements in the mean time! + +Let's integrate some logging to this as well. Add the following log config right into `__main__` under `SyncCrazyflie` + + +``` + lg_stab = LogConfig(name='Position', period_in_ms=10) + lg_stab.add_variable('stateEstimate.x', 'float') + lg_stab.add_variable('stateEstimate.y', 'float') + cf = scf.cf + cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + + 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: + + + +``` +def log_pos_callback(timestamp, data, logconf): + print(data) +``` + +Make global variable which is a list called `position_estimate` and fill this in in the logging callback function with the x and y position. The `data` is a dict. + +Just double check that everything has been implemented correctly and then run the script. You will see the same behavior as with the previous step but then with the position estimated printed at the same time. + +*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 :)* +``` +... +from cflib.crazyflie.log import LogConfig + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) +position_estimate = [0,0] + + +def move_linear_simple(scf): + ... + +def take_off_simple(scf): + ... + +def log_pos_callback(timestamp, data, logconf): + print(data) + global postion_estimate + + position_estimate[0] = data["stateEstimate.x"] + position_estimate[1] = data["stateEstimate.y"] + +def param_deck_flow(name, value): +... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + scf.cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + + move_linear_simple(scf) + + logconf.stop() + +``` + +# Step 5: Combine logging and motion commander + +There is a reason why we put the position_estimate to catch the positions from the log, since we would like to now do something with it! + +## Back and forth with limits +Lets start with a new function above `move_in_box_limit(scf)`: + +``` +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + while (1): + + time.sleep(0.1) + +``` + +If you would run this (don't forget to replace `move_linear_simple()` in `__main`), you see the crazyflie take off but it will stay in the air. A keyboard interrupt (ctrl+c) will stop the script and make the crazyflie land again. + +Now we will add some behavior in the while loop: + +``` +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + start_forward() + + while (1): + if position_estimate[0] > BOX_LIMIT: + mc.start_back() + elif position_estimate[0] < -BOX_LIMIT + mc.start_forward() + time.sleep(0.1) + +``` + +Add `BOX_LIMIT = 0.5` underneath the definition of the `DEFAULT_HEIGHT = 0.5`. + +Run the script and you will see that the crazyflie will start moving back and forth until you hit ctrl+c. It changes its command based on the logging input, which is the state estimate x and y position. Once it indicates that it reached the border of the 'virtual' limit, it will change it's direction. + +You probably also noticed that we are using `mc.start_back()` and `mc.start_forward()` instead of the `mc.forward(0.5)` and `mc.back(0.5)` used in the previous steps. The main difference is that the *mc.forward* and *mc.back* are **blocking** functions that won't continue the code until the distance has been reached. The *mc.start_...()* will start the crazyflie in a direction and will not stop until the `mc.stop()` is given, which is given automatically when the motion commander instance is exited. That is why this is nice functions to use in reactive scenarios like these. + +## Bouncing in a bounding box + +Let's take it up a notch! Replace the content in the while loop with the following: +``` + body_x_cmd = 0.2; + body_y_cmd = 0.1; + max_vel = 0.2; + + while (1): + if position_estimate[0] > BOX_LIMIT: + body_x_cmd=-max_vel + elif position_estimate[0] < -BOX_LIMIT: + body_x_cmd=max_vel + if position_estimate[1] > BOX_LIMIT: + body_y_cmd=-max_vel + elif position_estimate[1] < -BOX_LIMIT: + body_y_cmd=max_vel + + mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) + + time.sleep(0.1) +``` + +This will now start a linear motion into a certain direction, and makes the Crazyflie bounce around in a virtual box of which the size is indicated by 'BOX_LIMIT'. So before you fly make sure that you pick a box_limit small enough so that it able to fit in your flying area. + +**Note**: if you are using the flowdeck, it might be that the orientation of this box will seem to change. This is due to that the flowdeck is not able to provide an absolute heading estimate, which will be only based on gyroscope measurements. This will drift over time, which is accelerated if you incorperate many turns in your application. There are also reports that happens quickly when the crazyflie is still on the ground. This should not happen with MoCap or the lighthouse deck. + + +Check out if your code still matches and run the script! +``` +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander +from cflib.crazyflie.log import LogConfig + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +position_estimate = [0,0] + +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + body_x_cmd = 0.2; + body_y_cmd = 0.1; + max_vel = 0.2; + + while (1): + if position_estimate[0] > BOX_LIMIT: + body_x_cmd=-max_vel + elif position_estimate[0] < -BOX_LIMIT: + body_x_cmd=max_vel + if position_estimate[1] > BOX_LIMIT: + body_y_cmd=-max_vel + elif position_estimate[1] < -BOX_LIMIT: + body_y_cmd=max_vel + + mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) + + time.sleep(0.1) + + + +def move_linear_simple(scf): + ... +def take_off_simple(scf): + ... +def log_pos_callback(timestamp, data, logconf): + ... +def param_deck_flow(name, value): + ... + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + time.sleep(1) + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + 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() + +``` + +You're done! The full code of this tutorial can be found in the example/step-by-step/ folder. + + +# What is next ? + +Now you are able to send velocity commands to the Crazyflie and react upon logging and parameters variables, so one step closer to writing your own application with the Crazyflie python library! Check out the motion_commander_demo.py in the example folder of the cflib if you would like to see what the commander can do. \ No newline at end of file diff --git a/examples/step-by-step/connect_log_param.py b/examples/step-by-step/sbs_connect_log_param.py similarity index 100% rename from examples/step-by-step/connect_log_param.py rename to examples/step-by-step/sbs_connect_log_param.py diff --git a/examples/step-by-step/sbs_motion_commander.py b/examples/step-by-step/sbs_motion_commander.py new file mode 100644 index 000000000..d4f52456c --- /dev/null +++ b/examples/step-by-step/sbs_motion_commander.py @@ -0,0 +1,128 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2017 Bitcraze AB +# +# Crazyflie Nano Quadcopter Client +# +# This program is free software; you can redistribute it and/or +# modify it under the terms of the GNU General Public License +# as published by the Free Software Foundation; either version 2 +# of the License, or (at your option) any later version. +# +# This program is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# You should have received a copy of the GNU General Public License +# along with this program; if not, write to the Free Software +# Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, +# MA 02110-1301, USA. + +import logging +import time + +import cflib.crtp +from cflib.crazyflie import Crazyflie +from cflib.crazyflie.syncCrazyflie import SyncCrazyflie +from cflib.positioning.motion_commander import MotionCommander +from cflib.crazyflie.log import LogConfig + + +URI = 'radio://0/80/2M/E7E7E7E7E7' +DEFAULT_HEIGHT = 0.5 +BOX_LIMIT = 0.5 + +is_deck_attached = False + +logging.basicConfig(level=logging.ERROR) + +position_estimate = [0,0] + +def move_box_limit(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + body_x_cmd = 0.2; + body_y_cmd = 0.1; + max_vel = 0.2; + + while (1): + '''if position_estimate[0] > BOX_LIMIT: + mc.start_back() + elif position_estimate[0] < -BOX_LIMIT + mc.start_forward() + ''' + + if position_estimate[0] > BOX_LIMIT: + body_x_cmd=-max_vel + elif position_estimate[0] < -BOX_LIMIT: + body_x_cmd=max_vel + if position_estimate[1] > BOX_LIMIT: + body_y_cmd=-max_vel + elif position_estimate[1] < -BOX_LIMIT: + body_y_cmd=max_vel + + mc.start_linear_motion(body_x_cmd, body_y_cmd, 0) + + time.sleep(0.1) + + + +def move_linear_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + mc.turn_left(180) + time.sleep(1) + mc.forward(0.5) + time.sleep(1) + +def take_off_simple(scf): + with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc: + time.sleep(3) + mc.stop() + +def log_pos_callback(timestamp, data, logconf): + print(data) + global postion_estimate + position_estimate[0] = data["stateEstimate.x"] + position_estimate[1] = data["stateEstimate.y"] + +def param_deck_flow(name, value): + global is_deck_attached + print(value) + if value: + is_deck_attached = True + print("Deck is attached!") + else: + is_deck_attached = False + print("Deck is NOT attached!") + +if __name__ == '__main__': + cflib.crtp.init_drivers(enable_debug_driver=False) + + + with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf: + + scf.cf.param.add_update_callback(group="deck", name="bcFlow2", + cb=param_deck_flow) + time.sleep(1) + + logconf = LogConfig(name='Position', period_in_ms=10) + logconf.add_variable('stateEstimate.x', 'float') + logconf.add_variable('stateEstimate.y', 'float') + scf.cf.log.add_config(logconf) + logconf.data_received_cb.add_callback(log_pos_callback) + + if is_deck_attached: + logconf.start() + + #take_off_simple(scf) + #move_linear_simple(scf) + move_box_limit(scf) + logconf.stop() \ No newline at end of file