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
print repr(version[1])
while (1):
#rc.S20000
vel = 10000
acc = 5000
pwm = 128
duty = 64
#rc.SpeedAccelM1(address, acc, vel)
#rc.ForwardBackwardM2(address, 64+duty)
#rc.SpeedAccelM2(address, acc, -vel)
for i in range(0, 5):
#displayspeed()
rc.ForwardBackwardM2(address, 32)
time.sleep(0.05)
#rc.ForwardBackwardM2(address, 48)
#print rc. ReadCurrents(address)
for i in range(0, 60):
#rc.ForwardBackwardM2(address, 64-i)
time.sleep(0.05)
# rc.SpeedM1(address,-3000)
# #rc.SpeedM2(address,12000)
# for i in range(0,200):
# displayspeed()
# time.sleep(0.01)
#rc.SpeedAccelM1(address, acc, 0)
time.sleep(0.5)
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 "
import time
from roboclaw import Roboclaw
#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
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 ==='
