import control_functions
# Set Trim & PID Parameters Alt_Kp Alt_Ki Derecha Delante Gira
#control_functions.setPIDValues(0, 0, 128, 128, 128) # Kp_altura, Offsets: Derecha/Izquierda, Delante/Detras, GiroIzquierda/Derecha
# -------------------------------------- Flight -------------------------------------- #
# Takeoff
print('Taking off...')
# Set Trim & PID Parameters Alt_Kp Alt_Ki Derecha Delante Gira
control_functions.setPIDValues(
0, 0, 128, 128, 128) # Kp_altura, Offsets: Der/Izq, Del/Det, GirIzq/Der
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(65, 128, 128, 128, 0.5) # Aprox to 1,04m height
# Flight
print('Fliying...')
# Set Flight Parameters Arriba Derecha Delante Gira Order Duration
# Landing
print('Landing...')
# Set Flight Parameters AA DI DD GID Order Duration
import control_functions
# Set Trim Parameters Arriba Derecha Delante Gira
control_functions.setTrimmValues(105, 128, 128, 128) # Offsets: Up/Down Right/Left Forward/Backward TurnRight/TurnLeft (Cargado)
# -------------------------------------- Takeoff -------------------------------------- #
# Set Flight Parameters AA DI DD GID Order Duration
print('Taking off...')
control_functions.setcontrols(0, 128, 128, 128, 1)
control_functions.setcontrols(55, 128, 128, 145, 0.5)
#control_functions.setcontrols(128, 128, 128, 128, 3)
# -------------------------------------- Flight -------------------------------------- #
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(0, 128, 150, 128, 1)
#control_functions.setcontrols(128, 128, 128, 200, 1.5) # Adelante
#control_functions.setcontrols(150, 128, 50, 128, 1) # Atras
#control_functions.setcontrols(128, 140, 128, 128, 0.5) # Derecha
#control_functions.setcontrols(128, 116, 116, 128, 0.5) # Vuelve
# Takeoff
print('Taking off...')
alt_kp = 39
alt_ki = 3
alt_kd = 20
xy_kp = 28
#xy_kp = 22;
xy_ki = 255
xy_kd = 100
# ------------ ETAPA 1 ------------ # (error*XY_Ki^2)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(75, 4, 100, 40, 2, 50, 128) # Con XY
control_functions.setPIDValues(alt_kp, alt_ki, alt_kd, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(25, 128, 128, 128, 5)
# ------------ ETAPA 2 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(30, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(20, 128, 128, 128, 1)
# ------------ ETAPA 3 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(50, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(30, 128, 128, 128, 4)
# Flight
#print('Fliying...')
# Landing
xy_kd = 50
# Tensados
alt_kp = 90
alt_ki = 3
alt_kd = 125
xy_kp = 11
xy_ki = 255
xy_kd = 50
# --------------------------- TAKE OFF ---------------------------
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(0, alt_ki, 0, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 0.5)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(alt_kp, alt_ki, alt_kd, xy_kp, xy_ki, xy_kd,
128) # Con XY
control_functions.setcontrols(20, 128, 128, 128, 2.5)
# --------------------------- FLIGHT ---------------------------
# Avanza
print('Flight...')
flight_Delante = 240
flight_Derecha = 128
flight_GiroDer = 128
# Set Flight Parameters AA DI DD GID Duration Steps
control_functions.settrace(20, flight_Derecha, flight_Delante, flight_GiroDer,
4.0, 20)
print('Stop...')
import control_functions
# Set Trim & PID Parameters Alt_Kp Alt_Ki Derecha Delante Gira
#control_functions.setPIDValues(0, 0, 128, 128, 128) # Kp_altura, Offsets: Derecha/Izquierda, Delante/Detras, GiroIzquierda/Derecha
# -------------------------------------- Flight -------------------------------------- #
print('STOP...')
control_functions.setPIDValues(
50, 16, 0, 128, 122, 128
) # Kp_altura, Ki_altura, Kd_altura, Offsets: Der/Izq, Del/Det, GirIzq/Der
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(12, 128, 128, 128, 2) # Aprox to 1,04m height
control_functions.setcontrols(0, 128, 128, 128, 0.5) # Aprox to 1,04m height
import control_functions
# -------------------------------------- Flight -------------------------------------- #
# Takeoff
print('Taking off...')
# ------------ ETAPA 1 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(64, 4, 50, 40, 0, 0, 128) # Con XY
#control_functions.setPIDValues(64, 4, 0, 0, 0, 0, 128) # Sin XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 2.5)
# ------------ ETAPA 2 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(30, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(20, 128, 128, 128, 1)
# ------------ ETAPA 3 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(50, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(30, 128, 128, 128, 4)
# Flight
#print('Fliying...')
# Landing
print('Landing...')
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(40, 4, 8, 30, 0, 0, 128) # Con XY
#control_functions.setPIDValues(64, 4, 0, 0, 0, 0, 128) # Sin XY
# Set Flight Parameters AA DI DD GID Order Duration
import control_functions
# Set Trim Parameters Arriba Derecha Delante Gira
control_functions.setTrimmValues(138, 133, 126, 125) # Offsets: Up/Down Right/Left Forward/Backward TurnRight/TurnLeft (Cargado)
#control_functions.setTrimmValues(136, 134, 127, 125) # Offsets: Up/Down Right/Left Forward/Backward TurnRight/TurnLeft (Cargado)
# -------------------------------------- Takeoff -------------------------------------- #
# Set Flight Parameters AA DI DD GID Order Duration
print('Taking off...')
control_functions.setcontrols(0, 128, 128, 128, 1)
control_functions.setcontrols(80, 128, 128, 128, 1.5)
# -------------------------------------- Flight -------------------------------------- #
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(75, 128, 128, 128, 1.5)
control_functions.setcontrols(75, 133, 126, 128, 1.5)
control_functions.setcontrols(75, 132, 126, 128, 1.5)
# -------------------------------------- Landing -------------------------------------- #
# Set Flight Parameters AA DI DD GID Order Duration
print('Landing...')
xy_kd = 50
# Bajo ensayo
alt_kp = 50
alt_ki = 4
alt_kd = 120
xy_kp = 12
xy_ki = 255
xy_kd = 50
# -------------------------------------- TAKE OFF -------------------------------------- #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(0, alt_ki, 0, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 0.5)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(alt_kp, alt_ki, 0, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 1)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(alt_kp, alt_ki, alt_kd, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 0.1)
# -------------------------------------- FLIGHT --------------------------------------
print('Flight...')
# Espera por teclado
xy_kd = 50
# Mas tenso
alt_kp = 90
alt_ki = 3
alt_kd = 120
xy_kp = 12
xy_ki = 255
xy_kd = 50
# -------------------------------------- TAKE OFF -------------------------------------- #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(0, alt_ki, 0, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(25, 128, 128, 128, 0.5)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(alt_kp, alt_ki, alt_kd, xy_kp, xy_ki, xy_kd,
128) # Con XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 1)
# -------------------------------------- FLIGHT --------------------------------------
print('Flight...')
# Espera por teclado
press = 0
while press == 0:
if msvcrt.kbhit():
key = msvcrt.getch()
press = 1
flight_Delante = 128
import control_functions
# -------------------------------------- Flight -------------------------------------- #
# Takeoff
print('Taking off...')
# ------------ ETAPA 1 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(20, 8, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 1)
# ------------ ETAPA 2 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(30, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 1)
# ------------ ETAPA 3 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(50, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(30, 128, 128, 128, 4)
# Flight
#print('Fliying...')
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(40, 16, 0, 30, 0, 0, 128)
import control_functions
# -------------------------------------- Flight -------------------------------------- #
# Takeoff
print('Taking off...')
#xy_kp = 35;
# ------------ ETAPA 1 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setPIDValues(75, 4, 100, 40, 0, 100, 128) # Con XY
#control_functions.setPIDValues(64, 4, 0, 0, 0, 0, 128) # Sin XY
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(22, 128, 128, 128, 5)
# ------------ ETAPA 2 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(30, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(20, 128, 128, 128, 1)
# ------------ ETAPA 3 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
#control_functions.setPIDValues(50, 16, 0, 32, 0, 0, 128)
# Set Flight Parameters AA DI DD GID Order Duration
#control_functions.setcontrols(30, 128, 128, 128, 4)
# Flight
#print('Fliying...')
# Landing
print('Landing...')
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
control_functions.setcontrols(15, 128, 128, 128, 0.8)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd XY_Kp XY_Ki XY_Kd Giro
import control_functions
# Set Trim Parameters Arriba Derecha Delante Gira
#control_functions.setTrimmValues(115, 133, 132, 128) # Offsets: Up/Down Right/Left Forward/Backward TurnRight/TurnLeft (Descargado)
control_functions.setTrimmValues(
100, 134, 132, 128
) # Offsets: Up/Down Right/Left Forward/Backward TurnRight/TurnLeft (Cargado)
# -------------------------------------- Flight -------------------------------------- #
# Takeoff
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(0, 128, 128, 128, 1)
control_functions.setcontrols(220, 128, 128, 128, 0.3)
#control_functions.setcontrols(128, 128, 128, 128, 3)
# Flight
# Set Flight Parameters Arriba Derecha Delante Gira Order Duration
control_functions.setcontrols(128, 128, 180, 128, 1) # Adelante
control_functions.setcontrols(150, 128, 50, 128, 1) # Atras
#control_functions.setcontrols(128, 140, 128, 128, 0.5) # Derecha
#control_functions.setcontrols(128, 116, 116, 128, 0.5) # Vuelve
# Landing
# Set Flight Parameters AA DI DD GID Order Duration
print('Landing...')
# Set Flight Parameters Arriba Derecha Delante Gira Order Duration
control_functions.setcontrols(118, 128, 150, 128, 1.5)
control_functions.setcontrols(0, 128, 128, 128, 0.1)
# Set Trim & PID Parameters Alt_Kp Alt_Ki Derecha Delante Gira
#control_functions.setPIDValues(0, 0, 128, 128, 128) # Kp_altura, Offsets: Derecha/Izquierda, Delante/Detras, GiroIzquierda/Derecha
# -------------------------------------- Flight -------------------------------------- #
# Takeoff
print('Taking off...')
# ------------ ETAPA 1 ------------ #
# Set Trim & PID Parameters Alt_Kp Alt_Ki Alt_Kd Derecha Delante Gira
control_functions.setPIDValues(20, 8, 0, 130, 125, 128) # Kp_altura, Ki_altura, Kd_altura, Offsets: Der/Izq, Del/Det, GirIzq/Der
# Set Flight Parameters AA DI DD GID Order Duration
control_functions.setcontrols(20, 128, 128, 128, 1) #
# ------------ ETAPA 2 ------------ #
control_functions.setPIDValues(40, 16, 0, 130, 125, 128) # Kp_altura, Ki_altura, Kd_altura, Offsets: Der/Izq, Del/Det, GirIzq/Der
control_functions.setcontrols(20, 128, 128, 128, 1) # AA DI DD GID Order Duration
# ------------ ETAPA 3 ------------ #
control_functions.setPIDValues(40, 16, 0, 130, 125, 128) # Kp_altura, Ki_altura, Kd_altura, Offsets: Der/Izq, Del/Det, GirIzq/Der
control_functions.setcontrols(30, 128, 128, 128, 1) # AA DI DD GID Order Duration
# Flight
#print('Fliying...')
# ------------ ETAPA 2 ------------ #
#control_functions.setPIDValues(80, 16, 0, 130, 125, 128) # Kp_altura, Ki_altura, Kd_altura, Offsets: Der/Izq, Del/Det, GirIzq/Der
#control_functions.setcontrols(30, 128, 128, 128, 2) # AA DI DD GID Order Duration