def main(): global speed speed = 64 #half the [0-127] to get 0 speed in ForwardBackward command msgAckermann = AckermannDrive() rospy.init_node('mobile_base_node', anonymous=True) rospy.Subscriber("/ackermann", AckermannDrive, callbackAckermann) rate = rospy.Rate(10) rc = Roboclaw("/dev/ttyACM1", 115200) rc.Open() address = 0x80 br = tf.TransformBroadcaster() autobot_x = 0 while not rospy.is_shutdown(): autobot_x += speed * 0.001 msgAckermann.speed = speed rc.ForwardBackwardM1(address, int(msgAckermann.speed)) #0 power forward = 64 rc.ForwardBackwardM2(address, int(-msgAckermann.speed)) #0 power backward = 64 br.sendTransform((autobot_x, 0, 0), tf.transformations.quaternion_from_euler(0, 0, 0), rospy.Time.now(), "base_link", "odom") rate.sleep()
address = 0x80 while (1): rc.ForwardM1(address, 32) #1/4 power forward rc.BackwardM2(address, 32) #1/4 power backward time.sleep(2) rc.BackwardM1(address, 32) #1/4 power backward rc.ForwardM2(address, 32) #1/4 power forward time.sleep(2) rc.BackwardM1(address, 0) #Stopped rc.ForwardM2(address, 0) #Stopped time.sleep(2) m1duty = 16 m2duty = -16 rc.ForwardBackwardM1(address, 64 + m1duty) #1/4 power forward rc.ForwardBackwardM2(address, 64 + m2duty) #1/4 power backward time.sleep(2) m1duty = -16 m2duty = 16 rc.ForwardBackwardM1(address, 64 + m1duty) #1/4 power backward rc.ForwardBackwardM2(address, 64 + m2duty) #1/4 power forward time.sleep(2) rc.ForwardBackwardM1(address, 64) #Stopped rc.ForwardBackwardM2(address, 64) #Stopped time.sleep(2)
import time from roboclaw import Roboclaw #Windows comport name #rc = Roboclaw("COM3",115200) #Linux comport name Frontal = Roboclaw("/dev/ttyACM0", 115200) #Para los motores frontales Trasero = Roboclaw("/dev/ttyACM1", 115200) #Para los motores traseros Frontal.Open() Trasero.Open() address = 0x80 rep = 0 while (rep < 1): Frontal.ForwardM1(address, 64) #1/4 power forward Frontal.ForwardM2(address, 64) #1/4 power forward Trasero.ForwardM1(address, 64) #1/4 power forward Trasero.ForwardM2(address, 64) #1/4 power forward time.sleep(2) Frontal.ForwardBackwardM1(address, 64) #Stopped Frontal.ForwardBackwardM2(address, 64) #Stopped Trasero.ForwardBackwardM1(address, 64) #Stopped Trasero.ForwardBackwardM2(address, 64) #Stopped time.sleep(2) rep = rep + 1
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 "
#Windows comport name #rc = Roboclaw("COM9",115200) #Linux comport name rc = Roboclaw("/dev/ttyACM0", 9600) rc.Open() address = 0x80 rc.ForwardBackwardM2(address, 64 + 63) #full forwards print "running forward" time.sleep(3) rc.ForwardBackwardM2(address, 64) #stopped print "stopped" rc.ForwardBackwardM2(address, 64 - 63) #full back print "running backwards" time.sleep(3) rc.ForwardBackwardM2(address, 64) #stopped print "stopped" rc.ForwardBackwardM1(address, 64 + 32) #half forwards print "running half forward" time.sleep(3) rc.ForwardBackwardM1(address, 64 + 0) #stopped print "stopped" rc.ForwardBackwardM1(address, 64 - 32) #half back print "running half backwards" time.sleep(3) rc.ForwardBackwardM1(address, 64 + 0) #stopped print "stopped"
distance1=int(distance), speed2=int(speed), distance2=int(distance), buffer=int(1)) print "=== accelerate command issued ===" rc.SpeedAccelDistanceM1M2(address=rc_address, accel=int(655360), speed1=int(0), distance1=int(0), speed2=int(0), distance2=int(0), buffer=int(0)) print "=== decelarate command issued ===" buffers = (0, 0, 0) while (buffers[1] != 0x80 and buffers[2] != 0x80): displayspeed() buffers = rc.ReadBuffers(rc_address) # time.sleep(0.01) print "=== all commands complete ==" rc.ForwardBackwardM1(rc_address, 64) # Stopped rc.ForwardBackwardM2(rc_address, 64) # Stopped count = 0 while (count < 50): displayspeed() time.sleep(0.01) count += 1 print '=== experiment finished ==='