def GetEncSpeed(motor_sel): AccessRoboclaw() if motor_sel[1] == 1: enc_val = list(roboclaw.ReadSpeedM1(motor_sel[0])) elif motor_sel[1] == 2: enc_val = list(roboclaw.ReadSpeedM2(motor_sel[0])) else: raise NotImplementedError #if enc_val[0] == 0: # "Could not read encoder value for " + str(motor_sel) #if enc_val[0] == 0 or enc_val[2] != motor_sel[0]: # received signal from wrong encoder, so we invalidate the result # enc_val[0] = 0 return enc_val
def read_encoder_speed(self, motor):
if self.motor_num[motor] == 1:
out = roboclaw_driver.ReadSpeedM1(self.address[motor])
else:
out = roboclaw_driver.ReadSpeedM2(self.address[motor])
return out