def calibrateR(self,motor,Mass): I = self.fI/float(self.number) #OI = [0.177, 0.177, 0.334] #I = [(OI[1]-OI[2])/OI[0], -(OI[0]-OI[2])/OI[1], (OI[0]-OI[1])/OI[2]] #This works if I is well known and simulation step is small enough so that the difference between oldomega and omega is small Rx = (self.Ialpha[0][0] - self.Iomega[0][1]*self.Iomega[0][2]*I[0])/self.fP[0]#pow(self.fP[0],2) #Rx = ((self.Ialpha[1][0]-self.Ialpha[0][0]) - I[0]*(self.Iomega[1][1]*self.Iomega[1][2] - self.Iomega[0][1]*self.Iomega[0][2]))/(pow(self.fP[1],2)-pow(self.fP[0], 2)) Ry = (self.Ialpha[0][1] - self.Iomega[0][0]*self.Iomega[0][2]*I[1])/self.fP[0]#pow(self.fP[0],2) #Ry = ((self.Ialpha[1][1]-self.Ialpha[0][1]) - I[1]*(self.Iomega[1][0]*self.Iomega[1][2] - self.Iomega[0][0]*self.Iomega[0][2]))/(pow(self.fP[1],2)-pow(self.fP[0], 2)) Rz = (self.Ialpha[0][2] - self.Iomega[0][0]*self.Iomega[0][1]*I[2])/self.fP[0]#pow(self.fP[0],2) #Rz = ((self.Ialpha[1][2]-self.Ialpha[0][2]) - I[2]*(self.Iomega[1][0]*self.Iomega[1][1] - self.Iomega[0][0]*self.Iomega[0][1]))/(pow(self.fP[1],2)-pow(self.fP[0], 2)) #print "Estimated Rx " + str(Rx) #print "Estimated Ry " + str(Ry) #print "Estimated Ry " + str(Rz) q1 = self.Iq[0] a1 = self.Ia[0] a1 = a1.rotate(q1.conj()) q2 = self.Iq[1] a2 = self.Ia[1] a2 = a2.rotate(q2.conj()) g = Vector([0,0, 9.81]) g1 = g.rotate(q1.conj()) g2 = g.rotate(q2.conj()) #Ok but needs to be with very small velocity to neglect friction Rt = Mass*((a1- a2) + (g1-g2))/(self.fP[0]-self.fP[1]) self.R[motor] = [Rx,Ry,Rz,Rt[2]] return True
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())