class Motors:
def __init__(self, config):
roboclaw_vid = 0x03EB # VID of Roboclaw motor driver in hex
for port in comports():
if port.vid == roboclaw_vid:
self.rc = Roboclaw(port.device, 0x80)
break
else:
raise IOError("Roboclaw motor driver not found")
self.rc.Open()
self.address = 0x80
version = self.rc.ReadVersion(self.address)
self.l_ticks_per_m = config['ticks_per_m'][
'l'] # number of left encoder ticks per m traveled
self.r_ticks_per_m = config['ticks_per_m'][
'r'] # number of right encoder ticks per m traveled
self.track_width = config[
'track_width'] # width between the two tracks, in m
self.mapping = config['mapping']
if self.mapping not in ('rl', 'lr'):
raise ValueError("Invalid motor mapping '{self.mapping}'")
self.last_setpoint = None
if version[0] == False:
raise IOError("Roboclaw motor driver: GETVERSION failed")
else:
print(f"Roboclaw: {version[1]}")
def drive(self, speed, angle):
"""
Set the speed of the robot. They maintain this speed until stopped.
speed: Forward speed in m/s
angle: Clockwise angular rate in radians/s
"""
max_angle = np.pi / 2
angle = max(-max_angle, min(angle, max_angle))
left_speed = int(
(speed - self.track_width / 2 * angle) * self.l_ticks_per_m)
right_speed = int(
(speed + self.track_width / 2 * angle) * self.r_ticks_per_m)
if self.last_setpoint != (left_speed, right_speed):
self.last_setpoint = (left_speed, right_speed)
if self.mapping == "rl":
self.rc.SpeedM1M2(self.address, right_speed, left_speed)
else:
self.rc.SpeedM1M2(self.address, left_speed, right_speed)
def stop(self):
self.last_setpoint = None
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 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 "
roboclaw.BackwardM1(address,40)
roboclaw.ForwardM2(address,40)
elif motor_direction == "neither":
print("Neutral")
print (distance)
roboclaw.SpeedDistanceM1M2(address,0,0,0,0,1)
if (distance >35):
roboclaw.SpeedDistanceM1M2(0x81,-150,10,-150,10,1)#replace with something more bulletfproof
if (distance > 20 and distance <35):
print ('pickup object')
#roboclaw.BackwardM1M2(0x81,64)
#distanceMove = float(distance/35) * 150 #integrate a better value for how far
roboclaw.SpeedDistanceM2(0x80,-1000,500,1)
roboclaw.SpeedDistanceM1M2(0x81,-150,300,-150,300,1)
sleep(3)
roboclaw.SpeedM1M2(0x80,0,0)
roboclaw.SpeedDistanceM2(0x80,1000,500,1)
#rest of the code
sleep(3)
roboclaw.SpeedM1M2(0x80,0,0)
roboclaw.SpeedM1M2(0x81,0,0)
currentDetection = "face"
client.publish("voice", "robot")
sleep(1)
client.publish("voice", "face")
p.ChangeDutyCycle(6)
time.sleep(0.5)
p.ChangeDutyCycle(0)
sleep(8)#wait for image data to roll in
#find an easy way to do the whole reset of data points and servo angle done by the on message part.
else:
