0
$\begingroup$

I'm trying to implement a drone in vertical descend. As for maximum simplification, I maintain all its angles to 0, and just try to get to the my desired descend. However, when I debug, I see that my z_acel variable changes value when it comes to the velocity integration. And obviously cant reach to my desired height, which is 10cm below. Anyone can help?

import numpy as np
import math
import matplotlib.pyplot as plt

from mpmath import *


#***DRONES****

#Dynamic parameters
L=25 
b = 1e-5 
I = np.diag([5e-7, 5e-7, 10e-7]) 
k=3e-5
m=0.5
kd=0.25
#PID Values
Kp, Ki, Kd = 4.05, 0, 20.64 
g=9.80
dt=0.1

#variables inicialization
N=500
phi_acel= angulotheta_acel= psi_acel= [0] * N
phi_vel= angulotheta_vel= psi_vel= [0]*N
phi = angulotheta = psi = [0]*N
z= z_vel= z_acel= [0]*N
Torque=inputs=[0]*4

height_objective=-10 #reach from 0 to -10cm height.

for i in range(2,N):

   #HEIGHT CONTROLLER
   err_height=height_objective-z[i-1]

   Torque[0]=err_height*Kp + err_height*Ki*dt - (z_vel[i-1]-z_vel[i-2])*Kd*dt + (m*g)/(math.cos(phi[i-1])*math.cos(angulotheta[i-1])) 
   if Torque[0]>20:
       Torque[0]=20
   elif Torque[0]<0:
       Torque[0]=0


   z_acel[i]= -g + (math.cos(angulotheta[i-1])*math.cos(phi[i-1]))*Torque[0]/m  #with all the angles zero, z_acel[i] should be -g = -9.8

   z_vel[i]= z_vel[i-1] + z_acel[i]/2*dt   #however, in this step, the z_acel[i] changes the value

   z[i]= z[i-1] + z_vel[i]/2*dt

   plt.plot(i,z[i],'or')
   plt.pause(0.1)
plt.show() 
$\endgroup$
0
$\begingroup$

Ok, i got the answer. When I inicialize the variables, I wrote $z= z_v= z_a= [0]*N, $, even I thought it had no more influence in the code, it has. As it is written now the code, the value of z_vel is the new value for z_acel in every iteration.

| improve this answer | |
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.