159 lines
3.5 KiB
Python
159 lines
3.5 KiB
Python
from typing import Callable
|
|
from tello_sim.tello_sim_client import TelloSimClient
|
|
|
|
|
|
import time
|
|
|
|
SPEED_SETTING_CM_S = 30
|
|
MOVEMENT_MAGNITUDE = 30
|
|
TIME_PER_ACTION_SECS = 3
|
|
|
|
# Create a Tello instance
|
|
tello = TelloSimClient()
|
|
|
|
# Connect to Tello
|
|
tello.connect()
|
|
|
|
print("Starting flying in ...")
|
|
for i in range(3, 0, -1):
|
|
print(i)
|
|
time.sleep(1)
|
|
|
|
# Takeoff
|
|
print("Take off")
|
|
tello.takeoff()
|
|
|
|
print("Setting speed to", SPEED_SETTING_CM_S)
|
|
tello.set_speed(SPEED_SETTING_CM_S)
|
|
|
|
|
|
def do_action_for_time(label: str, action: Callable, time_in_seconds: int):
|
|
print(label)
|
|
action()
|
|
time.sleep(time_in_seconds)
|
|
|
|
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = 0
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Staying still",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Move forward
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = MOVEMENT_MAGNITUDE
|
|
up_down_velocity = 0
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Moving Forward",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Move Backwards
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = -MOVEMENT_MAGNITUDE
|
|
up_down_velocity = 0
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Moving backwards",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
|
|
# Move Left
|
|
left_right_velocity = MOVEMENT_MAGNITUDE
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = 0
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Moving left",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Move Right
|
|
left_right_velocity = -MOVEMENT_MAGNITUDE
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = 0
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Moving right",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Move Up
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = MOVEMENT_MAGNITUDE
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Moving up",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Move down
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = -MOVEMENT_MAGNITUDE
|
|
yaw_velocity = 0
|
|
do_action_for_time(
|
|
"Moving down",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Turn Left
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = 0
|
|
yaw_velocity = MOVEMENT_MAGNITUDE
|
|
do_action_for_time(
|
|
"Turning left",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
# Turn Right
|
|
left_right_velocity = 0
|
|
forward_backward_velocity = 0
|
|
up_down_velocity = 0
|
|
yaw_velocity = -MOVEMENT_MAGNITUDE
|
|
do_action_for_time(
|
|
"Turning right",
|
|
lambda: tello.send_rc_control(
|
|
left_right_velocity, forward_backward_velocity, up_down_velocity, yaw_velocity
|
|
),
|
|
TIME_PER_ACTION_SECS,
|
|
)
|
|
|
|
print("Landing")
|
|
# Land
|
|
tello.land()
|
|
|
|
# End the connection
|
|
tello.end()
|