def __init__(self, body, simu): self.cali = Calibrator() self.body = body self.simu = simu
class Calibration(object): def __init__(self, body, simu): self.cali = Calibrator() self.body = body self.simu = simu def calibrateI(self, dt, minMotor): dMotor = 1 self.body.CtrlInput = [minMotor, minMotor, minMotor, minMotor] self.body.CtrlInput[0] = minMotor+dMotor for j in range(0, int(0.1/dt)): self.simu.nextStep() for i in range(0,2): #set motor power self.body.CtrlInput[0] = minMotor + pow(-1,i)*dMotor #get point for j in range(0, int(0.1/dt)): self.simu.nextStep() newOmega = Vector(self.body.sensors.Omega) newAlpha = Vector(self.body.sensors.Alpha) #newOmega = self.body.Omega #newAlpha = self.body.Alpha #newOmega[0] = simu.test.omega #newAlpha[0] = simu.test.alpha #self.cali.newPoint(self.CtrlInput[0], self.Omega, self.Alpha, 0, 0) print newOmega self.cali.newPoint(self.body.CtrlInput[0], newOmega, newAlpha, 0, 0) for j in range(0, int(0.1/dt)): self.simu.nextStep() newOmega = Vector(self.body.sensors.Omega) newAlpha = Vector(self.body.sensors.Alpha) #newOmega = self.body.Omega #newAlpha = self.body.Alpha #newOmega[0] = simu.test.omega #newAlpha[0] = simu.test.alpha print newOmega self.cali.newPoint(self.body.CtrlInput[0], newOmega, newAlpha, 0, 0) self.cali.calibrateI() self.cali.clearPoints() self.body.CtrlInput[0] = minMotor-dMotor for j in range(0, int(0.1/dt)): self.simu.nextStep() self.body.CtrlInput = [0,0,0,0] def calibrateMotor(self, motor, dt, minMotor): dMotor = 3 self.body.CtrlInput = [minMotor, minMotor, minMotor, minMotor] self.body.CtrlInput[motor] = minMotor+dMotor for j in range(0, int(0.1/dt)): self.simu.nextStep() #get point for j in range(0, int(0.1/dt)): self.simu.nextStep() self.cali.newPoint(self.body.CtrlInput[motor]-minMotor, self.body.sensors.Omega, self.body.sensors.Alpha, self.body.sensors.Acceleration, self.body.sensors.Quat) self.body.CtrlInput[motor] = minMotor+2*dMotor for j in range(0, int(0.1/dt)): self.simu.nextStep() #get point for j in range(0, int(0.1/dt)): self.simu.nextStep() self.cali.newPoint(self.body.CtrlInput[motor]-minMotor, self.body.sensors.Omega, self.body.sensors.Alpha, self.body.sensors.Acceleration, self.body.sensors.Quat) self.cali.calibrateR(motor,Params.Mass)#self.m1.K*pow(Params.MaxOmega,2)/(10000)) self.cali.clearPoints() self.body.CtrlInput[motor] = minMotor-2*dMotor for j in range(0, int(0.2/dt)): self.simu.nextStep() self.body.CtrlInput[motor] = minMotor-dMotor for j in range(0, int(0.2/dt)): self.simu.nextStep() self.body.CtrlInput = [0,0,0,0] def level(self): minMotor = 0 minMotor+=1 self.body.CtrlInput = [minMotor, minMotor, minMotor, minMotor] self.simu.nextStep() while(self.Acceleration[2]<0): minMotor+=1 self.body.CtrlInput = [minMotor, minMotor, minMotor, minMotor] self.simu.nextStep() return minMotor def exportCalib(self): exportVals = {"R": [self.cali.getR(0),self.cali.getR(1),self.cali.getR(2),self.cali.getR(3)], "I":self.cali.getIAxis()} oFile = open('calib.dat', "wb") pickle.dump(exportVals, oFile) oFile.close() def importCalib(self): oFile = open('calib.dat', "rb") vals = pickle.load(oFile) oFile.close() R = vals["R"] I = vals["I"] self.body.ctrl.setMotorCoefficient(R[0],R[1],R[2],R[3]) self.body.ctrl.setI(I)