diff --git a/solo12.py b/solo12.py index 5976db4..d27f907 100644 --- a/solo12.py +++ b/solo12.py @@ -18,17 +18,11 @@ def InitRobotSpecificParameters(self): self.maximumCurrent = 12.0 # A # To get this offsets, run the calibration with self.encoderOffsets at 0, # then manualy move the robot in zero config, and paste the position here (note the negative sign!) - #self.encoderOffsets = - np.array([1.940310, -2.658198, -2.893262, -1.918342, - # 1.501880, -0.694378, 2.974716, -0.415520, -0.326077, 0.444250, 2.433491, -0.384136]) # Solo12 of Max Planck - # self.encoderOffsets = - np.array([-1.7298685312271118, 1.5541203022003174, -0.22502104938030243, -2.7335188388824463, - # 2.563904047012329, -2.5461583137512207, 1.5116088390350342, -2.1389687061309814, -1.7647048234939575, 0.12051336467266083, -3.485957145690918, -0.834662675857544]) - #self.encoderOffsets = - np.array([-1.7356888055801392, 1.8896119594573975, -0.31902867555618286, -2.6425065994262695, - # 2.7726054191589355, -2.7252161502838135, 1.5070443153381348, -2.3966293334960938, 0.2065046727657318, -1.5028218030929565, 3.048154592514038, -1.1061315536499023]) - self.encoderOffsets = - np.array([-1.743848204612732, 1.8073862791061401, -0.38901469111442566, -2.7215936183929443, 2.8377127647399902, -2.721593141555786, 1.2487566471099854, - -2.0542333126068115, 1.7747472524642944, -1.5836209058761597, 2.703704833984375, -0.7676450610160828]) - - self.encoderOffsets -= 9.0 * np.array([-0.01122508, 0.00826484, -0.04627015, 0.02105599, -0.01103931, -0.00173937, - 0.0111513 , -0.03019726, 0.07182457, 0.02774821, 0.01398398, 0.00784015]) + self.encoderOffsets = - np.array([-1.7551466226577759, 1.8692125082015991, -0.33785927295684814, + -2.8217074871063232, 2.4837048053741455, -2.5392541885375977, + -1.7476146221160889, -2.1298675537109375, 1.9545761346817017, + 2.2128634452819824, -3.4803080558776855, -1.0698696374893188]) + # self.encoderOffsets *= 0. # 180 degree roll # self.rotateImuVectors = lambda x: [-x[0], -x[1], x[2]] diff --git a/utils/abstractRobotHal.py b/utils/abstractRobotHal.py index 7a286a7..bc25f55 100644 --- a/utils/abstractRobotHal.py +++ b/utils/abstractRobotHal.py @@ -209,6 +209,11 @@ def UpdateMeasurment(self): self.baseAccelerometer[:] = self.rotateImuVectors( [self.hardware.imu_data_accelerometer(0), self.hardware.imu_data_accelerometer(1), self.hardware.imu_data_accelerometer(2)]) + # Power data from the powerboard + self.current = self.hardware.powerboard_current() + self.voltage = self.hardware.powerboard_voltage() + self.energy = self.hardware.powerboard_energy() + return def SendCommand(self, WaitEndOfCycle=True): @@ -265,6 +270,7 @@ def Print(self): print(chr(27) + "[2J") self.hardware.PrintIMU() self.hardware.PrintADC() + self.hardware.PrintPowerBoard() self.hardware.PrintMotors() self.hardware.PrintMotorDrivers() self.hardware.PrintStats()