def main():
rospy.init_node('mobile_base_node', anonymous=True)
rospy.Subscriber(
"/cmd_vel", Twist,
cmd_vel_callback) #the value in /cmd_vel gows from -0.5 to 0.5 (m/S)
rate = rospy.Rate(10)
rc = Roboclaw("/dev/ttyACM1", 115200)
rc.Open()
address = 0x80
br = tf.TransformBroadcaster()
autobot_x = 0
while not rospy.is_shutdown():
#rospy.loginfo(msg.linear.x)
autobot_x += msg.linear.x * 0.1
if msg.linear.x > 0:
rc.ForwardM1(address,
int(msg.linear.x * 100)) #1/4 power forward = 32
rc.BackwardM2(address,
int(msg.linear.x * 100)) #1/4 power backward = 32
elif msg.linear.x < 0:
rc.BackwardM1(address, int(msg.linear.x * (-100)))
rc.ForwardM2(address, int(msg.linear.x * (-100)))
else:
rc.ForwardM1(address, int(msg.linear.x))
rc.BackwardM2(address, int(msg.linear.x))
br.sendTransform((autobot_x, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "base_link", "odom")
#print "Sending transform"
rate.sleep()
class RoboClawAdvance:
def __init__(self):
self.PWM_MAX = 127
self.address = 0x80
self.roboclaw = Roboclaw("/dev/ttyS1", 38400)
self.roboclaw.Open()
def MotorDrive1(self, speed, direction):
rpm = int(abs(speed) * direction)
if speed >= 0:
# Reverse
self.roboclaw.ForwardM1(self.address, rpm)
else:
self.roboclaw.BackwardM1(self.address, rpm)
def MotorDrive2(self, speed, direction):
rpm = int(abs(speed) * direction)
if speed >= 0:
self.roboclaw.ForwardM2(self.address, rpm)
else:
# Forward / stoppe
self.roboclaw.BackwardM2(self.address, rpm)
import time
from roboclaw import Roboclaw
#Windows comport name
rc = Roboclaw("COM11", 115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open()
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
from roboclaw import Roboclaw
from time import sleep
if __name__ == "__main__":
address = 0x80
roboclaw = Roboclaw("/dev/ttyS0", 38400)
roboclaw.Open()
roboclaw.SetM1VelocityPID(0x81,21.9,15.53,0,375)
roboclaw.SetM2VelocityPID(0x81,21.9,15.53,0,375)
while True:
roboclaw.BackwardM1(0x81,70)
roboclaw.BackwardM2(0x81,70)
sleep(3)
roboclaw.ForwardM1(0x81,55)
roboclaw.BackwardM2(0x81,55)
sleep(2)
roboclaw.BackwardM1(0x81,70)
roboclaw.BackwardM2(0x81,70)
sleep(3)
roboclaw.ForwardM1(0x81,0)
roboclaw.ForwardM2(0x81,0)
sleep(10)
import time
from roboclaw import Roboclaw
#Windows comport name
rc = Roboclaw("COM11", 115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc.Open()
address = 0x80
while (1):
rc.ForwardM1(address, 32) #1/4 power forward
rc.BackwardM2(address, 32)
time.sleep(2)
class motor_driver_ada:
def __init__(self, log):
self.lfbias = 68 # experimentally determined for 'Spot 2'
self.lrbias = 60
self.rrbias = 57
self.rfbias = 60
self.left_limit = -36
self.right_limit = 36
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.rr_motor = kit.servo[0]
self.rf_motor = kit.servo[1]
self.lf_motor = kit.servo[2]
self.lr_motor = kit.servo[3]
self.rr_motor.actuation_range = 120
self.rf_motor.actuation_range = 120
self.lf_motor.actuation_range = 120
self.lr_motor.actuation_range = 120
self.rr_motor.set_pulse_width_range(700, 2300)
self.rf_motor.set_pulse_width_range(700, 2300)
self.lf_motor.set_pulse_width_range(700, 2300)
self.lr_motor.set_pulse_width_range(700, 2300)
self.log = log
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.lf_motor.angle = self.rfbias
self.rf_motor.angle = self.lfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.stop_all()
def diag(self):
print("servo rr =" + str(self.rr_motor.angle))
print("servo rf =" + str(self.rf_motor.angle))
print("servo lf =" + str(self.lf_motor.angle))
print("servo lr =" + str(self.lr_motor.angle))
# self.turn_motor(0x80, vel, voc, 1)
def turn_motor(self, address, v, av1, av2):
v1 = int(v * av1)
v2 = int(v * av2)
if v >= 0:
self.rc.ForwardM1(address, v1)
self.rc.ForwardM2(address, v2)
else:
self.rc.BackwardM1(address, abs(v1))
self.rc.BackwardM2(address, abs(v2))
# print("m1, m2 = "+str(v1)+", "+str(v2))
def stop_all(self):
self.turn_motor(0X80, 0, 0, 0)
self.turn_motor(0X81, 0, 0, 0)
self.turn_motor(0X82, 0, 0, 0)
def motor_speed(self):
speed1 = self.rc.ReadSpeedM1(0x80)
speed2 = self.rc.ReadSpeedM2(0x80)
self.log.write("motor speed = %d, %d\n", speed1, speed2)
speed1 = self.rc.ReadSpeedM1(0x81)
speed2 = self.rc.ReadSpeedM2(0x81)
self.log.write("motor speed = %d, %d\n", speed1, speed2)
speed1 = self.rc.ReadSpeedM1(0x82)
speed2 = self.rc.ReadSpeedM2(0x82)
self.log.write("motor speed = %d, %d\n", speed1, speed2)
# based on speed & steer, command all motors
def motor(self, speed, steer):
# print("Motor speed, steer "+str(speed)+", "+str(steer))
if (steer < self.left_limit):
steer = self.left_limit
if (steer > self.right_limit):
steer = self.right_limit
# vel = speed * 1.27
vel = speed * 1.26
voc = 0
vic = 0
#roboclaw speed limit 0 to 127
# see BOT-2/18 (181201)
# rechecked 200329
if steer != 0: #if steering angle not zero, compute angles, wheel speed
angle = math.radians(abs(steer))
ric = self.d3 / math.sin(angle) #turn radius - inner corner
rm = ric * math.cos(angle) + self.d1 #body central radius
roc = math.sqrt((rm + self.d1)**2 + self.d3**2) #outer corner
rmo = rm + self.d4 #middle outer
rmi = rm - self.d4 #middle inner
phi = math.degrees(math.asin(self.d3 / roc))
if steer < 0:
phi = -phi
voc = roc / rmo #velocity corners & middle inner
vic = ric / rmo
vim = rmi / rmo
# SERVO MOTORS ARE COUNTER CLOCKWISE
# left turn
if steer < 0:
self.lf_motor.angle = self.lfbias - steer
self.rf_motor.angle = self.rfbias - phi
self.lr_motor.angle = self.lrbias + steer
self.rr_motor.angle = self.rrbias + phi
self.turn_motor(0x80, vel, voc, 1) #RC 1 - rf, rm
self.turn_motor(0x81, vel, vim, vic) #RC 2 - lm, lf
self.turn_motor(0x82, vel, voc, vic) #RC 3 - rr, lr
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#right turn
elif steer > 0:
self.lf_motor.angle = self.lfbias - phi
self.rf_motor.angle = self.rfbias - steer
self.lr_motor.angle = self.lrbias + phi
self.rr_motor.angle = self.rrbias + steer
self.turn_motor(0x80, vel, vic, vim)
self.turn_motor(0x81, vel, 1, voc)
self.turn_motor(0x82, vel, vic, voc)
# print("80 vic, vim ",vic,vim)
# print("81 vic, voc ",vic,voc)
# print("82 vom, voc ", 1, voc)
# cstr = "v, vout, vin %f %f %f\n" % (vel, voc, vic)
# self.log.write(cstr)
#straight ahead
else:
self.lf_motor.angle = self.lfbias
self.rf_motor.angle = self.rfbias
self.lr_motor.angle = self.lrbias
self.rr_motor.angle = self.rrbias
self.turn_motor(0x80, vel, 1, 1)
self.turn_motor(0x81, vel, 1, 1)
self.turn_motor(0x82, vel, 1, 1)
# print("v, vout, vin "+str(vel)+", "+str(voc)+", "+str(vic))
# self.diag()
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 Agitator(object):
'''Class for controlling the EXPRES fiber agitator
Inputs
------
comport : str
The hardware COM Port for the Roboclaw motor controller
Public Methods
--------------
start(exp_time, timeout, rot1, rot2):
Threaded agitation
start_agitation(exp_time, rot1, rot2):
Unthreaded agitation
stop():
Stop either threaded or unthreaded agitation
stop_agitation():
Hard-stop agitation but will not close thread
'''
def __init__(self, comport=__DEFAULT_PORT__):
self._rc = Roboclaw(comport=comport,
rate=__DEFAULT_BAUD_RATE__,
addr=__DEFAULT_ADDR__,
timeout=__DEFAULT_TIMEOUT__,
retries=__DEFAULT_RETRIES__,
inter_byte_timeout=__DEFAULT_INTER_BYTE_TIMEOUT__)
# Create a logger for the agitator
self.logger = logging.getLogger('expres_agitator')
self.logger.setLevel(logging.DEBUG)
# Create file handler to log all messages
try:
fh = logging.handlers.TimedRotatingFileHandler(
'C:/Users/admin/agitator_logs/agitator.log',
when='D',
interval=1,
utc=True,
backupCount=5)
except FileNotFoundError:
fh = logging.handlers.TimedRotatingFileHandler('agitator.log',
when='D',
interval=1,
utc=True,
backupCount=5)
fh.setLevel(logging.DEBUG)
# Create console handler to log info messages
ch = logging.StreamHandler()
ch.setLevel(logging.INFO)
# create formatter and add it to the handlers
formatter = logging.Formatter(
'%(asctime)s - %(name)s - %(levelname)s - %(message)s')
fh.setFormatter(formatter)
ch.setFormatter(formatter)
# add the handlers to the logger
self.logger.addHandler(fh)
self.logger.addHandler(ch)
self.thread = None # In case stop() is called before a thread is created
self.stop_event = Event() # Used for stopping threads
self.stop_agitation() # Just to make sure
def __del__(self):
'''
When the object is deleted, make sure all threads are closed and
agitator is stopped. Unfortunately, these actions cannot be logged
because the logger closes by the time __del__ is called...
'''
self.stop(verbose=False)
self.stop_agitation(verbose=False)
def threaded_agitation(self, exp_time, timeout, **kwargs):
'''Threadable function allowing stop event'''
self.logger.info(
'Starting agitator thread for {}s exposure with {}s timeout'.
format(exp_time, timeout))
self.start_agitation(exp_time, **kwargs)
t = 0
start_time = time.time()
while not self.stop_event.is_set() and t < timeout:
time.sleep(1)
t = time.time() - start_time
self.logger.info('{}/{}s for {}s exposure. I1: {}, I2: {}'.format(
round(t, 1), timeout, exp_time, self.current1, self.current2))
self.stop_agitation()
self.stop_event.clear() # Allow for future agitation events
def start(self, exp_time=60.0, timeout=None, **kwargs):
'''
Start a thread that starts agitation and stops if a stop event is
called or if the timeout is reached
'''
self.stop() # To close any previously opened threads
if timeout is None: # Allow for some overlap time
timeout = exp_time + 10.0
self.thread = Thread(target=self.threaded_agitation,
args=(exp_time, timeout),
kwargs=kwargs)
self.thread.start()
def stop(self, verbose=True):
'''Stop the agitation thread if it is running'''
if self.thread is not None and self.thread.is_alive():
while self.voltage1 > 0 or self.voltage2 > 0:
if verbose:
self.logger.info('Attempting to stop threaded agitation')
self.stop_event.set()
self.thread.join(2)
if self.voltage1 > 0 or self.voltage2 > 0:
# As a backup in case something went wrong
if verbose:
self.logger.error(
'Something went wrong when trying to stop threaded agitation. Forcing agitator to stop.'
)
self.stop_agitation()
def start_agitation(self, exp_time=60.0, rot=None):
'''Set the motor voltages for the given number of rotations in exp_time'''
if exp_time <= 0:
self.logger.warning(
'Non-positive exposure time given to agitator object')
self.stop_agitation()
return
if rot is None:
rot = 0.5 * exp_time
if rot <= 0:
self.logger.warning(
'Non-positive rotation number given to agitator object')
self.stop_agitation()
return
freq1 = rot / exp_time
freq2 = 0.9 * rot / exp_time
self._freq = freq1
self.logger.info('Starting agitation at approximately {} Hz'.format(
self._freq))
self.set_voltage1(Motor1.calc_voltage(self.battery_voltage, freq1))
self.set_voltage2(Motor2.calc_voltage(self.battery_voltage, freq2))
def stop_agitation(self, verbose=True):
'''Set both motor voltages to 0'''
if verbose:
self.logger.info('Stopping agitation')
self.set_voltage(0)
self._freq = 0
def set_voltage(self, voltage):
'''Set both motor voltages to the given voltage'''
self.set_voltage1(voltage)
self.set_voltage2(voltage)
# Getter for the frequency
def get_freq(self):
self.logger.info('Requesting frequency')
return self._freq
# Getters and setters for the motor voltages
def get_voltage1(self):
return self._voltage1
def set_voltage1(self, voltage):
battery_voltage = self.battery_voltage
if abs(voltage) > battery_voltage:
voltage = np.sign(voltage) * battery_voltage
if voltage >= 0:
self._rc.ForwardM1(int(voltage / battery_voltage * 127))
else:
self._rc.BackwardM1(int(-voltage / battery_voltage * 127))
self._voltage1 = voltage
voltage1 = property(get_voltage1, set_voltage1)
def get_voltage2(self):
return self._voltage2
def set_voltage2(self, voltage):
battery_voltage = self.battery_voltage
if abs(voltage) > battery_voltage:
voltage = np.sign(voltage) * battery_voltage
if voltage >= 0:
self._rc.ForwardM2(int(voltage / battery_voltage * 127))
else:
self._rc.BackwardM2(int(-voltage / battery_voltage * 127))
self._voltage2 = voltage
voltage2 = property(get_voltage2, set_voltage2)
def get_max_voltage(self):
return self._rc.ReadMinMaxMainVoltages()[2] / 10
def set_max_voltage(self, voltage):
self._rc.SetMainVoltages(int(self.min_voltage * 10), int(voltage * 10))
max_voltage = property(get_max_voltage, set_max_voltage)
def get_min_voltage(self):
return self._rc.ReadMinMaxMainVoltages()[1] / 10
def set_min_voltage(self, voltage):
self._rc.SetMainVoltages(int(voltage * 10), int(self.max_voltage * 10))
min_voltage = property(get_min_voltage, set_min_voltage)
# Getter for the motor controller power source voltage
def get_battery_voltage(self):
return self._rc.ReadMainBatteryVoltage()[1] / 10
battery_voltage = property(get_battery_voltage)
# Getters and setters for the motor currents
def get_current1(self):
return self._rc.ReadCurrents()[1] / 100
current1 = property(get_current1)
def get_current2(self):
return self._rc.ReadCurrents()[2] / 100
current2 = property(get_current2)
def get_max_current1(self):
return self._rc.ReadM1MaxCurrent()[1] / 100
def set_max_current1(self, current):
self._rc.SetM1MaxCurrent(current)
max_current1 = property(get_max_current1, set_max_current1)
def get_max_current2(self):
return self._rc.ReadM2MaxCurrent()[1] / 100
def set_max_current2(self, current):
self._rc.SetM2MaxCurrent(current)
max_current2 = property(get_max_current2, set_max_current2)
while (rep < 1):
#adelante
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)
#para
Frontal.ForwardBackwardM1(address, 64) #Stopped
Frontal.ForwardBackwardM2(address, 64) #Stopped
# Trasero.ForwardBackwardM1(address,64) #Stopped
# Trasero.ForwardBackwardM2(address,64) #Stopped
time.sleep(2)
#atras
Frontal.BackwardM1(address, 64) #1/4 power forward
Frontal.BackwardM2(address, 64) #1/4 power forward
# Trasero.BackwardM1(address,64) #1/4 power forward
# Trasero.BackwardM2(address,64) #1/4 power forward
time.sleep(2)
#para
Frontal.ForwardBackwardM1(address, 64) #Stopped
Frontal.ForwardBackwardM2(address, 64) #Stopped
# Trasero.ForwardBackwardM1(address,64) #Stopped
# Trasero.ForwardBackwardM2(address,64) #Stopped
time.sleep(2)
#izquierda
Frontal.ForwardM1(address, 64) #1/4 power forward
Frontal.BackwardM2(address, 64) #1/4 power forward
# Trasero.BackwardM1(address,64) #1/4 power forward
# Trasero.ForwardM2(address,64) #1/4 power forward
time.sleep(2)
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 motor_driver:
def __init__(self):
self.i2c = busio.I2C(SCL, SDA)
self.pca = PCA9685(self.i2c)
self.pca.frequency = 50
self.br_motor = servo.Servo(self.pca.channels[0],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.fr_motor = servo.Servo(self.pca.channels[1],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.fl_motor = servo.Servo(self.pca.channels[2],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.bl_motor = servo.Servo(self.pca.channels[3],
actuation_range=119,
min_pulse=700,
max_pulse=2300)
self.rc = Roboclaw("/dev/ttyS0", 115200)
i = self.rc.Open()
self.d1 = 7.254 #C/L to corner wheels
self.d2 = 10.5 #mid axle to fwd axle
self.d3 = 10.5 #mid axle to rear axle
self.d4 = 10.073 #C/L to mid wheels
self.fl_motor.angle = bias
self.fr_motor.angle = bias
self.bl_motor.angle = bias
self.br_motor.angle = bias
def diag(self):
print("servo br =" + str(self.br_motor.angle))
print("servo fr =" + str(self.fr_motor.angle))
print("servo fl =" + str(self.fl_motor.angle))
print("servo bl =" + str(self.bl_motor.angle))
# self.turn_motor(0x80, vel, voc, 1)
def turn_motor(self, address, v, av1, av2):
v1 = int(v * av1)
v2 = int(v * av2)
if v >= 0:
self.rc.ForwardM1(address, v1)
self.rc.ForwardM2(address, v2)
else:
self.rc.BackwardM1(address, abs(v1))
self.rc.BackwardM2(address, abs(v2))
# print("m1, m2 = "+str(v1)+", "+str(v2))
def stop_all(self):
self.turn_motor(0X80, 0, 0, 0)
self.turn_motor(0X81, 0, 0, 0)
self.turn_motor(0X82, 0, 0, 0)
def motor_speed(self):
speed = self.rc.ReadSpeedM1(0x82)
print("motor speed =" + str(speed))
speed = self.rc.ReadSpeedM2(0x81)
print("motor speed =" + str(speed))
# based on speed & steer, command all motors
def motor(self, speed, steer):
# print("Motor speed, steer "+str(speed)+", "+str(steer))
if (steer < left_limit):
steer = left_limit
if (steer > right_limit):
steer = right_limit
# vel = speed * 1.27
vel = speed * 1.26
voc = 0
vic = 0
#roboclaw speed limit -127 to 127
if steer != 0: #if steering angle not zero, compute angles, wheel speed
angle = math.radians(abs(steer))
ric = self.d3 / math.sin(angle) #turn radius - inner corner
rm = ric * math.cos(angle) + self.d1 #body central radius
roc = math.sqrt((rm + self.d1)**2 + self.d3**2) #outer corner
rmo = rm + self.d4 #middle outer
rmi = rm - self.d4 #middle inner
phi = math.degrees(math.asin(self.d3 / roc))
if steer < 0:
phi = -phi
voc = roc / rmo #velocity corners & middle inner
vic = ric / rmo
vim = rmi / rmo
# SERVO MOTORS ARE COUNTER CLOCKWISE
# left turn
if steer < 0:
self.fl_motor.angle = bias - steer
self.fr_motor.angle = bias - phi
self.bl_motor.angle = bias + steer
self.br_motor.angle = bias + phi
self.turn_motor(0x80, vel, vic, vim)
self.turn_motor(0x81, vel, vic, voc)
self.turn_motor(0x82, vel, 1, voc)
#right turn
elif steer > 0:
self.fl_motor.angle = bias - phi
self.fr_motor.angle = bias - steer
self.bl_motor.angle = bias + phi
self.br_motor.angle = bias + steer
self.turn_motor(0x80, vel, voc, 1)
self.turn_motor(0x81, vel, voc, vic)
self.turn_motor(0x82, vel, vim, vic)
#straight ahead
else:
self.fl_motor.angle = bias
self.fr_motor.angle = bias
self.bl_motor.angle = bias
self.br_motor.angle = bias
self.turn_motor(0x80, vel, 1, 1)
self.turn_motor(0x81, vel, 1, 1)
self.turn_motor(0x82, vel, 1, 1)
LAButtonU = 1
while (LAButtonU == 1):
roboclaw.ForwardM2(LinearActuators, 127)
roboclaw.ForwardM1(LinearActuators, 127)
print("Linear Actuators Up")
if joystick.get_button(2) == 0:
roboclaw.ForwardM1(LinearActuators, 0)
roboclaw.ForwardM2(LinearActuators, 0)
LAButtonU = 0
break
if joystick.get_button(3) == 1:
LAButtonD = 1
while (LAButtonD == 1):
roboclaw.BackwardM1(LinearActuators, 127)
roboclaw.BackwardM2(LinearActuators, 127)
print("Linear Actuators Down")
if joystick.get_button(3) == 0:
roboclaw.BackwardM1(LinearActuators, 0)
roboclaw.BackwardM2(LinearActuators, 0)
LAButtonD = 0
break
if (joystick.get_button(0) == 1):
DButton = 1
while (DButton == 1):
roboclaw.ForwardM1(BigAssChainsaw, 127)
print("Start of Death Device")
if (joystick.get_button(0) == 0):
roboclaw.ForwardM1(BigAssChainsaw, 0)
DButton = 0
rc.ForwardM2(128, 20)
rc.BackwardM1(129, 20)
rc.ForwardM2(129, 20)
rc.ForwardM1(130, 20)
rc.ForwardM2(130, 20)
print('mooooooove')
elif line == 'a':
rc.ForwardM1(131, 40)
time.sleep(1)
rc.BackwardM1(131, 40)
rc.BackwardM1(131, 0)
rc.ForwardM2(131, 40)
time.sleep(1)
rc.BackwardM2(131, 40)
rc.BackwardM2(131, 0)
rc.ForwardM1(132, 40)
time.sleep(1)
rc.BackwardM1(132, 40)
rc.BackwardM1(132, 0)
rc.ForwardM2(132, 40)
time.sleep(1)
rc.BackwardM2(132, 40)
rc.BackwardM2(132, 0)
elif line == 's':
rc.BackwardM1(128, 20)
rc.BackwardM2(128, 20)
while True:
if (needToSleep == False):
if hotwordTime<time.time():
GPIO.output(20, GPIO.LOW)
GPIO.output(21, GPIO.LOW)
print "voice timeout"
if (currentDetection == "face"):
if (time.time() > startTime + 10):
motor_direction = "unsure"
if (motor_direction == "left"):
print ("R")
try:
roboclaw.ForwardM1(address,40)
roboclaw.BackwardM2(address,40)
except:
print("")
elif (motor_direction == "right"):
print ("L")
try:
roboclaw.ForwardM2(address,40)
roboclaw.BackwardM1(address,40)
except:
print("")
#sleep(0.3)
#motor_direction = ""
elif (motor_direction == "unsure" and firstRun == False):
print("rotating")
roboclaw.BackwardM1(address,60)
roboclaw.ForwardM2(address,60)
