Esempio n. 1
0
 def __init__(self):
     '''
     Constructor
     '''
     #self.QRef = qmath.quaternion([0, 0, 0])
     self.QRef = Quaternion([0,0,0])
     self.POmega = 0
     self.PQ = 0
     self.PV = 0
     self.PA = 0
     self.I = Vector([0, 0, 0])
     self.Torque = Vector([0, 0, 0])
     self.Output = Vector([0,0,0,0])
     self.VRef = Vector([0,0,0])
     self.Thrust = Vector([0,0,0])
Esempio n. 2
0
class PSquare:
    '''
    classdocs
    '''
    lastqRef = Quaternion()
    lastvRef = Vector()
    
    QRef = Quaternion()
    VRef = Vector()
    PQ = None
    POmega = None
    PInt = None
    I = Matrix()
    Torque = Vector()
    Thrust = Vector()
    
    Km1 = None
    Km2 = None
    Km3 = None
    Km4 = None
    
    Output = None

    K = None
    b = None
    L = None
    Mass = None
    
    QInt = Quaternion()
    

    def __init__(self):
        '''
        Constructor
        '''
        #self.QRef = qmath.quaternion([0, 0, 0])
        self.QRef = Quaternion([0,0,0])
        self.POmega = 0
        self.PQ = 0
        self.PV = 0
        self.PA = 0
        self.I = Vector([0, 0, 0])
        self.Torque = Vector([0, 0, 0])
        self.Output = Vector([0,0,0,0])
        self.VRef = Vector([0,0,0])
        self.Thrust = Vector([0,0,0])
    
    def setI(self, I):
        self.I = Matrix([[I[0],0,0],[0,I[1],0],[0,0,I[2]]])
    
    def setPs(self, v):
        self.PQ = v[0]
        self.POmega = v[1]
        self.PInt = v[2]
        
    def setQRef(self, q, v):
        self.QRef = q
        self.VRef = v
    
    def setMotorCoefficient(self, m1, m2, m3, m4):
        self.Km1 = m1
        self.Km2 = m2
        self.Km3 = m3
        self.Km4 = m4
    
    def setParameters(self, k,L,b,m):
        self.K = k
        self.L = L
        self.b = b
        self.Mass = m
    
    def controlSignal(self):
        if self.QRef.vector().mag()==0:
            aControl = 0
        else:
            aControl = 1

        if self.VRef.mag()==0 and aControl!=0:    
            vControl = 0
        else:
            vControl = 1
        
        return [vControl, aControl]
    
    def velocityControl(self, qM, aM, vM, vControl):
        if vControl:
            vErr = self.VRef-vM
            self.lastvRef = self.VRef
        else:
            vErr = Vector([aM[0], aM[1], 0])
            self.lastvRef = Vector([0,0,0])
        
        #TODO: try having a real PID with here rather than P with Kp=1
        
        #print "AReq " + (-Params.Gravity)
        #print "vErr " + vErr
        
        #TODO: try multiplying by the mass here as this is actually computing the acceleration
        Thrust = vErr - Params.Gravity
        
        qRef = createRotation(Thrust, Vector([0,0,1]))
        #This is required to keep the z component compensating for gravity. 
        #If not, the norm is kept when rotating and the z component is reduced accordingly.
        #Maybe try by creating the rotation between z and the error and adding the gravity after!
        #TODO: ou alors c'est fait plus loin en divisant par z' et celui-ci ne sert a rien?
        qRef.w = 1
        qRef.normalize()
        
        return [Thrust, qRef]
    
    def attitudeControl(self, qM, omegaM, qRef, dt):
        qErr = qRef * qM.conj()
        
        self.QInt = self.QInt + qErr*dt*self.PInt
        #TODO: QInt here?
        if qErr[0]<0:
            intErr = -Vector(self.QInt)
        else:
            intErr = Vector(self.QInt)

        if qErr[0]<0:
            axisErr = -Vector(qErr)
        else:
            axisErr = Vector(qErr)
        
        
        #PInt>>0 -> precision; PInt<<0 -> Reaction speed
        Torque = self.I*(axisErr*self.PQ - omegaM*self.POmega - intErr)
        #Iinv = Matrix([[self.I[0][0],0,0],[0,self.I[1][1],0],[0,0,self.I[2][2]]])
        #Torque = Iinv*(axisErr*self.PQ - omegaM*self.POmega)
        return Torque
        
    def computePP(self, qM, omegaM, aM, vM, dt):
        #print "qM " + qM
        #print "omegaM " + omegaM
        #print "aM " + aM
        #print "vM " + vM
        
        print "VRef " + self.VRef
        print "QRef " + self.QRef
        
        [vControl, aControl] = self.controlSignal()
        
        print "Control signal : " + str(vControl) + " " + str(aControl) 
        
        [self.Thrust, vQRef] = self.velocityControl(qM, aM, vM, vControl)
        
        #print "qm inv " + qM.conj()
        #print "Thrust " + self.Thrust*self.Mass + " (" + str(sqrt(self.Thrust.mag())) + ")"
        #print "Rotation " + qM
        
        #TODO: remove the -qM as -qM==qM
        #TODO: je pense qu'en ce qui concerne l'axe z ici, comme on rotate z et qu'on prends la composante z, q ou q* donne les meme resultat
        #TODO: je pense que c'est ca qui rescale T pour contrer correctement la gravite. 
        T = self.Thrust*self.Mass*(1/(Vector([0,0,1]).rotate(-qM.conj())[2]))
        #T = self.Thrust.rotate(-qM.conj())*self.Mass
        
        #print "T " + T + " (" + str(sqrt(T.mag())) + ")"
        T = T[2]
        #print "T " + str(T)
        
        if vControl or not aControl:
            qRef = vQRef
        else:
            qRef = self.QRef
        
        print "qRef for torque " + qRef
        self.lastqRef = qRef
        self.Torque = self.attitudeControl(qM, omegaM, qRef, dt)
        #fit
        if Params.TorqueIsSet:
            return self.Torque
        
        #print "Requested T " + str(T)
        #print "Requested Torque " + self.Torque
        #print self.Km1
        #print self.Km2
        #print self.Km3
        #print self.Km4
        deno = (self.Km1[0]*(self.Km2[1]*(self.Km3[2]*self.Km4[3]+self.Km4[2]*self.Km3[3])+self.Km4[1]*(self.Km2[2]*self.Km3[3]+self.Km3[2]*self.Km2[3]))+self.Km3[0]*(self.Km2[1]*(self.Km1[2]*self.Km4[3]+self.Km4[2]*self.Km1[3])+self.Km4[1]*(self.Km1[2]*self.Km2[3]+self.Km2[2]*self.Km1[3])))
        #print deno
        p1=((self.Km3[0])*((self.Km2[1])*((self.Km4[2])*T+(self.Torque[2])*(self.Km4[3]))+(self.Km4[1])*((self.Km2[2])*T+(self.Torque[2])*(self.Km2[3]))+(self.Torque[1])*((self.Km2[2])*(self.Km4[3])-(self.Km4[2])*(self.Km2[3])))+(self.Torque[0])*((self.Km2[1])*((self.Km3[2])*(self.Km4[3])+(self.Km4[2])*(self.Km3[3]))+(self.Km4[1])*((self.Km2[2])*(self.Km3[3])+(self.Km3[2])*(self.Km2[3]))))/deno
        p2=-((self.Km1[0])*((self.Km4[1])*((self.Torque[2])*(self.Km3[3])-(self.Km3[2])*T)+(self.Torque[1])*(-(self.Km3[2])*(self.Km4[3])-(self.Km4[2])*(self.Km3[3])))+(self.Km3[0])*((self.Km4[1])*((self.Torque[2])*(self.Km1[3])-(self.Km1[2])*T)+(self.Torque[1])*(-(self.Km1[2])*(self.Km4[3])-(self.Km4[2])*(self.Km1[3])))+(self.Torque[0])*(self.Km4[1])*((self.Km3[2])*(self.Km1[3])-(self.Km1[2])*(self.Km3[3])))/deno
        p3=-((self.Km1[0])*((self.Km2[1])*(-(self.Km4[2])*T-(self.Torque[2])*(self.Km4[3]))+(self.Km4[1])*(-(self.Km2[2])*T-(self.Torque[2])*(self.Km2[3]))+(self.Torque[1])*((self.Km4[2])*(self.Km2[3])-(self.Km2[2])*(self.Km4[3])))+(self.Torque[0])*((self.Km2[1])*((self.Km1[2])*(self.Km4[3])+(self.Km4[2])*(self.Km1[3]))+(self.Km4[1])*((self.Km1[2])*(self.Km2[3])+(self.Km2[2])*(self.Km1[3]))))/deno
        p4=-((self.Km1[0])*((self.Km2[1])*((self.Torque[2])*(self.Km3[3])-(self.Km3[2])*T)+(self.Torque[1])*((self.Km2[2])*(self.Km3[3])+(self.Km3[2])*(self.Km2[3])))+(self.Km3[0])*((self.Km2[1])*((self.Torque[2])*(self.Km1[3])-(self.Km1[2])*T)+(self.Torque[1])*((self.Km1[2])*(self.Km2[3])+(self.Km2[2])*(self.Km1[3])))+(self.Torque[0])*(self.Km2[1])*((self.Km3[2])*(self.Km1[3])-(self.Km1[2])*(self.Km3[3])))/deno
        self.Output = Vector([p1,p2,p3,p4])
        #print "Torque0 = " + str(p1*self.Km1[0]-p3*self.Km3[0])
        #print "Torque1 = " + str(p2*self.Km2[1]-p4*self.Km4[1])
        #print "Torque2 = " + str(p1*self.Km1[2]-p2*self.Km2[2]+p3*self.Km3[2]-p4*self.Km4[2])
        #print "T = " + str(p1*self.Km1[3]+p2*self.Km2[3]+p3*self.Km3[3]+p4*self.Km4[3])
        return self.Output