コード例 #1
0
ファイル: Body.py プロジェクト: nlurkin/Drone
class Body(object):
    '''
    classdocs
    '''

    oldomega = Vector()
    m1 = Motor()
    m2 = Motor()
    m3 = Motor()
    m4 = Motor()
    
    '''internal state:
    alpha    -> computed from Torque
    omega    -> Sensor(gyro)
    quat    -> Sensor(DMZ)
    acceleration    -> Sensor(acc)
    velocity
    position
    '''
    
    ctrl = None
    
    #set by controller
    CtrlInput = [0,0,0,0]
    
    #from motor
    Torque = Vector() #vector
    Thrust = Vector() #vector
    
    #from physics
    Friction = Vector() #vector
    
    #state
    Alpha = Vector() #vector
    Omega = Vector() #vector
    Quat = Quaternion() #quaternion
    ThetaDot = Vector()
    Angles = Vector()
    Acceleration = Vector() #vector
    Velocity = Vector() #vector
    Position = Vector() #vector
    
    #model parameters
    L = None #longueur des bras #scalar
    Mass = None #masse #scalar
    I = Vector() #inertia matrix #vector
    K_d = None #friction-linear velocity #scalar
    
    #simulation parameters
    TorqueIsSet = False #switch #scalar
    MaxTorque = 100
    UseController = False
    
    def __init__(self, simu):
        '''
        Constructor
        '''
        if Params.ctrlType==0:
            self.ctrl = PID()
            print "Using controller PID"
        elif Params.ctrlType==1:
            self.ctrl = PSquare()
            print "Using controller PSquare"
        self.m1.setParams(1)
        self.m2.setParams(-1)
        self.m3.setParams(1)
        self.m4.setParams(-1)
        
        self.sensors = SensorValues()
        self.calib = Calibration(self, simu)
    
    def setModel(self, I, K_d, L, Mass):
        self.I = I
        self.K_d = K_d
        self.L = L
        self.Mass = Mass
        #print "inertia factor x " + str((self.I[1]-self.I[2])/self.I[0])
        
    def setParameters(self,TorqueIsSet, MaxTorque, UseController):
        self.TorqueIsSet = TorqueIsSet
        self.MaxTorque = MaxTorque 
        self.UseController = UseController
    
    def setReference(self, qRef, vRef):
        self.ctrl.setQRef(qRef, vRef)
    
    def initController(self):
        self.ctrl.setI(self.I)
        if Params.ctrlType==0:
            self.ctrl.setPs([10,15,2])
        elif Params.ctrlType==1:
            self.ctrl.setPs(Params.PCoeff)
    
    def applyController(self):
        t = self.ctrl.computePP(self.Quat, self.Omega, self.Acceleration, self.Velocity, Params.dt)
        if self.TorqueIsSet: 
            if t[0]>self.MaxTorque:
                t[0] = self.MaxTorque
            if t[1]>self.MaxTorque:
                t[1] = self.MaxTorque
            if t[2]>self.MaxTorque:
                t[2] = self.MaxTorque
            if t[0]<-self.MaxTorque:
                t[0] = -self.MaxTorque
            if t[1]<-self.MaxTorque:
                t[1] = -self.MaxTorque
            if t[2]<-self.MaxTorque:
                t[2] = -self.MaxTorque
        self.CtrlInput = t
        #print "Controller input " + t
    
    def changeInput(self, motor, value):
        self.CtrlInput[motor] = value;
        print "CtrlInput is now " + str(self.CtrlInput)
        
    def nextStep(self, dt):
        #controller
        if self.UseController:
            self.applyController()
        
        ooldomega = Vector(self.Omega)
        oldaplha = Vector(self.Alpha)
        #motor
        self.computeMotor(dt)
        #torque
        self.computeTorque()
        #thrust
        self.computeThrust()
        #fiction
        self.computeFriction()   
        #acceleration
        self.computeAcceleration()
        #vitesse
        self.computeVelocity(dt)
        #position
        self.computePosition(dt)
        #alpha
        self.computeAlpha(dt)
        #omega
        self.computeOmega(dt)
        #thetaDot
        self.computeThetaDot(dt)
        #theta
        self.computeAngles(dt)
        #quaternion
        self.computeQuaternion(dt)
        
        self.sensors.setValues(self.Acceleration, self.Omega, self.Quat, self.Alpha)
        
        ''''I = (oldaplha[0]-self.Alpha[0])/(ooldomega[1]*ooldomega[2]-self.Omega[1]*self.Omega[2])
        if I > 1:
            #print "oldomega " + oldomega
            print "oldomega " + ooldomega
            print "omega " + self.Omega
            print "oldalpha " + oldaplha
            print "alpha " + self.Alpha
            #print "self.oldomega + " + self.oldomega
            print "diff " + str(ooldomega[1]*ooldomega[2]-self.Omega[1]*self.Omega[2]) 
            print "I Body " + str(I)'''
        #I = (self.I[2]-self.I[1])/self.I[0]
        #rx = self.L*self.m1.K*pow(Params.MaxOmega,2)/(10000.*self.I[0])
        '''print "true I " + str(I)
        print "true rx " + str(rx)
        print "true alpha " + str(self.Alpha)
        print "omega motor " + str(self.m1.Omega)
        print "omega ici " + str(self.m1.Power*Params.MaxOmega/100.)
        print "torque " + str(self.Torque[0]/self.I[0])
        print "torque ici " + str(rx*pow(self.m1.Power,2))
        print "alpha second terme " + str(oldomega[1]*oldomega[2]*I)'''
        #alpha = pow(self.m1.Power,2)*rx - ooldomega[1]*ooldomega[2]*I
        #I = -0.887005649718
        #Rx = (self.Alpha[0]+ooldomega[1]*ooldomega[2]*I)/pow(self.m1.Power,2)
        #print Rx
        #print "calxulated alpha " + str(alpha)
        #print "Calculated rx " + str(Rx)
        #self.oldomega = Vector(ooldomega)
        
        
    def setMotorConstants(self, Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D):
        self.m1.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
        self.m2.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
        self.m3.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
        self.m4.setConstants(Rho, K_v, K_t, K_tau, I_M, A_swept, A_xsec, Radius, C_D)
        if self.UseController:
            Rx = self.L*self.m1.K*pow(Params.MaxOmega,2)/(10000)
            Ry = self.L*self.m1.K*pow(Params.MaxOmega,2)/(10000)
            Rz = self.m1.B*pow(Params.MaxOmega,2)/(10000)
            RT = self.m1.K*pow(Params.MaxOmega,2)/(10000)
            print "Real parameters :"
            print "R1 " + str([Rx, 0, Rz, RT])
            print "R2 " + str([0, Ry, Rz, RT])
            print "R3 " + str([Rx, 0, Rz, RT])
            print "R4 " + str([0, Ry, Rz, RT])
            print Params.I
            print "I_1 " + str((Params.I[1]-Params.I[2])/Params.I[0])
            print "I_2 " + str((Params.I[0]-Params.I[2])/Params.I[1])
            print "I_3 " + str((Params.I[0]-Params.I[1])/Params.I[2])
            self.ctrl.setMotorCoefficient([Rx, 0, Rz, RT],
                                          [0, Ry, Rz, RT],
                                          [Rx, 0, Rz, RT],
                                          [0, Ry, Rz, RT])
            self.ctrl.setParameters(self.m1.K, self.L, self.m1.B, self.Mass)
    
    def computeMotor(self, dt):
        if not self.TorqueIsSet:
            self.m1.setMeasure(self.CtrlInput[0], dt)
            self.m2.setMeasure(self.CtrlInput[1], dt)
            self.m3.setMeasure(self.CtrlInput[2], dt)
            self.m4.setMeasure(self.CtrlInput[3], dt)
    
    def computeTorque(self):
        if not self.TorqueIsSet:
            self.Torque = Vector([self.L*(self.m1.Thrust-self.m3.Thrust), self.L*(self.m2.Thrust-self.m4.Thrust), self.m1.Tau_z+self.m2.Tau_z+self.m3.Tau_z+self.m4.Tau_z])
            '''print "M1 thrust " + str(self.L*self.m1.Thrust)
            print "M2 thrust " + str(self.L*self.m2.Thrust)
            print "M3 thrust " + str(self.L*self.m3.Thrust)
            print "M4 thrust " + str(self.L*self.m4.Thrust)
            print "M1 tauZ " + str(self.m1.Tau_z)
            print "M2 tauZ " + str(self.m2.Tau_z)
            print "M3 tauZ " + str(self.m3.Tau_z)
            print "M4 tauZ " + str(self.m4.Tau_z)'''
            #print "Body torque " + self.Torque
        else:
            self.Torque = self.CtrlInput
        
    def computeFriction(self):        
        self.Friction = -self.Velocity*self.K_d
    
    def computeAlpha(self, dt):
        #print "Omega alpha: " + self.Omega
        #oldalpha = self.Alpha
        self.Alpha = Vector([(self.Torque[0] - (self.I[1]-self.I[2])*self.Omega[1]*self.Omega[2])/self.I[0],
                      (self.Torque[1] - (self.I[2]-self.I[0])*self.Omega[0]*self.Omega[2])/self.I[1],
                      (self.Torque[2] - (self.I[0]-self.I[1])*self.Omega[0]*self.Omega[1])/self.I[2]])
        #print "I factor apha " + str((self.Alpha[0] - oldalpha[0])/(self.Omega[1]*self.Omega[2] - self.oldomega[1]*self.oldomega[2]))
        #print self.Alpha
        #print "alpha second term(true) " + str((self.I[2]-self.I[1])*self.Omega[1]*self.Omega[2]/self.I[0])
    
    def computeOmega(self, dt):
        self.Omega = self.Omega + self.Alpha*dt
        #print self.Omega
    
    def computeThetaDot(self, dt):
        mat1 = Matrix([[1, sin(self.Angles[0])*tan(self.Angles[1]), cos(self.Angles[0])*tan(self.Angles[1])], 
                       [0, cos(self.Angles[0]), -sin(self.Angles[0])], 
                       [0, sin(self.Angles[0])/cos(self.Angles[1]), cos(self.Angles[0])/cos(self.Angles[1])]])
        self.ThetaDot = mat1*self.Omega

    def computeAngles(self, dt):
        self.Angles = self.Angles + self.ThetaDot*dt
        
    def computeQuaternion(self, dt):
        self.Quat = Quaternion([self.Angles[0], self.Angles[1], self.Angles[2]])
        
    def mass(self):
        return self.Mass
    
    def setOmega(self, v):
        self.Omega = v
    
    def setTorque(self, t):
        self.Torque = t
    
    def setCtrlInput(self, i):
        self.CtrlInput = i
    
    def computeThrust(self):
        if not self.TorqueIsSet:
            self.Thrust = Vector([0, 0, self.m1.Thrust+self.m2.Thrust+self.m3.Thrust+self.m4.Thrust])
        else:
            self.Thrust = Vector([0,0,0])
    
    def computeAcceleration(self):
        #rotate thrust
        #print "Thrust before rotation " + self.Thrust
        #print "Rotation " + self.Quat
        T = self.Thrust.rotate(self.Quat)
        #print "Thrust " + T
        self.Acceleration = T/self.Mass
        self.Acceleration = Params.Gravity+T*(1/self.Mass)+self.Friction*(1/self.Mass)
    
    def computeVelocity(self, dt):
        #print dt
        self.Velocity += self.Acceleration*dt
        #print self.Velocity
    
    def computePosition(self, dt):
        self.Position += self.Velocity*dt
        if(Params.ground and self.Position[2]<0):
            #On the ground. Reset all values
            self.Position[2]=0
            self.Velocity = Vector([0,0,0])
            self.Acceleration = Vector([0,0,0])
            self.Alpha = Vector([0,0,0])
            self.Omega = Vector([0,0,0])
            self.ThetaDot = Vector([0,0,0])
            self.Angles = Vector([0,0,0])
            self.Quat = Quaternion([self.Angles[0], self.Angles[1], self.Angles[2]])
    
    def calibrate(self,dt):
        #dt = 0.001
        self.UseController = False
        
        #minMotor = self.level(simu)
        #print minMotor
        minMotor = 886
        
        self.calib.calibrateI(dt, minMotor)
        self.calib.calibrateMotor(0, dt, minMotor)
        self.calib.calibrateMotor(1, dt, minMotor)
        self.calib.calibrateMotor(2, dt, minMotor)
        self.calib.calibrateMotor(3, dt, minMotor)
        print self.calib.cali.getAveragedI()
        print self.calib.cali.getIAxis()
        print self.calib.cali.getR(0)
        print self.calib.cali.getR(1)
        print self.calib.cali.getR(2)
        print self.calib.cali.getR(3)
        self.UseController = True
        self.ctrl.setMotorCoefficient( self.calib.cali.getR(0), self.calib.cali.getR(1), self.calib.cali.getR(2), self.calib.cali.getR(3))
        self.ctrl.setI( self.calib.cali.getIAxis())