Skip to content

Commit

Permalink
Merge pull request #315 from bitcraze/jonasdn/314
Browse files Browse the repository at this point in the history 
Fix errors in examples
  • Loading branch information
@knmcguire
knmcguire authored Jan 27, 2022
2 parents bab55f9 + ee2f0d8 commit 358de04
Show file tree
Hide file tree
Showing 3 changed files with 124 additions and 73 deletions.
161 changes: 102 additions & 59 deletions docs/user-guides/sbs_motion_commander.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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__':

Expand Down Expand Up @@ -58,53 +64,53 @@ 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.

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!')


Expand All @@ -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.
Expand All @@ -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.
Expand All @@ -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):
...
Expand All @@ -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)

```

Expand Down Expand Up @@ -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)


Expand Down Expand Up @@ -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
Expand All @@ -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:
Expand All @@ -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)

Expand Down Expand Up @@ -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()


```
Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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:
Expand All @@ -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)

Expand Down Expand Up @@ -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!')


Expand All @@ -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.
Expand Down
Loading

0 comments on commit 358de04

Please sign in to comment.