Pathfinding-Simulator/robotCommande.py at main · capopaul/Pathfinding-Simulator · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
# -*- coding: utf-8 -*-

#----------------------------------
import math
import serial
import numpy as np
import math as m
from threading import Thread
import time

#----------------------------------
from Usefull.fonctionsFichier import *

#----------------------------------

class SerialInteract(Thread):

    """Thread chargement desimplement d'afficher une lettre dans la console."""
    """

    Da; Db, Gb, Ga,


    1400;-1400;-1400;1400;
    1526;-1400;-1400;1526;


"""
    def __init__(self, canvas):
        Thread.__init__(self)
        self.canvas = canvas
        self.statut = 0
        try:
            # -1: erreur initialisation : 0: valeur au depart: 1 = initialisation faite avec succès ; 2 traitement en cours thread lancé; 3 : fini
            self.port_serie = serial.Serial(port=config().Port,baudrate=9600,timeout=0.01)

            self.PosRoue = np.array([[11,10],[11,-10],[-11,-10],[-11,10]])

            self.ConstanteRotation = 26.31/500

            self.MatTransVec2Wheel = np.matrix([[1,-1,-210],[1,1,-210],[1,-1,210],[1,1,210]])

            self.MatTransWheel2Vec = np.matrix([[0.25,0.25,0.25,0.25],[-0.25,0.25,-0.25,0.25],[-1./(4.*210.),-1/(4.*210.),1./(4.*210.),1./(4.*210.)]])

            self.Kp=0.2

            self.Ki=0

            self.Kd=0

            self.taille = float(config().Reel_Echelle)
            self.CoeffRotation = float(config().CoeffRotation)
            self.statut = 1
        except serial.serialutil.SerialException:
            self.statut = -1
            print("Branchez le dispositif de communication")


    def run(self):
        """Code  exécuté pendant l'exécution du thread."""

        def distance(xa, ya, xb, yb):
            return math.sqrt(((xb-xa)**2)+((yb-ya)**2))

        if self.statut == 1:

            timer = 0
            Tlast = time.time()
            self.canvas.simulationReel = True

            #compteur de point
            i = 0


            listeVecteur = self.taille*np.array(self.canvas.pointSolution.flatten())
            RobotPosition = [(-1)*self.taille*self.canvas.coordRobot[0], self.taille*self.canvas.coordRobot[1], 0]
            try:
                RobotPoint = np.array([listeVecteur[i], listeVecteur[i+1],0], dtype=np.uint32)


                rotation = self.CoeffRotation*RobotPoint[2]
                RobotAim = np.array([(-1)*RobotPoint[0], RobotPoint[1], rotation])
                LastErreur =  np.matrix([[0],[0],[0]])

            except IndexError:
                self.statut = -1
                self.port_serie.close()
                return
            time.sleep(2)
            self.statut = 2

            while self.canvas.simulationReel:


                    if math.fabs(distance(RobotPosition[0], RobotPosition[1], RobotAim[0], RobotAim[1])) <= float(config().DistanceChangePoint):
                        i += 2
                        if len(listeVecteur) > i:
                            RobotPoint = [listeVecteur[i], listeVecteur[i+1],0]
                            rotation = self.CoeffRotation*RobotPoint[2]
                            RobotAim = np.array([(-1)*RobotPoint[0], RobotPoint[1], rotation])
                        else:
                            self.canvas.simulationReel = False
                    try:
                        ligne = self.port_serie.readline().decode('utf-8').split(";")
                    except UnicodeDecodeError:
                        print("Erreur bizarre qui sort de je ne sais ou")
                        break
                    if(len(ligne) == 5):

                        Tnow = time.time()


                        Tdelta = Tnow - Tlast

                        MatWheelRecup = np.matrix([[-int(ligne[3])],[-int(ligne[2])],[int(ligne[1])],[int(ligne[0])]])/14

                        MatDirectionVector = ( self.MatTransWheel2Vec * MatWheelRecup ) / 28

                        if self.canvas.ligneDetectee == False:

                            RobotPosition[0] += MatDirectionVector[0,0]*Tdelta
                            RobotPosition[1] += MatDirectionVector[1,0]*Tdelta
                            RobotPosition[2] += MatDirectionVector[2,0]*Tdelta
                            self.canvas.coordRobot = [-RobotPosition[0]/self.taille,RobotPosition[1]/self.taille]
                        else:
                            RobotPosition[0] = (-1)*self.taille*self.canvas.coordRobot[0]
                            RobotPosition[1] = self.taille*self.canvas.coordRobot[1]


                        Tlast = time.time()

                    x = RobotAim[0]- RobotPosition[0]
                    y = RobotAim[1]- RobotPosition[1]
                    t = RobotAim[2]- RobotPosition[2]


                    Erreur = np.matrix([[x],[y],[t]])


                    DeltaErreur = Erreur - LastErreur

                    MatTranslat = ( Erreur * self.Kp + DeltaErreur*self.Kd )

                    LastErreur = Erreur

                    MatWheelVel = ( self.MatTransVec2Wheel * MatTranslat ) *28*14

                    commande = str(int(MatWheelVel[3,0]))+";"+str(int(MatWheelVel[2,0]))+";"+str(int(-MatWheelVel[1,0]))+";"+str(int(-MatWheelVel[0,0]))+";"

                    self.port_serie.write(commande.encode("utf-8"))

                    time.sleep(0.05)

                    timer+=0.05

            #Fin de la boucle principale
            commande = "0;0;0;0;"
            for _ in range(5):
                self.port_serie.write(commande.encode("utf-8"))
                time.sleep(0.03)

            self.port_serie.close()
            self.statut = 3
        else:
            print("Désolé mais l'initialisation n'a pas fonctionnée")