class RoboclawWrapper(object):
    """Interface between the roboclaw motor drivers and the higher level rover code"""
    def __init__(self):
        rospy.loginfo("Initializing motor controllers")

        # initialize attributes
        self.rc = None
        self.err = [None] * 5
        self.address = []
        self.current_enc_vals = None
        self.corner_cmd_buffer = None
        self.drive_cmd_buffer = None

        self.roboclaw_mapping = rospy.get_param('~roboclaw_mapping')
        self.encoder_limits = {}
        self.establish_roboclaw_connections()
        self.stop_motors()  # don't move at start
        self.setup_encoders()

        # save settings to non-volatile (permanent) memory
        for address in self.address:
            self.rc.WriteNVM(address)

        for address in self.address:
            self.rc.ReadNVM(address)

        self.corner_max_vel = 1000
        # corner motor acceleration
        # Even though the actual method takes longs (2*32-1), roboclaw blog says 2**15 is 100%
        accel_max = 2**15 - 1
        accel_rate = rospy.get_param('/corner_acceleration_factor', 0.8)
        self.corner_accel = int(accel_max * accel_rate)
        self.roboclaw_overflow = 2**15 - 1
        # drive motor acceleration
        accel_max = 2**15 - 1
        accel_rate = rospy.get_param('/drive_acceleration_factor', 0.5)
        self.drive_accel = int(accel_max * accel_rate)
        self.velocity_timeout = rospy.Duration(
            rospy.get_param('/velocity_timeout', 2.0))
        self.time_last_cmd = rospy.Time.now()

        self.stop_motors()

        # set up publishers and subscribers
        self.corner_cmd_sub = rospy.Subscriber("/cmd_corner",
                                               CommandCorner,
                                               self.corner_cmd_cb,
                                               queue_size=1)
        self.drive_cmd_sub = rospy.Subscriber("/cmd_drive",
                                              CommandDrive,
                                              self.drive_cmd_cb,
                                              queue_size=1)
        self.enc_pub = rospy.Publisher("/encoder", JointState, queue_size=1)
        self.status_pub = rospy.Publisher("/status", Status, queue_size=1)

    def run(self):
        """Blocking loop which runs after initialization has completed"""
        rate = rospy.Rate(8)

        status = Status()

        counter = 0
        while not rospy.is_shutdown():

            # Check to see if there are commands in the buffer to send to the motor controller
            if self.drive_cmd_buffer:
                drive_fcn = self.send_drive_buffer_velocity
                drive_fcn(self.drive_cmd_buffer)
                self.drive_cmd_buffer = None

            if self.corner_cmd_buffer:
                self.send_corner_buffer(self.corner_cmd_buffer)
                self.corner_cmd_buffer = None

            # read from roboclaws and publish
            try:
                self.read_encoder_values()
                self.enc_pub.publish(self.current_enc_vals)
            except AssertionError as read_exc:
                rospy.logwarn("Failed to read encoder values")

            # Downsample the rate of less important data
            if (counter >= 5):
                status.battery = self.read_battery()
                status.temp = self.read_temperatures()
                status.current = self.read_currents()
                status.error_status = self.read_errors()
                counter = 0

            # stop the motors if we haven't received a command in a while
            now = rospy.Time.now()
            if now - self.time_last_cmd > self.velocity_timeout:
                # rather than a hard stop, send a ramped velocity command
                self.drive_cmd_buffer = CommandDrive()
                self.send_drive_buffer_velocity(self.drive_cmd_buffer)
                self.time_last_cmd = now  # so this doesn't get called all the time

            self.status_pub.publish(status)
            counter += 1
            rate.sleep()

    def establish_roboclaw_connections(self):
        """
        Attempt connecting to the roboclaws

        :raises Exception: when connection to one or more of the roboclaws is unsuccessful
        """
        self.rc = Roboclaw(
            rospy.get_param('/motor_controller/device', "/dev/serial0"),
            rospy.get_param('/motor_controller/baud_rate', 115200))
        self.rc.Open()

        address_raw = rospy.get_param('motor_controller/addresses')
        address_list = (address_raw.split(','))
        self.address = [None] * len(address_list)
        for i in range(len(address_list)):
            self.address[i] = int(address_list[i])

        # initialize connection status to successful
        all_connected = True
        for address in self.address:
            rospy.logdebug(
                "Attempting to talk to motor controller ''".format(address))
            version_response = self.rc.ReadVersion(address)
            connected = bool(version_response[0])
            if not connected:
                rospy.logerr(
                    "Unable to connect to roboclaw at '{}'".format(address))
                all_connected = False
            else:
                rospy.logdebug(
                    "Roboclaw version for address '{}': '{}'".format(
                        address, version_response[1]))
        if all_connected:
            rospy.loginfo(
                "Sucessfully connected to RoboClaw motor controllers")
        else:
            raise Exception(
                "Unable to establish connection to one or more of the Roboclaw motor controllers"
            )

    def setup_encoders(self):
        """Set up the encoders"""
        for motor_name, properties in self.roboclaw_mapping.iteritems():
            if "corner" in motor_name:
                enc_min, enc_max = self.read_encoder_limits(
                    properties["address"], properties["channel"])
                self.encoder_limits[motor_name] = (enc_min, enc_max)
            else:
                self.encoder_limits[motor_name] = (None, None)
                self.rc.ResetEncoders(properties["address"])

    def read_encoder_values(self):
        """Query roboclaws and update current motors status in encoder ticks"""
        enc_msg = JointState()
        enc_msg.header.stamp = rospy.Time.now()
        for motor_name, properties in self.roboclaw_mapping.iteritems():
            enc_msg.name.append(motor_name)
            position = self.read_encoder_position(properties["address"],
                                                  properties["channel"])
            velocity = self.read_encoder_velocity(properties["address"],
                                                  properties["channel"])
            current = self.read_encoder_current(properties["address"],
                                                properties["channel"])
            enc_msg.position.append(
                self.tick2position(position,
                                   self.encoder_limits[motor_name][0],
                                   self.encoder_limits[motor_name][1],
                                   properties['ticks_per_rev'],
                                   properties['gear_ratio']))
            enc_msg.velocity.append(
                self.qpps2velocity(velocity, properties['ticks_per_rev'],
                                   properties['gear_ratio']))
            enc_msg.effort.append(current)

        self.current_enc_vals = enc_msg

    def corner_cmd_cb(self, cmd):
        """
        Takes the corner command and stores it in the buffer to be sent
        on the next iteration of the run() loop.
        """

        rospy.logdebug("Corner command callback received: {}".format(cmd))
        self.time_last_cmd = rospy.Time.now()
        self.corner_cmd_buffer = cmd

    def send_corner_buffer(self, cmd):
        """
        Sends the corner command to the motor controller.
        """

        # convert position to tick
        encmin, encmax = self.encoder_limits["corner_left_front"]
        left_front_tick = self.position2tick(
            cmd.left_front_pos, encmin, encmax,
            self.roboclaw_mapping["corner_left_front"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_left_front"]["gear_ratio"])
        encmin, encmax = self.encoder_limits["corner_left_back"]
        left_back_tick = self.position2tick(
            cmd.left_back_pos, encmin, encmax,
            self.roboclaw_mapping["corner_left_back"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_left_back"]["gear_ratio"])
        encmin, encmax = self.encoder_limits["corner_right_back"]
        right_back_tick = self.position2tick(
            cmd.right_back_pos, encmin, encmax,
            self.roboclaw_mapping["corner_right_back"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_right_back"]["gear_ratio"])
        encmin, encmax = self.encoder_limits["corner_right_front"]
        right_front_tick = self.position2tick(
            cmd.right_front_pos, encmin, encmax,
            self.roboclaw_mapping["corner_right_front"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_right_front"]["gear_ratio"])

        self.send_position_cmd(
            self.roboclaw_mapping["corner_left_front"]["address"],
            self.roboclaw_mapping["corner_left_front"]["channel"],
            left_front_tick)
        self.send_position_cmd(
            self.roboclaw_mapping["corner_left_back"]["address"],
            self.roboclaw_mapping["corner_left_back"]["channel"],
            left_back_tick)
        self.send_position_cmd(
            self.roboclaw_mapping["corner_right_back"]["address"],
            self.roboclaw_mapping["corner_right_back"]["channel"],
            right_back_tick)
        self.send_position_cmd(
            self.roboclaw_mapping["corner_right_front"]["address"],
            self.roboclaw_mapping["corner_right_front"]["channel"],
            right_front_tick)

    def drive_cmd_cb(self, cmd):
        """
        Takes the drive command and stores it in the buffer to be sent
        on the next iteration of the run() loop.
        """

        rospy.logdebug("Drive command callback received: {}".format(cmd))
        self.drive_cmd_buffer = cmd
        self.time_last_cmd = rospy.Time.now()

    def send_drive_buffer_velocity(self, cmd):
        """
        Sends the drive command to the motor controller, closed loop velocity commands
        """
        props = self.roboclaw_mapping["drive_left_front"]
        vel_cmd = self.velocity2qpps(cmd.left_front_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_left_middle"]
        vel_cmd = self.velocity2qpps(cmd.left_middle_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_left_back"]
        vel_cmd = self.velocity2qpps(cmd.left_back_vel, props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_right_back"]
        vel_cmd = self.velocity2qpps(cmd.right_back_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_right_middle"]
        vel_cmd = self.velocity2qpps(cmd.right_middle_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_right_front"]
        vel_cmd = self.velocity2qpps(cmd.right_front_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

    def send_position_cmd(self, address, channel, target_tick):
        """
        Wrapper around one of the send position commands

        :param address:
        :param channel:
        :param target_tick: int
        """
        cmd_args = [
            self.corner_accel, self.corner_max_vel, self.corner_accel,
            target_tick, 1
        ]
        if channel == "M1":
            return self.rc.SpeedAccelDeccelPositionM1(address, *cmd_args)
        elif channel == "M2":
            return self.rc.SpeedAccelDeccelPositionM2(address, *cmd_args)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

    def read_encoder_position(self, address, channel):
        """Wrapper around self.rc.ReadEncM1 and self.rcReadEncM2 to simplify code"""
        if channel == "M1":
            val = self.rc.ReadEncM1(address)
        elif channel == "M2":
            val = self.rc.ReadEncM2(address)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

        assert val[0] == 1
        return val[1]

    def read_encoder_limits(self, address, channel):
        """Wrapper around self.rc.ReadPositionPID and returns subset of the data

        :return: (enc_min, enc_max)
        """
        if channel == "M1":
            result = self.rc.ReadM1PositionPID(address)
        elif channel == "M2":
            result = self.rc.ReadM2PositionPID(address)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

        assert result[0] == 1
        return (result[-2], result[-1])

    def send_velocity_cmd(self, address, channel, target_qpps):
        """
        Wrapper around one of the send velocity commands

        :param address:
        :param channel:
        :param target_qpps: int
        """
        # clip values
        target_qpps = max(-self.roboclaw_overflow,
                          min(self.roboclaw_overflow, target_qpps))
        if channel == "M1":
            return self.rc.SpeedAccelM1(address, self.drive_accel, target_qpps)
        elif channel == "M2":
            return self.rc.SpeedAccelM2(address, self.drive_accel, target_qpps)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

    def read_encoder_velocity(self, address, channel):
        """Wrapper around self.rc.ReadSpeedM1 and self.rcReadSpeedM2 to simplify code"""
        if channel == "M1":
            val = self.rc.ReadSpeedM1(address)
        elif channel == "M2":
            val = self.rc.ReadSpeedM2(address)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

        assert val[0] == 1
        return val[1]

    def read_encoder_current(self, address, channel):
        """Wrapper around self.rc.ReadCurrents to simplify code"""
        if channel == "M1":
            return self.rc.ReadCurrents(address)[0]
        elif channel == "M2":
            return self.rc.ReadCurrents(address)[1]
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

    def tick2position(self, tick, enc_min, enc_max, ticks_per_rev, gear_ratio):
        """
        Convert the absolute position from ticks to radian relative to the middle position

        :param tick:
        :param enc_min:
        :param enc_max:
        :param ticks_per_rev:
        :return:
        """
        ticks_per_rad = ticks_per_rev / (2 * math.pi)
        if enc_min is None or enc_max is None:
            return tick / ticks_per_rad
        mid = enc_min + (enc_max - enc_min) / 2

        return (tick - mid) / ticks_per_rad * gear_ratio

    def position2tick(self, position, enc_min, enc_max, ticks_per_rev,
                      gear_ratio):
        """
        Convert the absolute position from radian relative to the middle position to ticks

                Clip values that are outside the range [enc_min, enc_max]

        :param position:
        :param enc_min:
        :param enc_max:
        :param ticks_per_rev:
        :return:
        """
        ticks_per_rad = ticks_per_rev / (2 * math.pi)
        if enc_min is None or enc_max is None:
            return position * ticks_per_rad
        mid = enc_min + (enc_max - enc_min) / 2
        tick = int(mid + position * ticks_per_rad / gear_ratio)

        return max(enc_min, min(enc_max, tick))

    def qpps2velocity(self, qpps, ticks_per_rev, gear_ratio):
        """
        Convert the given quadrature pulses per second to radian/s

        :param qpps: int
        :param ticks_per_rev:
        :param gear_ratio:
        :return:
        """
        return qpps * 2 * math.pi / (gear_ratio * ticks_per_rev)

    def velocity2qpps(self, velocity, ticks_per_rev, gear_ratio):
        """
        Convert the given velocity to quadrature pulses per second

        :param velocity: rad/s
        :param ticks_per_rev:
        :param gear_ratio:
        :return: int
        """
        return int(velocity * gear_ratio * ticks_per_rev / (2 * math.pi))

    def read_battery(self):
        """Read battery voltage from one of the roboclaws as a proxy for all roboclaws"""
        # roboclaw reports value in 10ths of a Volt
        return self.rc.ReadMainBatteryVoltage(self.address[0])[1] / 10.0

    def read_temperatures(self):
        temp = [None] * 5
        for i in range(5):
            # reported by roboclaw in 10ths of a Celsius
            temp[i] = self.rc.ReadTemp(self.address[i])[1] / 10.0

        return temp

    def read_currents(self):
        currents = [None] * 10
        for i in range(5):
            currs = self.rc.ReadCurrents(self.address[i])
            # reported by roboclaw in 10ths of an Ampere
            currents[2 * i] = currs[1] / 100.0
            currents[(2 * i) + 1] = currs[2] / 100.0

        return currents

    def stop_motors(self):
        """Stops all motors on Rover"""
        for i in range(5):
            self.rc.ForwardM1(self.address[i], 0)
            self.rc.ForwardM2(self.address[i], 0)

    def read_errors(self):
        """Checks error status of each motor controller, returns 0 if no errors reported"""
        err = [0] * 5
        for i in range(len(self.address)):
            err[i] = self.rc.ReadError(self.address[i])[1]
            if err[i] != 0:
                rospy.logerr(
                    "Motor controller '{}' reported error code {}".format(
                        self.address[i], err[i]))

        return err
示例#2
0
    else:
        print "failed",
    print("Speed2:"),
    if (speed2[0]):
        print speed2[1]
    else:
        print "failed "


rc.Open()
address = 0x80

version = rc.ReadVersion(address)
if version[0] == False:
    print "GETVERSION Failed"
else:
    print repr(version[1])

while (1):
    rc.SpeedAccelM1(address, 12000, 12000)
    rc.SpeedAccelM2(address, 12000, -12000)
    for i in range(0, 200):
        displayspeed()
        time.sleep(0.01)

    rc.SpeedAccelM1(address, 12000, -12000)
    rc.SpeedAccelM2(address, 12000, 12000)
    for i in range(0, 200):
        displayspeed()
        time.sleep(0.01)
示例#3
0
class SerialDriver(object):
	'''
	Class for serial UART interface to the RoboClaw Motor Controllers
	
	'''
	def __init__(self):
		rospy.loginfo("Initilizing the motor controllers..")

		self.e_stop 			= 1
		self.reg_enabled 		= 0
		self.temp 			= 0
		self.error 			= 0
		self.voltage 			= 0

		self.currents 			= [0,0]
		self._thread_lock 		= False
		
		self.prev_enc_ts 		= None
		self.prev_tick 			= [None, None]

		self.start_time			= datetime.now()
		self.left_currents		= []
		self.right_currents		= []
		self.max_left_integrator	= 0
		self.max_right_integrator 	= 0
		self.left_integrator		= 0
		self.right_integrator		= 0
		self.motor_lockout 		= 0

		self._cmd_buf_flag		= 0
		self._l_vel_cmd_buf		= 0
		self._r_vel_cmd_buf		= 0
		self.battery_percent 		= 0
		self.shutdown_warning		= False
		self.shutdown_flag		= False

		self.rate = rospy.get_param("/puffer/rate")
		self.delta_t = 2.0/self.rate
		self.rc = Roboclaw(
				rospy.get_param("/motor_controllers/serial_device_name"), 
				rospy.get_param("/motor_controllers/baud_rate")
				)

		self.rc.Open()
		self._set_operating_params()
		self._get_version()
		self.set_estop(0)					#Clears E-stop pin
		self.enable_12v_reg(1)					#Disables 12V Regulator
		self.kill_motors()					#Start the motors not moving
		
	#Private Methods
	def _get_version(self):
		'''
		Version check for communication verification to the motor controllers
		
		returns ther version number if sucessful, and 0 if not
		'''
		
		
		version = self.rc.ReadVersion(self.address)
		if version != 0:
			rospy.loginfo("[Motor __init__ ] Sucessfully connected to all Motor Controllers!")
			rospy.loginfo( version )
			rospy.set_param("/motor_controllers/firmware_version",version)
		else:
			raise Exception("Unable to establish connection to Motor controllers")
		return version

	def _set_operating_params(self):
		'''
		Sets all the operating parameters for control of PUFFER, Pinouts, and 
		safety parameters.
		
		returns None
		'''

		#GPIO settings for E-stop and Regulator Enable pins
		self.e_stop_pin = rospy.get_param("/gpio_pins/e_stop",1)
		self.reg_en_pin = rospy.get_param("/gpio_pins/reg_en",1)
		
		try:
			GPIO.setmode(GPIO.BCM)
			GPIO.setwarnings(False)
			GPIO.setup(self.e_stop_pin, GPIO.OUT)
			GPIO.setup(self.reg_en_pin, GPIO.OUT)
		except:
			pass
		#Threadlock used for serial comm to RoboClaw
		self.thread_timeout		= rospy.get_param("/threadlock/timeout")

		#Motor operating parameters
		self.wheel_d = rospy.get_param("/wheels/diameter")
		self.enc_cpr = rospy.get_param("/wheels/encoder/cts_per_rev")
		factor = rospy.get_param("/wheels/encoder/factor")
		stage_1 = rospy.get_param("/wheels/gearbox/stage1")
		stage_2 = rospy.get_param("/wheels/gearbox/stage2")
		stage_3 = rospy.get_param("/wheels/gearbox/stage3")
		
		self.accel_const = rospy.get_param("/puffer/accel_const")
		self.max_vel_per_s = rospy.get_param("/puffer/max_vel_per_s")
		self.tick_per_rev = int(self.enc_cpr * factor * stage_1 * stage_2 * stage_3)
		rospy.loginfo(self.tick_per_rev)
		rospy.set_param("tick_per_rev", self.tick_per_rev)

		self.max_cts_per_s = int((self.max_vel_per_s * self.tick_per_rev)/(math.pi * self.wheel_d))
		self.max_accel = int(self.max_cts_per_s * self.accel_const)
		
		rospy.set_param("max_cts_per_s", self.max_cts_per_s)
		rospy.set_param("max_accel", self.max_accel)
		self.address = rospy.get_param("/motor_controllers/address", 0x80)
		self.rc.SetMainVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/main/low", 12.0) * 10),int(rospy.get_param("motor_controllers/battery/main/high", 18.0 ) * 10))
		self.rc.SetLogicVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/logic/low") * 10),int(rospy.get_param("motor_controllers/battery/logic/high") * 10))
		
		self.max_current = rospy.get_param("/motor_controllers/current/max_amps")
		self.motor_lockout_time = rospy.get_param("/puffer/motor_lockout_time")
		
		m1p = rospy.get_param("/motor_controllers/m1/p")
		m1i = rospy.get_param("/motor_controllers/m1/i")
		m1d = rospy.get_param("/motor_controllers/m1/d")
		m1qpps = rospy.get_param("/motor_controllers/m1/qpps")
		m2p = rospy.get_param("/motor_controllers/m2/p")
		m2i = rospy.get_param("/motor_controllers/m2/i")
		m2d = rospy.get_param("/motor_controllers/m2/d")
		m2qpps = rospy.get_param("/motor_controllers/m2/qpps")
		

		self.battery_max_time = rospy.get_param("/battery/max_time")
		self.battery_max_volts = rospy.get_param("/battery/max_volts")
		self.battery_coef_a = rospy.get_param("/battery/a")
		self.battery_coef_b = rospy.get_param("/battery/b")
		self.battery_coef_c = rospy.get_param("/battery/c")
		self.battery_warning = rospy.get_param("/battery/warning_percent")
		self.battery_shutdown = rospy.get_param("/battery/shutdown_percent")

		self.rc.SetM1VelocityPID(self.address,m1p, m1i, m1d, m1qpps)
		self.rc.SetM2VelocityPID(self.address,m2p, m2i, m2d, m2qpps)
		
		self.rc.WriteNVM(self.address)
		time.sleep(0.001)
		self.rc.ReadNVM(self.address)
		
	def _lock_thread(self,lock):
		'''
		Checks the thread lock and then grabs it when it frees up

		no return value

		'''
		if (lock):
			start = time.time()
			while self._thread_lock:
				#rospy.loginfo("in threadlock")
				if time.time() - start > self.thread_timeout:
					raise Exception("Thread lock timeout")
				time.sleep(0.001)
			self._thread_lock = True
		else:
			self._thread_lock = False

	def _get_Temp(self):
		'''
		Gets the temperature of the motor controllers
		
		return:
		list [2] (int): Temperature values * 10 degrees C
		
		'''
		self._lock_thread(1)
		self.temp = self.rc.ReadTemp(self.address)[1]
		self._lock_thread(0)
		self.temp = int(self.temp*100)/1000.0
		
		return self.temp	

	def _get_Voltage(self):
		'''
		Gets the voltage of the motor controllers
		
		return:
		voltage (int) : Voltage values * 10 volts
		
		'''
		self._lock_thread(1)
		v = self.rc.ReadMainBatteryVoltage(self.address)[1]
		v = int(v*100)/1000.0
		if v != 0:
			self.voltage = v + 0.4     #accounts for the voltage drop in the diode
		self._lock_thread(0)
		return v

	def _get_Currents(self):
		'''
		Gets the current of the motor controllers
		
		return:
		list [2] (int): Current values * 100 Amps
		
		'''
		self._lock_thread(1)
		cur = self.rc.ReadCurrents(self.address)
		self._lock_thread(0)
		r_current = int(cur[1])/100.0
		l_current = int(cur[2])/100.0
		self.currents = [l_current,r_current]
		
		self.left_currents.insert(0, l_current)
		self.right_currents.insert(0, r_current)

		if (len(self.left_currents) > self.rate/2):
			del self.left_currents[-1]
			del self.right_currents[-1]

		left_power = 0
		right_power = 0

		for i in range(len(self.left_currents)):
			left_power += math.pow(self.left_currents[i],2) * self.delta_t
			right_power += math.pow(self.right_currents[i],2) * self.delta_t

		if (left_power >= math.pow(self.max_current,2) or right_power >= math.pow(self.max_current,2)):
			rospy.loginfo("Motor power exceeded max allowed! Disabling motors for %d seconds" %(self.motor_lockout_time))
			self.motor_lockout = 1
			self.set_estop(1)
			self.lockout_timestamp = time.time()
			
		if (self.motor_lockout and (time.time() - self.lockout_timestamp >= self.motor_lockout_time)):
			rospy.loginfo("Re-enabling the motors from timeout lock")
			self.motor_lockout = 0
			self.set_estop(0)
			
		self.left_integrator = left_power
		self.right_integrator = right_power

		self.max_left_integrator = max(self.max_left_integrator, self.left_integrator)
		self.max_right_integrator = max(self.max_right_integrator, self.right_integrator)
		
		


		#print self.left_integrator, self.right_integrator

		return self.currents
	
	def _get_Errors(self):
		'''
		Gets the error status of the motor controllers
		
		return:
		error (int): Error code for motor controller
		
		'''
		self._lock_thread(1)
		self.error = self.rc.ReadError(self.address)[1]
		self._lock_thread(0)
		return self.error
	
	def _get_Encs(self):
		'''
		Gets the encoder values of the motor controllers
		
		return:
		list [2] (int): Speed of motors in radians/s, computed at each delta T
		
		'''
		self._lock_thread(1)
		r_enc = self.rc.ReadEncM1(self.address)[1]
		l_enc = self.rc.ReadEncM2(self.address)[1]
		dt = datetime.now()
		self._lock_thread(0)
		
		if self.prev_tick == [None, None]:
			l_vel_rad_s, r_vel_rad_s = 0,0
			self.timestamp = 0
		else:
			delta_t = ((dt-self.prev_enc_ts).microseconds)/1000000.0
			self.timestamp = int(self.timestamp + (delta_t  * 100000))
			l_vel = (l_enc - self.prev_tick[0])/(delta_t)
			r_vel = (r_enc - self.prev_tick[1])/(delta_t)
			
			l_vel_rad_s = l_vel * (2 * math.pi/self.tick_per_rev)
			r_vel_rad_s = r_vel * (2 * math.pi/self.tick_per_rev)
		
			l_vel_rad_s = int(l_vel_rad_s * 1000)/1000.0
			r_vel_rad_s = int(r_vel_rad_s * 1000)/1000.0
		
		self.prev_enc_ts = dt
		self.prev_tick = [l_enc, r_enc]
		self.enc = [l_vel_rad_s, r_vel_rad_s]
		
		return l_vel_rad_s, r_vel_rad_s

	def _send_motor_cmds(self):

		l_vel = self._l_vel_cmd_buf
		r_vel = self._r_vel_cmd_buf

		l_vel *= (self.tick_per_rev/(2*math.pi))
		r_vel *= (self.tick_per_rev/(2*math.pi))

		l_vel = max(min(l_vel,self.max_cts_per_s), -self.max_cts_per_s)
		r_vel = max(min(r_vel,self.max_cts_per_s), -self.max_cts_per_s)

		if (( abs(l_vel) <= self.max_cts_per_s) and (abs(r_vel) <= self.max_cts_per_s)):
			if not self.motor_lockout:
				self._lock_thread(1)
				self.rc.SpeedAccelM1(self.address, self.max_accel, int(r_vel))
				self.rc.SpeedAccelM2(self.address, self.max_accel, int(l_vel))
				self._lock_thread(0)
		else:
			rospy.loginfo( "values not in accepted range" )
			self.send_motor_cmds(0,0)

	# Public Methods

	def set_estop(self,val):
		'''
		Sets the E-stop pin to stop Motor control movement until cleared
		
		Parameters:
		val (int): 
			0 - Clears E-stop
			1 - Sets E-stop, disabling motor movement

		no return value
		
		'''

		if (val != self.e_stop):
			if val:
				rospy.loginfo( "Enabling the E-stop")
				GPIO.output(self.e_stop_pin, 1)
				self.e_stop = 1
			else:

				rospy.loginfo( "Clearing the E-stop")
				GPIO.output(self.e_stop_pin, 0)
				self.e_stop = 0


	def battery_state_esimator(self):
		x = self.voltage
		y = math.pow(x,2) * self.battery_coef_a + self.battery_coef_b * x + self.battery_coef_c
		self.battery_percent = (100 * y/float(self.battery_max_time))
		if (self.battery_percent <= self.battery_warning):
			self.shutdown_warning = True
		else:
			self.shutdown_warning = False
		if (self.battery_percent <= self.battery_shutdown):
			self.shutdown_flag = True
			self.set_estop(1)


	def update_cmd_buf(self, l_vel, r_vel):
		'''
		Updates the command buffers and sets the flag that new command has been received
		
		Parameters:
		l_vel (int): [-0.833, 0.833] units of [rad/s]
		r_vel (int): [-0.833, 0.833] units of [rad/s]
		
		no return value
		'''
		self._l_vel_cmd_buf = l_vel
		self._r_vel_cmd_buf = r_vel
		self._cmd_buf_flag = 1
			
	def kill_motors(self):
		'''
		Stops all motors on the assembly
		'''
		self._lock_thread(1)
		self.rc.ForwardM1(self.address, 0)
		self.rc.ForwardM2(self.address, 0)
		self._lock_thread(0)

	def loop(self, counter):
		'''
		Gets the data from the motor controllers to populate as class variables
		all data function classes are private because of threadlocks
	
		Downsample non-critical values to keep serial comm line free

		Parameters:
		counter (int): 

		no return value
		
		'''

		if (self._cmd_buf_flag):
			self._send_motor_cmds()
			self._cmd_buf_flag = 0

		self._get_Encs()
		if not counter % 2:
			self._get_Currents()
		if not counter % 5:
			self._get_Errors()
		if not counter % 20:
			self._get_Temp()
			self._get_Voltage()
			self.battery_state_esimator()

	def enable_12v_reg(self, en):
		'''
		Turns on/off the 12V regulator GPIO pin
		
		Parameters:
		en (int):
			0: Disabled
			1: Enabled
		
		'''
		try:
			if (en != self.reg_enabled):
				if en:
					rospy.loginfo("Enabling the 12V Regulator")
					GPIO.output(self.reg_en_pin, 1)
					self.reg_enabled = 1
				else:
					rospy.loginfo("Disabling the 12V Regulator")
					GPIO.output(self.reg_en_pin, 0)
					self.reg_enabled = 0
		except:
			pass

	def cleanup(self):
		'''
		Cleans up the motor controller node, stopping motors and killing all threads 

		no return value
		
		'''
		rospy.loginfo("Cleaning up the motor_controller node..")
		self._thread_lock = False
		self.kill_motors()
		self.set_estop(1)
		try:
			GPIO.cleanup()
		except:
			pass
class RoboclawWrapper(object):
    """Interface between the roboclaw motor drivers and the higher level rover code"""
    def __init__(self):
        rospy.loginfo("Initializing motor controllers")

        # initialize attributes
        self.rc = None
        self.err = [None] * 5
        self.address = []
        self.current_enc_vals = None
        self.mutex = False

        self.roboclaw_mapping = rospy.get_param('~roboclaw_mapping')
        self.encoder_limits = {}
        self.establish_roboclaw_connections()
        self.killMotors()  # don't move at start
        self.setup_encoders()

        # save settings to non-volatile (permanent) memory
        for address in self.address:
            self.rc.WriteNVM(address)

        for address in self.address:
            self.rc.ReadNVM(address)

        self.corner_max_vel = 1000
        self.corner_accel = 2000
        self.roboclaw_overflow = 2**15 - 1
        accel_max = 655359
        accel_rate = 0.5
        self.accel_pos = int((accel_max / 2) + accel_max * accel_rate)
        self.accel_neg = int((accel_max / 2) - accel_max * accel_rate)
        self.errorCheck()

        self.killMotors()

        # set up publishers and subscribers
        self.corner_cmd_sub = rospy.Subscriber("/cmd_corner",
                                               CommandCorner,
                                               self.corner_cmd_cb,
                                               queue_size=1)
        self.drive_cmd_sub = rospy.Subscriber("/cmd_drive",
                                              CommandDrive,
                                              self.drive_cmd_cb,
                                              queue_size=1)
        self.enc_pub = rospy.Publisher("/encoder", JointState, queue_size=1)
        self.status_pub = rospy.Publisher("/status", Status, queue_size=1)

    def run(self):
        """Blocking loop which runs after initialization has completed"""
        rate = rospy.Rate(5)
        mutex_rate = rospy.Rate(10)

        status = Status()

        counter = 0
        while not rospy.is_shutdown():

            while self.mutex and not rospy.is_shutdown():
                mutex_rate.sleep()
            self.mutex = True

            # read from roboclaws and publish
            try:
                self.read_encoder_values()
                self.enc_pub.publish(self.current_enc_vals)
            except AssertionError as read_exc:
                rospy.logwarn("Failed to read encoder values")

            if (counter >= 10):
                status.battery = self.getBattery()
                status.temp = self.getTemp()
                status.current = self.getCurrents()
                status.error_status = self.getErrors()
                self.status_pub.publish(status)
                counter = 0

            self.mutex = False
            counter += 1
            rate.sleep()

    def establish_roboclaw_connections(self):
        """
        Attempt connecting to the roboclaws

        :raises Exception: when connection to one or more of the roboclaws is unsuccessful
        """
        self.rc = Roboclaw(
            rospy.get_param('/motor_controller/device', "/dev/serial0"),
            rospy.get_param('/motor_controller/baud_rate', 115200))
        self.rc.Open()

        address_raw = rospy.get_param('motor_controller/addresses')
        address_list = (address_raw.split(','))
        self.address = [None] * len(address_list)
        for i in range(len(address_list)):
            self.address[i] = int(address_list[i])

        # initialize connection status to successful
        all_connected = True
        for address in self.address:
            rospy.logdebug(
                "Attempting to talk to motor controller ''".format(address))
            version_response = self.rc.ReadVersion(address)
            connected = bool(version_response[0])
            if not connected:
                rospy.logerr(
                    "Unable to connect to roboclaw at '{}'".format(address))
                all_connected = False
            else:
                rospy.logdebug(
                    "Roboclaw version for address '{}': '{}'".format(
                        address, version_response[1]))
        if all_connected:
            rospy.loginfo(
                "Sucessfully connected to RoboClaw motor controllers")
        else:
            raise Exception(
                "Unable to establish connection to one or more of the Roboclaw motor controllers"
            )

    def setup_encoders(self):
        """Set up the encoders"""
        for motor_name, properties in self.roboclaw_mapping.iteritems():
            if "corner" in motor_name:
                enc_min, enc_max = self.read_encoder_limits(
                    properties["address"], properties["channel"])
                self.encoder_limits[motor_name] = (enc_min, enc_max)
            else:
                self.encoder_limits[motor_name] = (None, None)
                self.rc.ResetEncoders(properties["address"])

    def read_encoder_values(self):
        """Query roboclaws and update current motors status in encoder ticks"""
        enc_msg = JointState()
        enc_msg.header.stamp = rospy.Time.now()
        for motor_name, properties in self.roboclaw_mapping.iteritems():
            enc_msg.name.append(motor_name)
            position = self.read_encoder_position(properties["address"],
                                                  properties["channel"])
            velocity = self.read_encoder_velocity(properties["address"],
                                                  properties["channel"])
            current = self.read_encoder_current(properties["address"],
                                                properties["channel"])
            enc_msg.position.append(
                self.tick2position(position,
                                   self.encoder_limits[motor_name][0],
                                   self.encoder_limits[motor_name][1],
                                   properties['ticks_per_rev'],
                                   properties['gear_ratio']))
            enc_msg.velocity.append(
                self.qpps2velocity(velocity, properties['ticks_per_rev'],
                                   properties['gear_ratio']))
            enc_msg.effort.append(current)

        self.current_enc_vals = enc_msg

    def corner_cmd_cb(self, cmd):
        r = rospy.Rate(10)
        rospy.logdebug("Corner command callback received: {}".format(cmd))

        while self.mutex and not rospy.is_shutdown():
            r.sleep()

        self.mutex = True

        # convert position to tick
        encmin, encmax = self.encoder_limits["corner_left_front"]
        left_front_tick = self.position2tick(
            cmd.left_front_pos, encmin, encmax,
            self.roboclaw_mapping["corner_left_front"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_left_front"]["gear_ratio"])
        encmin, encmax = self.encoder_limits["corner_left_back"]
        left_back_tick = self.position2tick(
            cmd.left_back_pos, encmin, encmax,
            self.roboclaw_mapping["corner_left_back"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_left_back"]["gear_ratio"])
        encmin, encmax = self.encoder_limits["corner_right_back"]
        right_back_tick = self.position2tick(
            cmd.right_back_pos, encmin, encmax,
            self.roboclaw_mapping["corner_right_back"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_right_back"]["gear_ratio"])
        encmin, encmax = self.encoder_limits["corner_right_front"]
        right_front_tick = self.position2tick(
            cmd.right_front_pos, encmin, encmax,
            self.roboclaw_mapping["corner_right_front"]["ticks_per_rev"],
            self.roboclaw_mapping["corner_right_front"]["gear_ratio"])

        self.send_position_cmd(
            self.roboclaw_mapping["corner_left_front"]["address"],
            self.roboclaw_mapping["corner_left_front"]["channel"],
            left_front_tick)
        self.send_position_cmd(
            self.roboclaw_mapping["corner_left_back"]["address"],
            self.roboclaw_mapping["corner_left_back"]["channel"],
            left_back_tick)
        self.send_position_cmd(
            self.roboclaw_mapping["corner_right_back"]["address"],
            self.roboclaw_mapping["corner_right_back"]["channel"],
            right_back_tick)
        self.send_position_cmd(
            self.roboclaw_mapping["corner_right_front"]["address"],
            self.roboclaw_mapping["corner_right_front"]["channel"],
            right_front_tick)
        self.mutex = False

    def drive_cmd_cb(self, cmd):
        r = rospy.Rate(10)
        rospy.logdebug("Drive command callback received: {}".format(cmd))

        while self.mutex and not rospy.is_shutdown():
            r.sleep()

        self.mutex = True

        props = self.roboclaw_mapping["drive_left_front"]
        vel_cmd = self.velocity2qpps(cmd.left_front_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_left_middle"]
        vel_cmd = self.velocity2qpps(cmd.left_middle_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_left_back"]
        vel_cmd = self.velocity2qpps(cmd.left_back_vel, props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_right_back"]
        vel_cmd = self.velocity2qpps(cmd.right_back_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_right_middle"]
        vel_cmd = self.velocity2qpps(cmd.right_middle_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        props = self.roboclaw_mapping["drive_right_front"]
        vel_cmd = self.velocity2qpps(cmd.right_front_vel,
                                     props["ticks_per_rev"],
                                     props["gear_ratio"])
        self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)

        self.mutex = False

    def send_position_cmd(self, address, channel, target_tick):
        """
        Wrapper around one of the send position commands

        :param address:
        :param channel:
        :param target_tick: int
        """
        cmd_args = [
            self.corner_accel, self.corner_max_vel, self.corner_accel,
            target_tick, 1
        ]
        if channel == "M1":
            return self.rc.SpeedAccelDeccelPositionM1(address, *cmd_args)
        elif channel == "M2":
            return self.rc.SpeedAccelDeccelPositionM2(address, *cmd_args)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

    def read_encoder_position(self, address, channel):
        """Wrapper around self.rc.ReadEncM1 and self.rcReadEncM2 to simplify code"""
        if channel == "M1":
            val = self.rc.ReadEncM1(address)
        elif channel == "M2":
            val = self.rc.ReadEncM2(address)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

        assert val[0] == 1
        return val[1]

    def read_encoder_limits(self, address, channel):
        """Wrapper around self.rc.ReadPositionPID and returns subset of the data

        :return: (enc_min, enc_max)
        """
        if channel == "M1":
            result = self.rc.ReadM1PositionPID(address)
        elif channel == "M2":
            result = self.rc.ReadM2PositionPID(address)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

        assert result[0] == 1
        return (result[-2], result[-1])

    def send_velocity_cmd(self, address, channel, target_qpps):
        """
        Wrapper around one of the send velocity commands

        :param address:
        :param channel:
        :param target_qpps: int
        """
        # clip values
        target_qpps = max(-self.roboclaw_overflow,
                          min(self.roboclaw_overflow, target_qpps))
        accel = self.accel_pos
        if channel == "M1":
            return self.rc.SpeedAccelM1(address, accel, target_qpps)
        elif channel == "M2":
            return self.rc.SpeedAccelM2(address, accel, target_qpps)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

    def read_encoder_velocity(self, address, channel):
        """Wrapper around self.rc.ReadSpeedM1 and self.rcReadSpeedM2 to simplify code"""
        if channel == "M1":
            val = self.rc.ReadSpeedM1(address)
        elif channel == "M2":
            val = self.rc.ReadSpeedM2(address)
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

        assert val[0] == 1
        return val[1]

    def read_encoder_current(self, address, channel):
        """Wrapper around self.rc.ReadCurrents to simplify code"""
        if channel == "M1":
            return self.rc.ReadCurrents(address)[0]
        elif channel == "M2":
            return self.rc.ReadCurrents(address)[1]
        else:
            raise AttributeError(
                "Received unknown channel '{}'. Expected M1 or M2".format(
                    channel))

    def tick2position(self, tick, enc_min, enc_max, ticks_per_rev, gear_ratio):
        """
        Convert the absolute position from ticks to radian relative to the middle position

        :param tick:
        :param enc_min:
        :param enc_max:
        :param ticks_per_rev:
        :return:
        """
        ticks_per_rad = ticks_per_rev / (2 * math.pi)
        if enc_min is None or enc_max is None:
            return tick / ticks_per_rad
        mid = enc_min + (enc_max - enc_min) / 2

        # positive values correspond to the wheel turning left (z-axis points up)
        return -(tick - mid) / ticks_per_rad * gear_ratio

    def position2tick(self, position, enc_min, enc_max, ticks_per_rev,
                      gear_ratio):
        """
        Convert the absolute position from radian relative to the middle position to ticks

                Clip values that are outside the range [enc_min, enc_max]

        :param position:
        :param enc_min:
        :param enc_max:
        :param ticks_per_rev:
        :return:
        """
        # positive values correspond to the wheel turning left (z-axis points up)
        position *= -1
        ticks_per_rad = ticks_per_rev / (2 * math.pi)
        if enc_min is None or enc_max is None:
            return position * ticks_per_rad
        mid = enc_min + (enc_max - enc_min) / 2
        tick = int(mid + position * ticks_per_rad / gear_ratio)

        return max(enc_min, min(enc_max, tick))

    def qpps2velocity(self, qpps, ticks_per_rev, gear_ratio):
        """
        Convert the given quadrature pulses per second to radian/s

        :param qpps: int
        :param ticks_per_rev:
        :param gear_ratio:
        :return:
        """
        return qpps / (2 * math.pi * gear_ratio * ticks_per_rev)

    def velocity2qpps(self, velocity, ticks_per_rev, gear_ratio):
        """
        Convert the given velocity to quadrature pulses per second

        :param velocity: rad/s
        :param ticks_per_rev:
        :param gear_ratio:
        :return: int
        """
        return int(velocity * 2 * math.pi * gear_ratio * ticks_per_rev)

    def getBattery(self):
        return self.rc.ReadMainBatteryVoltage(self.address[0])[1]

    def getTemp(self):
        temp = [None] * 5
        for i in range(5):
            temp[i] = self.rc.ReadTemp(self.address[i])[1]
        return temp

    def getCurrents(self):
        currents = [None] * 10
        for i in range(5):
            currs = self.rc.ReadCurrents(self.address[i])
            currents[2 * i] = currs[1]
            currents[(2 * i) + 1] = currs[2]
        return currents

    def getErrors(self):
        return self.err

    def killMotors(self):
        """Stops all motors on Rover"""
        for i in range(5):
            self.rc.ForwardM1(self.address[i], 0)
            self.rc.ForwardM2(self.address[i], 0)

    def errorCheck(self):
        """Checks error status of each motor controller, returns 0 if any errors occur"""
        for i in range(len(self.address)):
            self.err[i] = self.rc.ReadError(self.address[i])[1]
        for error in self.err:
            if error:
                self.killMotors()
                rospy.logerr("Motor controller Error: \n'{}'".format(error))
示例#5
0
class Controller(object):
    def __init__(self,
                 serial_port='/dev/ttyACM0',
                 baudrate=115200,
                 mode='PWM',
                 motors_connected={
                     "left": [],
                     "right": []
                 },
                 index=0):
        self.name = "mtr_" + str(index)
        self._serial_port = serial_port
        self._baudrate = baudrate
        self._address = 0x80
        self._rc = Roboclaw(self._serial_port, self._baudrate)
        self._rc.Open()
        self.stop_service = rospy.Service(self.name + '_stop_motors', stop,
                                          self.stop_srv)
        self._mode = mode
        self.current_speed = 0
        if len(motors_connected["left"]) + len(motors_connected["right"]) > 2:
            raise ValueError(
                "You can only connect two motors to these controllers!")
        self.motors_connected = {"left": [], "right": []}
        self.motor_number = 0
        for side in motors_connected.keys():
            for motor in motors_connected[side]:
                if motor:
                    if self.motor_number == 0:
                        self.motors_connected[side].append(
                            Motor(name=str(motor), side=side))
                        self.motor_number += 1
                    elif self.motor_number == 1:
                        self.motors_connected[side].append(
                            Motor(name=str(motor), side=side))
                    else:
                        raise ValueError('Too many motors, numer is: ' +
                                         str(self.motor_number))

        ## ROS Interfaces:
        self.r = rospy.Rate(10)
        self.encoder_publisher = rospy.Publisher(self.name + '_enc_val',
                                                 encoder_values,
                                                 queue_size=1)
        self.speed_publisher = rospy.Publisher(self.name + '_speed_val',
                                               speed_values,
                                               queue_size=1)
        self.mode_service = rospy.Service(self.name + "_mode_service",
                                          set_controller_mode,
                                          self.set_controller_mode)

    def get_motors_connected(self, side=None):
        if side == None or side not in ["left", "right"]:
            return self.motors_connected
        else:
            return self.motors_connected[side]

    def get_controller_mode(self):
        return self._mode

    def set_controller_mode(self, data):
        if data.controller_mode not in ['SPEED', 'PWM']:
            rospy.logerr_once(
                'ERROR: Given controller mode is not supported, either enter "SPEED" or "PWM"'
                + ', I got: ' + str(data.controller_mode))
            return set_controller_modeResponse("Cannot change mode to: " +
                                               str(data.controller_mode))
        self._mode = data.controller_mode
        return set_controller_modeResponse(
            'Successfully changed the controller mode to {0}'.format(
                self._mode))

    ## Mapping of RC Control methods:
    def set_speed(self, side, speed):
        #print "I got: "+ str(side)+ " and " + str(speed)
        # if speed == self.current_speed:
        #     return
        if self.motors_connected[side] == []:
            return
        for motor in self.motors_connected[side]:
            if motor.speed == speed:
                return
            acceleration = 2000
            if abs(speed) > 10000 and self._mode == "SPEED":
                speed = 10000 * numpy.sign(speed)
            if abs(speed) > 16 and self._mode == "PWM":
                speed = 16 * numpy.sign(speed)
            print "speed1:" + str(speed)
            if motor.isM1() == True:
                if self._mode == "SPEED":
                    print "Sending to M1:" + str(speed)
                    try:
                        self._rc.SpeedAccelM1(self._address, acceleration,
                                              speed)
                    except:
                        rospy.loginfo("Cannot communicate with controllers")
                elif self._mode == "PWM":
                    pwm = 64 + speed
                    print "sending to M1: " + str(pwm)
                    try:
                        self._rc.ForwardBackwardM1(self._address, pwm)
                    except:
                        rospy.loginfo("Cannot communicate with controller")
            print "speed2:" + str(speed)
            if motor.isM2() == True:
                if self._mode == "SPEED":
                    print "Sending to M2:" + str(speed)
                    try:
                        self._rc.SpeedAccelM2(self._address, acceleration,
                                              speed)
                    except:
                        rospy.loginfo("Cannot communicate with controllers")
                elif self._mode == "PWM":
                    pwm = 64 + speed
                    print "sending to M2: " + str(pwm)
                    try:
                        self._rc.ForwardBackwardM2(self._address, pwm)
                    except:
                        rospy.loginfo("Cannot communicate with controller")

            motor.set_speed(speed)

    ## Stop Method
    def stop(self):
        rospy.loginfo(self.name + ': Stopping both motors')
        if self._mode == "SPEED":
            try:
                self._rc.SpeedM1M2(self._address, 0, 0)
            except:
                rospy.logerr('Could not contact controller to stop motor!!!')
        if self._mode == "PWM":
            try:
                self._rc.ForwardBackwardM1(self._address, 64)
                self._rc.ForwardBackwardM2(self._address, 64)
            except:
                rospy.logerr('Could not contact controller to stop motor!!!')
        return

    ## Stop service method
    def stop_srv(self, data):
        if data.request:
            self.stop()
            return stopResponse('Motors are stopped')
        return stopResponse(
            "Don't call stop service with false, who does that?")

    def run_publishers(self):
        rc = self._rc
        address = self._address
        encoder_message = encoder_values()
        speed_message = speed_values()

        enc1 = rc.ReadEncM1(address)
        enc2 = rc.ReadEncM2(address)
        speed1 = rc.ReadSpeedM1(address)
        speed2 = rc.ReadSpeedM2(address)
        if (enc1[0] == 1):
            encoder_message.enc1 = enc1[1]
        else:
            rospy.logerr(self.name + ': Could not read data from first motor')

        if (enc2[0] == 1):
            encoder_message.enc2 = enc2[1]
        else:
            rospy.logerr(self.name + ': Could not read data from second motor')

        if (speed1[0]):
            print speed1[1],
        else:
            rospy.logerr(self.name + ': Could not read data from encoder 2')
        print("Speed2:"),
        if (speed2[0]):
            print speed2[1]
        else:
            print "failed "
示例#6
0
class Controller():
    '''
    Controller class where it contains the PID equation
    '''
    def __init__(self, configPath):

        print 'reading config from path...' + configPath
        parser = SafeConfigParser()
        parser.read(configPath)

        #Reading the configurations
        self._P = parser.get('pid_coeff', 'P')
        self._I = parser.get('pid_coeff', 'I')
        self._D = parser.get('pid_coeff', 'D')
        self._PR = parser.get('pid_coeff', 'PR')
        self._PL = parser.get('pid_coeff', 'PL')
        self._speed = parser.get('motion', 'speed')
        self._accel = parser.get('motion', 'accel')
        self._samplingRate = parser.get('pid_coeff', 'samplingRate')
        self._port = parser.get('systems', 'port')

        self._robot = Robot()

        #setup the roboclaw here
        self._rc = Roboclaw(self._port, 115200)
        self._rc.Open()

    #Obsolete
    # def P():
    #     doc = "The P property."
    #     def fget(self):
    #         return self._P
    #     def fset(self, value):
    #         self._P = value
    #     def fdel(self):
    #         del self._P
    #     return locals()
    # P = property(**P())
    #
    # def I():
    #     doc = "The I property."
    #     def fget(self):
    #         return self._I
    #     def fset(self, value):
    #         self._I = value
    #     def fdel(self):
    #         del self._I
    #     return locals()
    # I = property(**I())
    #
    # def D():
    #     doc = "The D property."
    #     def fget(self):
    #         return self._D
    #     def fset(self, value):
    #         self._D = value
    #     def fdel(self):
    #         del self._D
    #     return locals()
    # D = property(**D())

    def _getCurDegree(self):
        return self._robot.degree

    def _getCorrection(self, toBeDegree):
        #error Negative value == moving to the left
        #error Positive value == moving to the right
        error = toBeDegree - self._getCurDegree()

        print 'Error: ' + str(error)
        ctlSig = self._P * error  #Simple P control Signal
        print 'Control signal: ' + str(ctlSig)
        return ctlSig

    def _getLeftWheelSpeed(self, toBeDegree):
        ctlSig = self._getCorrection(toBeDegree)
        speed = self._speed - self._speed * ctlSig
        return speed

    def _getRightWheelSpeed(self, toBeDegree):
        ctlSig = self._getCorrection(toBeDegree)
        speed = (self._speed + self._speed * ctlSig) * -1
        return speed

    def _moveFW(self, speedL, speedR):
        self._rc.SpeedAccelM1(constants.ADDRESS, self._accel, speedL)
        self._rc.SpeedAccelM2(constants.ADDRESS, self._accel, speedR)

    def stop(self):
        self._rc.SpeedM1(constants.ADDRESS, 0)
        self._rc.SpeedM2(constants.ADDRESS, 0)

    def moveFWControlled(self, degree, seconds):
        time = 0

        while (time < seconds):
            speedL = self._getLeftWheelSpeed(degree)
            speedR = self._getRightWheelSpeed(degree)
            self._moveFW(speedL, speedR)
            sleep(self._samplingRate / 1000)  #convert ms to secs
            time = time + self._samplingRate / 1000
        self.stop()
示例#7
0
    else:
        print "failed "


rc.Open()
address = 0x80

version = rc.ReadVersion(address)
if version[0] == False:
    print "GETVERSION Failed"
else:
    print repr(version[1])

# rc.SpeedAccelDistanceM1(address,10000,2000,200,1);
# rc.SpeedAccelDistanceM2(address,10000,-2000,200,1);
rc.SpeedAccelM1(address, 0, 1500)
rc.SpeedAccelM2(address, 0, 1500)
time.sleep(1)
rc.SpeedAccelM1(address, 10000, 0)
rc.SpeedAccelM2(address, 10000, -0)

# for i in range(0,200):
# 	displayspeed()
# 	time.sleep(0.01)
#
# rc.SpeedAccelM1(address,12000,12000)
# rc.SpeedAccelM2(address,12000,12000)
# for i in range(0,200):
# 	displayspeed()
# 	time.sleep(0.01)
示例#8
0
class comm:
    def __init__(self):
        self.rc = Roboclaw("/dev/ttyS0", 115200)
        self.rc.Open()
        self.accel = [0] * 10
        self.qpps = [None] * 10
        self.err = [None] * 5
        self.address = [128, 129, 130, 131, 132]
        '''
		version = 1
		for address in self.address:
			print("Attempting to talk to motor controller",address)
			version = version & self.rc.ReadVersion(0x80)
			print(version)
		if version != 0:
			print("[Motor__init__] Sucessfully connected to RoboClaw motor controllers")
		else:
			raise Exception("Unable to establish connection to Roboclaw motor controllers")
		'''

    def accellDriveMotors(self, speeds):

        for i in range(3):
            self.rc.SpeedAccelM1(self.address[i], 7000,
                                 speeds[i * 2] * 32767 / 500)
            self.rc.SpeedAccelM2(self.address[i], 7000,
                                 speeds[(i * 2 + 1)] * 32767 / 500)

    def getCornerEncoders(self):

        enc = []

        for i in range(4):
            index = int(math.ceil((i + 1) / 2.0) + 2)
            if not (i % 2):
                enc.append(self.rc.ReadEncM1(self.address[index])[1])
            else:
                enc.append(self.rc.ReadEncM2(self.address[index])[1])

        return enc

    def moveCorners(self, tick):

        enc = []

        speed, accel = 1000, 2000

        for i in range(4):
            index = int(math.ceil((i + 1) / 2.0) + 2)
            if not (i % 2):
                enc.append(self.rc.ReadEncM1(self.address[index])[1])
            else:
                enc.append(self.rc.ReadEncM2(self.address[index])[1])

            if (abs(tick[i] - enc[i]) < 25):
                tick[i] = -1

            if (tick[i] != -1):
                if (i % 2):
                    self.rc.SpeedAccelDeccelPositionM2(self.address[index],
                                                       accel, speed, accel,
                                                       tick[i], 1)
                else:
                    self.rc.SpeedAccelDeccelPositionM1(self.address[index],
                                                       accel, speed, accel,
                                                       tick[i], 1)

            else:
                if (i % 2): self.rc.ForwardM2(self.address[index], 0)
                else: self.rc.ForwardM1(self.address[index], 0)
示例#9
0
		print "failed " ,
	print "Speed1:",
	if(speed1[0]):
		print speed1[1],
	else:
		print "failed",
	print("Speed2:"),
	if(speed2[0]):
		print speed2[1]
	else:
		print "failed "

rc.Open()
address = 0x80

version = rc.ReadVersion(address)
if version[0]==False:
	print "GETVERSION Failed"
else:
	print repr(version[1])

i = 1


while(1):
	rc.SpeedAccelM1(address,12000,i)
	rc.SpeedAccelM2(address,12000,-i)
	displayspeed()
	i = i + 1
	print(i)