def main(portName):
print "Inicializando motores en modo de PRUEBA"
###Connection with ROS
rospy.init_node("motor_node")
#Suscripcion a Topicos
subSpeeds = rospy.Subscriber(
"/hardware/motors/speeds", Float32MultiArray,
callbackSpeeds) #Topico para obtener vel y dir de los motores
#Publicacion de Topicos
#pubRobPos = rospy.Publisher("mobile_base/position", Float32MultiArray,queue_size = 1)
#Publica los datos de la posición actual del robot
pubOdometry = rospy.Publisher("mobile_base/odometry",
Odometry,
queue_size=1)
#Publica los datos obtenidos de los encoders y analizados para indicar la posicion actual del robot
#Estableciendo parametros de ROS
br = tf.TransformBroadcaster(
) #Adecuando los datos obtenidos al sistema de coordenadas del robot
rate = rospy.Rate(1)
#Comunicacion serial con la tarjeta roboclaw Roboclaw
print "Roboclaw.-> Abriendo conexion al puerto serial designacion: \"" + str(
portName) + "\""
RC = Roboclaw(portName, 38400)
#Roboclaw.Open(portName, 38400)
RC.Open()
address = 0x80
print "Roboclaw.-> Conexion establecida en el puerto serila designacion \"" + portName + "\" a 38400 bps (Y)"
print "Roboclaw.-> Limpiando lecturas de enconders previas"
RC.ResetEncoders(address)
#Varibles de control de la velocidad
global leftSpeed
global rightSpeed
global newSpeedData
leftSpeed = 0 #[-1:0:1]
rightSpeed = 0 #[-1:0:1]
newSpeedData = False #Al inciar no existe nuevos datos de movmiento
speedCounter = 5
#Variables para la Odometria
robotPos = Float32MultiArray()
robotPos = [0, 0, 0] # [X,Y,TETHA]
#Ciclo ROS
#print "[VEGA]:: Probando los motores de ROTOMBOTTO"
while not rospy.is_shutdown():
if newSpeedData: #Se obtuvieron nuevos datos del topico /hardware/motors/speeds
newSpeedData = False
speedCounter = 5
#Indicando la informacion de velocidades a la Roboclaw
#Realizando trasnformacion de la informacion
leftSpeed = int(leftSpeed * 127)
rightSpeed = int(rightSpeed * 127)
#Asignando las direcciones del motor derecho
if rightSpeed >= 0:
RC.ForwardM1(address, rightSpeed)
else:
RC.BackwardM1(address, -rightSpeed)
#Asignando las direcciones del motor izquierdo
if leftSpeed >= 0:
RC.ForwardM2(address, leftSpeed)
else:
RC.BackwardM2(address, -leftSpeed)
else: #NO se obtuvieron nuevos datos del topico, los motores se detienen
speedCounter -= 1
if speedCounter == 0:
RC.ForwardM1(address, 0)
RC.ForwardM2(address, 0)
if speedCounter < -1:
speedCounter = -1
#-------------------------------------------------------------------------------------------------------
#Obteniendo informacion de los encoders
encoderLeft = RC.ReadEncM2(
address) #Falta multiplicarlo por -1, tal vez no sea necesario
encoderRight = RC.ReadEncM1(
address
) #El valor negativo obtenido en este encoder proviene de la posicion de orientacion del motor.
RC.ResetEncoders(address)
#print "Lectura de los enconders Encoders: EncIzq" + str(encoderLeft) + " EncDer" + str(encoderRight)
###Calculo de la Odometria
robotPos = calculateOdometry(robotPos, encoderLeft, encoderRight)
#pubRobPos=.publish(robotPos) ##Publicando los datos de la pocición obtenida
ts = TransformStamped()
ts.header.stamp = rospy.Time.now()
ts.header.frame_id = "odom"
ts.child_frame_id = "base_link"
ts.transform.translation.x = robotPos[0]
ts.transform.translation.y = robotPos[1]
ts.transform.translation.z = 0
ts.transform.rotation = tf.transformations.quaternion_from_euler(
0, 0, robotPos[2])
br.sendTransform((robotPos[0], robotPos[1], 0), ts.transform.rotation,
rospy.Time.now(), ts.child_frame_id,
ts.header.frame_id)
msgOdom = Odometry()
msgOdom.header.stamp = rospy.Time.now()
msgOdom.pose.pose.position.x = robotPos[0]
msgOdom.pose.pose.position.y = robotPos[1]
msgOdom.pose.pose.position.z = 0
msgOdom.pose.pose.orientation.x = 0
msgOdom.pose.pose.orientation.y = 0
msgOdom.pose.pose.orientation.z = math.sin(robotPos[2] / 2)
msgOdom.pose.pose.orientation.w = math.cos(robotPos[2] / 2)
pubOdometry.publish(msgOdom) #Publicando los datos de odometría
rate.sleep()
#Fin del WHILE ROS
#------------------------------------------------------------------------------------------------------
RC.ForwardM1(address, 0)
RC.ForwardM2(address, 0)
RC.Close()
class MotorConnection:
def __init__(self,
roboclaw_port='/dev/roboclaw',
baud_rate=115200,
bucket_address=0x80):
self.right_motor = DriveMotor(DEFAULT_RIGHT_DRIVE_MOTOR_PORT, 0)
self.left_motor = DriveMotor(DEFAULT_LEFT_DRIVE_MOTOR_PORT, 1)
self.roboclaw = Roboclaw(roboclaw_port, baud_rate)
if self.roboclaw.Open():
self.status = RoboclawStatus.CONNECTED
else:
self.status = RoboclawStatus.DISCONNECTED
print self.status
print 'MotorConnection initialized.'
self.bucketAddress = bucket_address
self.left_motor_speed = 0
self.right_motor_speed = 0
self.actuator_motor_speed = 0
self.bucket_motor_speed = 0
@staticmethod
def direction_of_speed(speed):
if speed >= 0:
return 1
else:
return -1
def are_speed_directions_equal(self, speed1, speed2):
if self.direction_of_speed(speed1) is self.direction_of_speed(speed2):
return True
else:
return False
@staticmethod
def convert_speed_to_power(speed):
if abs(speed) > MAX_MOTOR_SPEED:
return 0
else:
power_percentage = float(speed) / float(MAX_MOTOR_SPEED)
power = int(power_percentage * float(MAX_MOTOR_POWER))
return power
@staticmethod
def convert_speed_to_rpm(speed):
if abs(speed) > MAX_MOTOR_SPEED:
return 0
else:
power_percentage = float(speed) / float(MAX_MOTOR_SPEED)
power = int(power_percentage * float(MAX_DRIVE_MOTOR_RPM))
return power
def left_drive(self, speed):
print 'Left motor at speed:', speed, '%'
self.left_motor_speed = speed
rpm = self.convert_speed_to_rpm(speed)
print 'Left motor at rpm:', rpm
self.left_motor.drive(rpm)
def right_drive(self, speed):
print 'Right motor at speed:', speed, '%'
self.right_motor_speed = speed
rpm = self.convert_speed_to_rpm(speed)
print 'Right motor at rpm:', rpm
self.right_motor.drive(rpm)
def bucket_actuate(self, speed):
if not self.are_speed_directions_equal(speed,
self.actuator_motor_speed):
print 'Actuator motor speed changed direction.'
self.roboclaw.ForwardM1(self.bucketAddress, 0)
time.sleep(DEFAULT_TIME_TO_DELAY_MOTOR)
print 'Actuator motor at speed:', speed, '%'
self.actuator_motor_speed = speed
power = self.convert_speed_to_power(speed)
print 'Actuator motor at power:', power
if power >= 0:
self.roboclaw.BackwardM1(self.bucketAddress, power)
else:
self.roboclaw.ForwardM1(self.bucketAddress, abs(power))
def bucket_rotate(self, speed):
if not self.are_speed_directions_equal(speed, self.bucket_motor_speed):
print 'Bucket motor speed changed direction.'
self.roboclaw.ForwardM2(self.bucketAddress, 0)
time.sleep(DEFAULT_TIME_TO_DELAY_MOTOR)
print 'Bucket motor at speed:', speed, '%'
self.bucket_motor_speed = speed
power = self.convert_speed_to_power(speed)
print 'Bucket motor at power:', power
if power >= 0:
self.roboclaw.BackwardM2(self.bucketAddress, power)
else:
self.roboclaw.ForwardM2(self.bucketAddress, abs(power))
def parse_message(self, message):
sub_messages = motorMessageRegex.findall(message)
threads = []
for sub_message in sub_messages:
motor_prefix = sub_message[0]
speed = int(sub_message[1])
try:
if motor_prefix == SubMessagePrefix.LEFT_MOTOR:
left_motor_thread = Thread(name='leftMotorThread',
target=self.left_drive(-speed))
threads.append(left_motor_thread)
left_motor_thread.start()
elif motor_prefix == SubMessagePrefix.RIGHT_MOTOR:
right_motor_thread = Thread(name='rightMotorThread',
target=self.right_drive(speed))
threads.append(right_motor_thread)
right_motor_thread.start()
elif motor_prefix == SubMessagePrefix.ACTUATOR:
actuator_thread = Thread(name='actuatorThread',
target=self.bucket_actuate(speed))
threads.append(actuator_thread)
actuator_thread.start()
elif motor_prefix == SubMessagePrefix.BUCKET:
bucket_thread = Thread(name='bucketThread',
target=self.bucket_rotate(speed))
threads.append(bucket_thread)
bucket_thread.start()
else:
print 'MotorPrefix "', motor_prefix, '" unrecognized.'
except AttributeError:
self.status = RoboclawStatus.DISCONNECTED
print 'Roboclaw disconnected...retrying connection'
if self.roboclaw.Open():
print 'Roboclaw connected...retrying command'
self.status = RoboclawStatus.CONNECTED
self.parse_message(message)
for thread in threads:
thread.join()
def close(self):
print 'Closed connection:', self.roboclaw.Close()
class Dumping:
#--------------------------------------------------------------------
# Dumping initialization function
#
# Establish the roboclaw connection for the linear actuator
#--------------------------------------------------------------------
def __init__(self):
self.serial_num = "00320100"
try:
print(
"Searching for dumping roboclaw, this may take a few seconds..."
)
#self.enable_roboclaw()
self.roboclaw = Roboclaw("/dev/ttyACM0", 38400)
self.roboclaw.Open()
print("Dumping roboclaw connected successfully")
except:
print("Unable to find dumping roboclaw")
#--------------------------------------------------------------------
# Helper function to operate the stepper motor
#
# param: *args -- a variable set of arguments used to send commands
#--------------------------------------------------------------------
def ticcmd(self, *args):
return subprocess.check_output(['ticcmd'] + list(args))
#--------------------------------------------------------------------
# Rotate the bucket forward with the stepper motor
#--------------------------------------------------------------------
def stepper_forward(self):
new_target = -200
self.ticcmd('--exit-safe-start', '-d', self.serial_num, '--position',
str(new_target))
#--------------------------------------------------------------------
# Rotate the bucket backward with the stepper motor
#--------------------------------------------------------------------
def stepper_backward(self):
new_target = -10
self.ticcmd('--exit-safe-start', '-d', self.serial_num, '--position',
str(new_target))
#--------------------------------------------------------------------
# Stop the nucket from rotating
#--------------------------------------------------------------------
def stepper_stop(self):
self.ticcmd('-d', self.serial_num, '--reset')
#--------------------------------------------------------------------
# Extend the linear actuator forward for its full length
#--------------------------------------------------------------------
def actuator_extend(self):
if self.roboclaw != None:
self.roboclaw.BackwardM1(128, 127)
#--------------------------------------------------------------------
# Fully retract the linear actuator
#--------------------------------------------------------------------
def actuator_retract(self):
if self.roboclaw != None:
self.roboclaw.ForwardM1(128, 127)
#--------------------------------------------------------------------
# Stop the linear actuator
#--------------------------------------------------------------------
def actuator_stop(self):
if self.roboclaw != None:
self.roboclaw.ForwardM1(128, 0)
#--------------------------------------------------------------------
# A full dump algorithm
#--------------------------------------------------------------------
def full_dump(self):
self.actuator_extend()
time.sleep(12)
self.stepper_forward()
time.sleep(4)
self.stepper_backward()
time.sleep(4)
self.actuator_retract()
time.sleep(12)
#--------------------------------------------------------------------
# Enables the roboclaw to communicate on the ACM1 port
#--------------------------------------------------------------------
def enable_roboclaw(self):
self.roboclaw = Roboclaw("/dev/ttyACM0", 38400)
self.roboclaw.Open()
#--------------------------------------------------------------------
# Disables the roboclaw to communicate on the ACM1 port
#--------------------------------------------------------------------
def disable_roboclaw(self):
self.actuator_stop()
time.sleep(0.1)
self.roboclaw.Close()
#--------------------------------------------------------------------
# Disengages the stepper motor by reseting the state
#--------------------------------------------------------------------
def disable_stepper(self):
self.ticcmd('-d', self.serial_num, '--reset')