def init_rc(): global rc global rc_address # Initialise the roboclaw motorcontroller print("logo: initialising roboclaw driver...") from roboclaw_3 import Roboclaw rc_address = 0x80 rc = Roboclaw("/dev/roboclaw", 115200) rc.Open() # Get roboclaw version to test if is attached version = rc.ReadVersion(rc_address) if version[0] is False: print("logo init: roboclaw get version failed") sys.exit() else: print("logo init:",repr(version[1])) # Set motor controller variables to those required by K9 rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS) rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS) rc.SetMainVoltages(rc_address,240,292) # 24V min, 29.2V max rc.SetPinFunctions(rc_address,2,0,0) # Zero the motor encoders rc.ResetEncoders(rc_address) # Print Motor PID Settings m1pid = rc.ReadM1VelocityPID(rc_address) m2pid = rc.ReadM2VelocityPID(rc_address) print("logo init: m1 p: " + str(m1pid[1]) + ", i:" + str(m1pid[2]) + ", d:" + str(m1pid[3])) print("m2 p: " + str(m2pid[1]) + ", i:" + str(m2pid[2]) + ", d:" + str(m2pid[3])) # Print Min and Max Main Battery Settings minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts print ("logo init: min main battery: " + str(int(minmaxv[1])/10.0) + "V") print ("logo init: max main battery: " + str(int(minmaxv[2])/10.0) + "V") # Print S3, S4 and S5 Modes S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined'] S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home'] S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home'] pinfunc = rc.ReadPinFunctions(rc_address) print ("logo init: s3 pin: " + S3mode[pinfunc[1]]) print ("logo init: s4 pin: " + S4mode[pinfunc[2]]) print ("logo init: s5 pin: " + S5mode[pinfunc[3]]) print("logo init: roboclaw motor controller initialised...")
while (heading < desired_angle - 0.02) or ( heading > desired_angle + 0.02 ): # rotate until you're within a degree (0.02 radians) of target frames = pipe.wait_for_frames() pose = frames.get_pose_frame() if pose: data = pose.get_pose_data() heading = get_heading(data) delta_angle = heading - desired_angle # get the difference between the current and intended angle direction = dir(heading, desired_angle) print("Current heading:", round(heading, 3), "Desired angle:", round(desired_angle, 3), "Direction: ", direction) rc.ResetEncoders(address) if (heading < desired_angle - 0.25) or ( heading > desired_angle + 0.25 ): # go fast if you're more than .25 radians away speed = 2500 # go fast else: speed = 1500 # go slow rc.SpeedDistanceM1(address, -1 * direction * speed, 1 * tickdistanceL, 1) # rotate a few degrees rc.SpeedDistanceM2(address, direction * speed, 1 * tickdistanceR, 1) buffers = (0, 0, 0) while (buffers[1] != 0x80 and buffers[2] != 0x80 ): #Loop until distance command has completed # displayspeed()
class Footballmachine: def __init__(self, address=0x80, baudrate=38400, port="/dev/ttyS0"): self.address = address self.rc = Roboclaw(port, baudrate) self.rc.Open() version = self.rc.ReadVersion(self.address) if version[0] == False: print("GETVERSION Failed - check power supply and conections") return else: print(repr(version[1])) def has_angle_motor_stopped_moving(self): interval = 1 first = self.rc.ReadEncM1(self.address) sleep(interval) second = self.rc.ReadEncM1(self.address) while (first != second): first = self.rc.ReadEncM1(self.address) sleep(interval) second = self.rc.ReadEncM1(self.address) def init_motors(self): print("Initializing all motors...") self.rc.BackwardM1(self.address, 126) self.has_angle_motor_stopped_moving() self.rc.BackwardM1(self.address, 0) self.rc.ResetEncoders(self.address) print("Angle encoder:", self.rc.ReadEncM1(self.address)[1]) def displayspeed(self): enc1 = self.rc.ReadEncM1(self.address) enc2 = self.rc.ReadEncM2(self.address) speed1 = self.rc.ReadSpeedM1(self.address) speed2 = self.rc.ReadSpeedM2(self.address) print(("Encoder1:"), end=' ') if (enc1[0] == 1): print(enc1[1], end=' ') print(format(enc1[2], '02x'), end=' ') else: print("failed", end=' ') print("Encoder2:", end=' ') if (enc2[0] == 1): print(enc2[1], end=' ') print(format(enc2[2], '02x'), end=' ') else: print("failed ", end=' ') print("Speed1:", end=' ') if (speed1[0]): print(speed1[1], end=' ') else: print("failed", end=' ') print(("Speed2:"), end=' ') if (speed2[0]): print(speed2[1]) else: print("failed ") def speed_to_QPPS(self, speed): radius = 0.1 encoder_pulses_per_rad = 1024 / 2 angular_speed = speed / (2 * pi * radius) QPPS = encoder_pulses_per_rad * angular_speed return int(QPPS) def angle_to_QP(self, angle): range_min = 0 range_max = 225 angle_min = 0 angle_max = 55 a1 = int(angle) - angle_min a2 = range_max - range_min a3 = angle_max - angle_min angle = int((a1 * a2) / a3 + range_min) return angle def set_angle(self, angle): print("Set_angle: ", angle) angle = self.angle_to_QP(angle) print("Target position M1:", angle) self.rc.SpeedAccelDeccelPositionM1(self.address, 10, 10, 10, angle, 0) self.has_angle_motor_stopped_moving() print("Angle encoder:", self.rc.ReadEncM1(self.address)[1]) def set_speed_then_stop(self, speed): print("Set_speed: ", speed) speed = self.speed_to_QPPS(int(speed)) self.rc.SpeedAccelM2(self.address, 22000, speed) sleep(4) self.rc.SpeedAccelM2(self.address, 22000, 0) def set_speed(self, speed): print("Set_speed: ", speed) speed = self.speed_to_QPPS(int(speed)) self.rc.SpeedAccelM2(self.address, 22000, speed) for i in range(0, 50): print(("{} # ".format(i)), end=' ') self.displayspeed() sleep(0.1) def check_encoders(self, seconds): for i in range(0, seconds): print(("{} # ".format(i)), end=' ') self.displayspeed() sleep(0.1)