Commit 4f7429bb authored by Thomas Muller's avatar Thomas Muller
Browse files

Replace main.py

parent 1296ed03
......@@ -16,78 +16,10 @@ import serial
from Detection_elements_image import *
from Resolution_laby import *
from tracker import *
from Asservissement import *
mask = cv2.imread("Images/mask2.png")
#############################################################
### PARAMETRES MODELE ET PERFORMANCES ASSERVISSEMENT
kb = 6.1; # Parametre de la modelisation du systeme
Te = 0.1; # Periode d'echantillonnage
alpha = 0.5; # Double zero polynome C(z) = (z-alpha)**2 (D(z) = C(z)*P(z))
tps_rep = 1.3; # Temps de reponse
tau = tps_rep/5.0; # Constante de temps tau
z1 = np.exp(-Te/tau); # Double zero polynome P(z) = (z-z1)**2 (double pole de la boucle fermee)
### COEFFICIENTS POLYNOMES
K = kb*Te*Te/2.0; # Gain du systeme echantillone
# Coefs polynome S
s0 = 1.0;
s1 = 2.0 - 2.0*(alpha + z1);
s2 = (1.0/4.0)*(z1*z1+4.0*z1*alpha+alpha*alpha-1.0+6.0*(1-alpha-z1)+2.0*z1*alpha*(alpha+z1)+z1*z1*alpha*alpha);
# Coefs polynome R
r0 = (1.0/K)*(-2.0*z1*alpha*(alpha+z1)-2.0*(1-alpha-z1)+3.0*s2-z1*z1*alpha*alpha);
r1 = (1.0/K)*(z1*z1*alpha*alpha-s2);
Q = (r0 + r1)/((1-alpha)*(1-alpha));
### PARAMETRES SIMULATION
nb_pas = 3; # Nombre de pas (k) simules
simuX = np.zeros((3, nb_pas)); # ligne 0 : yc, ligne 1 : u et ligne 2 : y
simuY = np.zeros((3, nb_pas)); # ligne 0 : yc, ligne 1 : u et ligne 2 : y
#[[yc[k-2],yc[k-1],y[k]],
# [u[k-2], u[k-1], u[k]],
# [y[k-2], y[k-1], y[k]]]
### FONCTIONS DE CALCUL DES ECHANTILLONS TEMPORELS
def u(k, simu, saturation):
commande = ((-s1*simu[1,k-1] - s2*simu[1,k-2] + Q*simu[0,k] - 2*alpha*Q*simu[0,k-1] + Q*alpha*alpha*simu[0,k-2] - r0*simu[2,k-1] - r1*simu[2,k-2])/s0)
if(commande > saturation):
return saturation
elif (commande < -saturation):
return -saturation
else:
return commande
def y1(k, simu):
return (2*z1*simu[2,k-1] - z1*z1*simu[2,k-2] + Q*K*(simu[0,k-1] + simu[0,k-2]))
def y2(k, simu):
return (2*simu[2,k-1] - simu[2,k-2] + K*(simu[1,k-1] + simu[1,k-2]))
def swap(simu):
simu[0,0] = simu[0,1]
simu[0,1] = simu[0,2]
simu[1,0] = simu[1,1]
simu[1,1] = simu[1,2]
simu[2,0] = simu[2,1]
simu[2,1] = simu[2,2]
return;
########################################################
def masquerBords(img, padding):
img[:,:padding] = 0
img[:,-padding:] = 0
......@@ -115,6 +47,7 @@ SE_dilate_finish = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (15,15))
contours_thickness = 15
RESOLUTION = 480
def init_camera():
camera = PiCamera()
......@@ -148,7 +81,26 @@ def center_position(X):
resx = x - CX
resy = RESOLUTION - y - CY # (0,0) de l'image en *haut* à gauche
return (resx, resy)
########################################################################### ser = serial.Serial('/dev/ttyACM0', 57600, timeout = 1) # Connection a l'Arduino ser.flush()
###########################################################################
### MEMORISATION PAS PRECEDENTS
nb_pas = 3; # Nombre de pas (k) simules
simuX = np.zeros((3, nb_pas)); # ligne 0 : yc, ligne 1 : u et ligne 2 : y
simuY = np.zeros((3, nb_pas)); # ligne 0 : yc, ligne 1 : u et ligne 2 : y
#[[yc[k-2],yc[k-1],y[k]],
# [u[k-2], u[k-1], u[k]],
# [y[k-2], y[k-1], y[k]]]
sat = 50; # saturation pour la commande envoyee aux servos
###########################################################################
### CONNEXION ARDUINO
ser = serial.Serial('/dev/ttyACM0', 57600, timeout = 1) # Connection a l'Arduino
ser.flush()
print("Connexion avec Arduino OK")
......@@ -156,14 +108,13 @@ pos_mid = 512; # Correspond a 0 degree en inclinaison
range_max = 100; # Inclinaison maximale en nombre de pas (1024 pas au total pour 300 degrees)
pos_min = pos_mid - range_max;
pos_max = pos_mid + range_max;
pos = pos_mid
time.sleep(1) # Pour la synchronisation des communications
###########################################################################
sat = 50;
time.sleep(1) # Pour la synchronisation des communications
########################################################################### INITIALISATION
########################################################################
### INITIALISATION
camera = init_camera()
......@@ -203,9 +154,14 @@ print("Passage a la boucle principale")
#Initialisation du tracker, penser a regler les constantes EPS et MAX
tracker = Tracker(path)
########################################################################### BOUCLE PRINCIPALE
for frame in camera.capture_continuous(raw_capture, format="bgr", use_video_port=True):
########################################################################
### BOUCLE PRINCIPALE
for frame in camera.capture_continuous(raw_capture, format="bgr", use_video_port=True):
### DETECTION POSITION BILLE ET AFFICHAGE
if traker.is_arrived():
print("La bille est arrivee, fin du programme.")
break
......@@ -226,13 +182,14 @@ for frame in camera.capture_continuous(raw_capture, format="bgr", use_video_port
consigneX, consigneY = center_position((consigneX, consigneY))
print(consigneX, ", ", consigneY)
#A decommenter pour voir la video, mais penser que ça fausse la vitesse d'execution
cv2.circle(img, (rX,rY), 4, (0,233,255),-1)#Ajout d'un point sur l'image d'origine, pour montrer ou ce trouve le centre detecte
cv2.circle(img, (CX,CY), 4, (255,0,255),-1)#Ajout d'un point sur l'image d'origine, pour montrer ou ce trouve le centre detecte
#cv2.circle(img, (listConsigneX[nbConsigne]-CX,listConsigneY[nbConsigne]-CY), 4, (255,255,0),-1)#Ajout d'un point sur l'image d'origine, pour montrer ou ce trouve le centre detecte
cv2.circle(img, (consigneX+CX,-consigneY+CY), 4, (255,0,255),-1)#Ajout d'un point sur l'image d'origine, pour montrer ou ce trouve le centre detecte
cv2.imshow("Frame", img)
cv2.imshow("Frame2", img_red)
cv2.circle(img, (rX,rY), 4, (0,233,255),-1)# Ajout point position bille
cv2.circle(img, (CX,CY), 4, (255,0,255),-1)# Ajout point position centre plateau
#cv2.circle(img, (listConsigneX[nbConsigne]-CX,listConsigneY[nbConsigne]-CY), 4, (255,255,0),-1)
cv2.circle(img, (consigneX+CX,-consigneY+CY), 4, (255,0,255),-1)# Ajout point objectif
cv2.imshow("Frame", img) # Image reelle
cv2.imshow("Frame2", img_red) # Image pour detection bille
# Clear the stream in preparation for the next frame
raw_capture.truncate(0)
......@@ -243,54 +200,46 @@ for frame in camera.capture_continuous(raw_capture, format="bgr", use_video_port
break
################################################################################
### CALCULE COMMANDE
# Consigne
simuX[0,2] = consigneX
simuY[0,2] = consigneY
#simuX[0,2] = consigneX
#simuY[0,2] = consigneY
# Simulation conjointe de u et y en fonction de yc
# Commande
simuX[1,2] = -u(2, simuX, sat)
simuY[1,2] = -u(2, simuY, sat)
#print("simuX: ", simuX[1,2],"simuY: ", simuY[1,2])
# Sortie
simuX[2,2] = posX
simuY[2,2] = posY
print("posX: ", posX,"posY: ", posY)
#print('yc = ', simuX[0,2], ', u = ', simuX[1,2], ' et y = ', simuX[2,2])
# Decalage temporel de la memorisation des pas
swap(simuX)
swap(simuY)
################################################################################
### MISE EN FORME COMMANDE ET ENVOIE A ARDUINO
#
# ################################################################################
#
factor = 2
#pos = (int)(512 + (simuX[1,2]/factor)) + (int)(512 + ((simuY[1,2]/factor)))*1024
#pos = (int)(128 + (simuX[1,2]/factor)) + (int)(128 + ((simuY[1,2]/factor)))*256
factor = 2 # Facteur d'attenuation de la commande
pos1 = simuX[1,2]/factor + 512
pos2 = simuY[1,2]/factor + 512 - 5
pos2 = simuY[1,2]/factor + 512 - 5 # "-5" : offset car bielle pas super bien reglee
print("u(X): ", (int)(pos1-512),"u(Y): ", (int)(pos2-512))
# Conditionnement information a envoyer
pos_value_list = [str((int)(pos1)),str((int)(pos2))]
send_string = ','.join(pos_value_list)
send_string += "\n" # pluto \0 ?
send_string += "\n"
# Send the string. Make sure you encode it before you send it to the Arduino.
# Envoie la chaine de caractere encodee en utf8
ser.write(send_string.encode('utf-8'))
iter += 1
# On s'assure que la periode de la boucle principale correspond a Te de l'asservissement
temps2 = time.time()
while(temps2-temps1 < Te):
time.sleep(0.005)
......@@ -298,11 +247,12 @@ for frame in camera.capture_continuous(raw_capture, format="bgr", use_video_port
#print(temps2-temps1)
temps1 = time.time()
# arret, remise a zero
# arret, remise a zero du plateau
# Conditionnement information a envoyer
pos_value_list = [str(0),str(0)]
send_string = ','.join(pos_value_list)
send_string += "\n" # pluto \0 ?
# Send the string. Make sure you encode it before you send it to the Arduino.
# Envoie la chaine de caractere encodee en utf8
ser.write(send_string.encode('utf-8'))
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment