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 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 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)
class SerialDriver(object):
'''
Class for serial UART interface to the RoboClaw Motor Controllers
'''
def __init__(self):
rospy.loginfo("Initilizing the motor controllers..")
self.e_stop = 1
self.reg_enabled = 0
self.temp = 0
self.error = 0
self.voltage = 0
self.currents = [0,0]
self._thread_lock = False
self.prev_enc_ts = None
self.prev_tick = [None, None]
self.start_time = datetime.now()
self.left_currents = []
self.right_currents = []
self.max_left_integrator = 0
self.max_right_integrator = 0
self.left_integrator = 0
self.right_integrator = 0
self.motor_lockout = 0
self._cmd_buf_flag = 0
self._l_vel_cmd_buf = 0
self._r_vel_cmd_buf = 0
self.battery_percent = 0
self.shutdown_warning = False
self.shutdown_flag = False
self.rate = rospy.get_param("/puffer/rate")
self.delta_t = 2.0/self.rate
self.rc = Roboclaw(
rospy.get_param("/motor_controllers/serial_device_name"),
rospy.get_param("/motor_controllers/baud_rate")
)
self.rc.Open()
self._set_operating_params()
self._get_version()
self.set_estop(0) #Clears E-stop pin
self.enable_12v_reg(1) #Disables 12V Regulator
self.kill_motors() #Start the motors not moving
#Private Methods
def _get_version(self):
'''
Version check for communication verification to the motor controllers
returns ther version number if sucessful, and 0 if not
'''
version = self.rc.ReadVersion(self.address)
if version != 0:
rospy.loginfo("[Motor __init__ ] Sucessfully connected to all Motor Controllers!")
rospy.loginfo( version )
rospy.set_param("/motor_controllers/firmware_version",version)
else:
raise Exception("Unable to establish connection to Motor controllers")
return version
def _set_operating_params(self):
'''
Sets all the operating parameters for control of PUFFER, Pinouts, and
safety parameters.
returns None
'''
#GPIO settings for E-stop and Regulator Enable pins
self.e_stop_pin = rospy.get_param("/gpio_pins/e_stop",1)
self.reg_en_pin = rospy.get_param("/gpio_pins/reg_en",1)
try:
GPIO.setmode(GPIO.BCM)
GPIO.setwarnings(False)
GPIO.setup(self.e_stop_pin, GPIO.OUT)
GPIO.setup(self.reg_en_pin, GPIO.OUT)
except:
pass
#Threadlock used for serial comm to RoboClaw
self.thread_timeout = rospy.get_param("/threadlock/timeout")
#Motor operating parameters
self.wheel_d = rospy.get_param("/wheels/diameter")
self.enc_cpr = rospy.get_param("/wheels/encoder/cts_per_rev")
factor = rospy.get_param("/wheels/encoder/factor")
stage_1 = rospy.get_param("/wheels/gearbox/stage1")
stage_2 = rospy.get_param("/wheels/gearbox/stage2")
stage_3 = rospy.get_param("/wheels/gearbox/stage3")
self.accel_const = rospy.get_param("/puffer/accel_const")
self.max_vel_per_s = rospy.get_param("/puffer/max_vel_per_s")
self.tick_per_rev = int(self.enc_cpr * factor * stage_1 * stage_2 * stage_3)
rospy.loginfo(self.tick_per_rev)
rospy.set_param("tick_per_rev", self.tick_per_rev)
self.max_cts_per_s = int((self.max_vel_per_s * self.tick_per_rev)/(math.pi * self.wheel_d))
self.max_accel = int(self.max_cts_per_s * self.accel_const)
rospy.set_param("max_cts_per_s", self.max_cts_per_s)
rospy.set_param("max_accel", self.max_accel)
self.address = rospy.get_param("/motor_controllers/address", 0x80)
self.rc.SetMainVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/main/low", 12.0) * 10),int(rospy.get_param("motor_controllers/battery/main/high", 18.0 ) * 10))
self.rc.SetLogicVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/logic/low") * 10),int(rospy.get_param("motor_controllers/battery/logic/high") * 10))
self.max_current = rospy.get_param("/motor_controllers/current/max_amps")
self.motor_lockout_time = rospy.get_param("/puffer/motor_lockout_time")
m1p = rospy.get_param("/motor_controllers/m1/p")
m1i = rospy.get_param("/motor_controllers/m1/i")
m1d = rospy.get_param("/motor_controllers/m1/d")
m1qpps = rospy.get_param("/motor_controllers/m1/qpps")
m2p = rospy.get_param("/motor_controllers/m2/p")
m2i = rospy.get_param("/motor_controllers/m2/i")
m2d = rospy.get_param("/motor_controllers/m2/d")
m2qpps = rospy.get_param("/motor_controllers/m2/qpps")
self.battery_max_time = rospy.get_param("/battery/max_time")
self.battery_max_volts = rospy.get_param("/battery/max_volts")
self.battery_coef_a = rospy.get_param("/battery/a")
self.battery_coef_b = rospy.get_param("/battery/b")
self.battery_coef_c = rospy.get_param("/battery/c")
self.battery_warning = rospy.get_param("/battery/warning_percent")
self.battery_shutdown = rospy.get_param("/battery/shutdown_percent")
self.rc.SetM1VelocityPID(self.address,m1p, m1i, m1d, m1qpps)
self.rc.SetM2VelocityPID(self.address,m2p, m2i, m2d, m2qpps)
self.rc.WriteNVM(self.address)
time.sleep(0.001)
self.rc.ReadNVM(self.address)
def _lock_thread(self,lock):
'''
Checks the thread lock and then grabs it when it frees up
no return value
'''
if (lock):
start = time.time()
while self._thread_lock:
#rospy.loginfo("in threadlock")
if time.time() - start > self.thread_timeout:
raise Exception("Thread lock timeout")
time.sleep(0.001)
self._thread_lock = True
else:
self._thread_lock = False
def _get_Temp(self):
'''
Gets the temperature of the motor controllers
return:
list [2] (int): Temperature values * 10 degrees C
'''
self._lock_thread(1)
self.temp = self.rc.ReadTemp(self.address)[1]
self._lock_thread(0)
self.temp = int(self.temp*100)/1000.0
return self.temp
def _get_Voltage(self):
'''
Gets the voltage of the motor controllers
return:
voltage (int) : Voltage values * 10 volts
'''
self._lock_thread(1)
v = self.rc.ReadMainBatteryVoltage(self.address)[1]
v = int(v*100)/1000.0
if v != 0:
self.voltage = v + 0.4 #accounts for the voltage drop in the diode
self._lock_thread(0)
return v
def _get_Currents(self):
'''
Gets the current of the motor controllers
return:
list [2] (int): Current values * 100 Amps
'''
self._lock_thread(1)
cur = self.rc.ReadCurrents(self.address)
self._lock_thread(0)
r_current = int(cur[1])/100.0
l_current = int(cur[2])/100.0
self.currents = [l_current,r_current]
self.left_currents.insert(0, l_current)
self.right_currents.insert(0, r_current)
if (len(self.left_currents) > self.rate/2):
del self.left_currents[-1]
del self.right_currents[-1]
left_power = 0
right_power = 0
for i in range(len(self.left_currents)):
left_power += math.pow(self.left_currents[i],2) * self.delta_t
right_power += math.pow(self.right_currents[i],2) * self.delta_t
if (left_power >= math.pow(self.max_current,2) or right_power >= math.pow(self.max_current,2)):
rospy.loginfo("Motor power exceeded max allowed! Disabling motors for %d seconds" %(self.motor_lockout_time))
self.motor_lockout = 1
self.set_estop(1)
self.lockout_timestamp = time.time()
if (self.motor_lockout and (time.time() - self.lockout_timestamp >= self.motor_lockout_time)):
rospy.loginfo("Re-enabling the motors from timeout lock")
self.motor_lockout = 0
self.set_estop(0)
self.left_integrator = left_power
self.right_integrator = right_power
self.max_left_integrator = max(self.max_left_integrator, self.left_integrator)
self.max_right_integrator = max(self.max_right_integrator, self.right_integrator)
#print self.left_integrator, self.right_integrator
return self.currents
def _get_Errors(self):
'''
Gets the error status of the motor controllers
return:
error (int): Error code for motor controller
'''
self._lock_thread(1)
self.error = self.rc.ReadError(self.address)[1]
self._lock_thread(0)
return self.error
def _get_Encs(self):
'''
Gets the encoder values of the motor controllers
return:
list [2] (int): Speed of motors in radians/s, computed at each delta T
'''
self._lock_thread(1)
r_enc = self.rc.ReadEncM1(self.address)[1]
l_enc = self.rc.ReadEncM2(self.address)[1]
dt = datetime.now()
self._lock_thread(0)
if self.prev_tick == [None, None]:
l_vel_rad_s, r_vel_rad_s = 0,0
self.timestamp = 0
else:
delta_t = ((dt-self.prev_enc_ts).microseconds)/1000000.0
self.timestamp = int(self.timestamp + (delta_t * 100000))
l_vel = (l_enc - self.prev_tick[0])/(delta_t)
r_vel = (r_enc - self.prev_tick[1])/(delta_t)
l_vel_rad_s = l_vel * (2 * math.pi/self.tick_per_rev)
r_vel_rad_s = r_vel * (2 * math.pi/self.tick_per_rev)
l_vel_rad_s = int(l_vel_rad_s * 1000)/1000.0
r_vel_rad_s = int(r_vel_rad_s * 1000)/1000.0
self.prev_enc_ts = dt
self.prev_tick = [l_enc, r_enc]
self.enc = [l_vel_rad_s, r_vel_rad_s]
return l_vel_rad_s, r_vel_rad_s
def _send_motor_cmds(self):
l_vel = self._l_vel_cmd_buf
r_vel = self._r_vel_cmd_buf
l_vel *= (self.tick_per_rev/(2*math.pi))
r_vel *= (self.tick_per_rev/(2*math.pi))
l_vel = max(min(l_vel,self.max_cts_per_s), -self.max_cts_per_s)
r_vel = max(min(r_vel,self.max_cts_per_s), -self.max_cts_per_s)
if (( abs(l_vel) <= self.max_cts_per_s) and (abs(r_vel) <= self.max_cts_per_s)):
if not self.motor_lockout:
self._lock_thread(1)
self.rc.SpeedAccelM1(self.address, self.max_accel, int(r_vel))
self.rc.SpeedAccelM2(self.address, self.max_accel, int(l_vel))
self._lock_thread(0)
else:
rospy.loginfo( "values not in accepted range" )
self.send_motor_cmds(0,0)
# Public Methods
def set_estop(self,val):
'''
Sets the E-stop pin to stop Motor control movement until cleared
Parameters:
val (int):
0 - Clears E-stop
1 - Sets E-stop, disabling motor movement
no return value
'''
if (val != self.e_stop):
if val:
rospy.loginfo( "Enabling the E-stop")
GPIO.output(self.e_stop_pin, 1)
self.e_stop = 1
else:
rospy.loginfo( "Clearing the E-stop")
GPIO.output(self.e_stop_pin, 0)
self.e_stop = 0
def battery_state_esimator(self):
x = self.voltage
y = math.pow(x,2) * self.battery_coef_a + self.battery_coef_b * x + self.battery_coef_c
self.battery_percent = (100 * y/float(self.battery_max_time))
if (self.battery_percent <= self.battery_warning):
self.shutdown_warning = True
else:
self.shutdown_warning = False
if (self.battery_percent <= self.battery_shutdown):
self.shutdown_flag = True
self.set_estop(1)
def update_cmd_buf(self, l_vel, r_vel):
'''
Updates the command buffers and sets the flag that new command has been received
Parameters:
l_vel (int): [-0.833, 0.833] units of [rad/s]
r_vel (int): [-0.833, 0.833] units of [rad/s]
no return value
'''
self._l_vel_cmd_buf = l_vel
self._r_vel_cmd_buf = r_vel
self._cmd_buf_flag = 1
def kill_motors(self):
'''
Stops all motors on the assembly
'''
self._lock_thread(1)
self.rc.ForwardM1(self.address, 0)
self.rc.ForwardM2(self.address, 0)
self._lock_thread(0)
def loop(self, counter):
'''
Gets the data from the motor controllers to populate as class variables
all data function classes are private because of threadlocks
Downsample non-critical values to keep serial comm line free
Parameters:
counter (int):
no return value
'''
if (self._cmd_buf_flag):
self._send_motor_cmds()
self._cmd_buf_flag = 0
self._get_Encs()
if not counter % 2:
self._get_Currents()
if not counter % 5:
self._get_Errors()
if not counter % 20:
self._get_Temp()
self._get_Voltage()
self.battery_state_esimator()
def enable_12v_reg(self, en):
'''
Turns on/off the 12V regulator GPIO pin
Parameters:
en (int):
0: Disabled
1: Enabled
'''
try:
if (en != self.reg_enabled):
if en:
rospy.loginfo("Enabling the 12V Regulator")
GPIO.output(self.reg_en_pin, 1)
self.reg_enabled = 1
else:
rospy.loginfo("Disabling the 12V Regulator")
GPIO.output(self.reg_en_pin, 0)
self.reg_enabled = 0
except:
pass
def cleanup(self):
'''
Cleans up the motor controller node, stopping motors and killing all threads
no return value
'''
rospy.loginfo("Cleaning up the motor_controller node..")
self._thread_lock = False
self.kill_motors()
self.set_estop(1)
try:
GPIO.cleanup()
except:
pass
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)
class RoboclawWrapper(object):
"""Interface between the roboclaw motor drivers and the higher level rover code"""
def __init__(self):
rospy.loginfo( "Initializing motor controllers")
# initialize attributes
self.rc = None
self.err = [None] * 5
self.address = []
self.current_enc_vals = None
self.mutex = False
self.roboclaw_mapping = rospy.get_param('~roboclaw_mapping')
self.encoder_limits = {}
self.establish_roboclaw_connections()
self.killMotors() # don't move at start
self.setup_encoders()
# save settings to non-volatile (permanent) memory
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
self.corner_max_vel = 1000
self.corner_accel = 2000
self.roboclaw_overflow = 2**15-1
accel_max = 655359
accel_rate = 0.5
self.accel_pos = int((accel_max /2) + accel_max * accel_rate)
self.accel_neg = int((accel_max /2) - accel_max * accel_rate)
self.errorCheck()
self.killMotors()
# set up publishers and subscribers
self.corner_cmd_sub = rospy.Subscriber("/cmd_corner", CommandCorner, self.corner_cmd_cb, queue_size=1)
self.drive_cmd_sub = rospy.Subscriber("/cmd_drive", CommandDrive, self.drive_cmd_cb, queue_size=1)
self.enc_pub = rospy.Publisher("/encoder", JointState, queue_size=1)
self.status_pub = rospy.Publisher("/status", Status, queue_size=1)
def run(self):
"""Blocking loop which runs after initialization has completed"""
rate = rospy.Rate(5)
mutex_rate = rospy.Rate(10)
status = Status()
counter = 0
while not rospy.is_shutdown():
while self.mutex and not rospy.is_shutdown():
mutex_rate.sleep()
self.mutex = True
# read from roboclaws and publish
try:
self.read_encoder_values()
self.enc_pub.publish(self.current_enc_vals)
except AssertionError as read_exc:
rospy.logwarn( "Failed to read encoder values")
if (counter >= 10):
status.battery = self.getBattery()
status.temp = self.getTemp()
status.current = self.getCurrents()
status.error_status = self.getErrors()
self.status_pub.publish(status)
counter = 0
self.mutex = False
counter += 1
rate.sleep()
def establish_roboclaw_connections(self):
"""
Attempt connecting to the roboclaws
:raises Exception: when connection to one or more of the roboclaws is unsuccessful
"""
self.rc = Roboclaw(rospy.get_param('/motor_controller/device', "/dev/serial0"),
rospy.get_param('/motor_controller/baud_rate', 115200))
self.rc.Open()
address_raw = rospy.get_param('motor_controller/addresses')
address_list = (address_raw.split(','))
self.address = [None]*len(address_list)
for i in range(len(address_list)):
self.address[i] = int(address_list[i])
# initialize connection status to successful
all_connected = True
for address in self.address:
rospy.logdebug("Attempting to talk to motor controller ''".format(address))
version_response = self.rc.ReadVersion(address)
connected = bool(version_response[0])
if not connected:
rospy.logerr("Unable to connect to roboclaw at '{}'".format(address))
all_connected = False
else:
rospy.logdebug("Roboclaw version for address '{}': '{}'".format(address, version_response[1]))
if all_connected:
rospy.loginfo("Sucessfully connected to RoboClaw motor controllers")
else:
raise Exception("Unable to establish connection to one or more of the Roboclaw motor controllers")
def setup_encoders(self):
"""Set up the encoders"""
for motor_name, properties in self.roboclaw_mapping.iteritems():
if "corner" in motor_name:
enc_min, enc_max = self.read_encoder_limits(properties["address"], properties["channel"])
self.encoder_limits[motor_name] = (enc_min, enc_max)
else:
self.encoder_limits[motor_name] = (None, None)
self.rc.ResetEncoders(properties["address"])
def read_encoder_values(self):
"""Query roboclaws and update current motors status in encoder ticks"""
enc_msg = JointState()
enc_msg.header.stamp = rospy.Time.now()
for motor_name, properties in self.roboclaw_mapping.iteritems():
enc_msg.name.append(motor_name)
position = self.read_encoder_position(properties["address"], properties["channel"])
velocity = self.read_encoder_velocity(properties["address"], properties["channel"])
current = self.read_encoder_current(properties["address"], properties["channel"])
enc_msg.position.append(self.tick2position(position,
self.encoder_limits[motor_name][0],
self.encoder_limits[motor_name][1],
properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.velocity.append(self.qpps2velocity(velocity,
properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.effort.append(current)
self.current_enc_vals = enc_msg
def corner_cmd_cb(self, cmd):
r = rospy.Rate(10)
rospy.logdebug("Corner command callback received: {}".format(cmd))
while self.mutex and not rospy.is_shutdown():
r.sleep()
self.mutex = True
# convert position to tick
encmin, encmax = self.encoder_limits["corner_left_front"]
left_front_tick = self.position2tick(cmd.left_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_front"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_left_back"]
left_back_tick = self.position2tick(cmd.left_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_back"]
right_back_tick = self.position2tick(cmd.right_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_front"]
right_front_tick = self.position2tick(cmd.right_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_front"]["gear_ratio"])
self.send_position_cmd(self.roboclaw_mapping["corner_left_front"]["address"],
self.roboclaw_mapping["corner_left_front"]["channel"],
left_front_tick)
self.send_position_cmd(self.roboclaw_mapping["corner_left_back"]["address"],
self.roboclaw_mapping["corner_left_back"]["channel"],
left_back_tick)
self.send_position_cmd(self.roboclaw_mapping["corner_right_back"]["address"],
self.roboclaw_mapping["corner_right_back"]["channel"],
right_back_tick)
self.send_position_cmd(self.roboclaw_mapping["corner_right_front"]["address"],
self.roboclaw_mapping["corner_right_front"]["channel"],
right_front_tick)
self.mutex = False
def drive_cmd_cb(self, cmd):
r = rospy.Rate(10)
rospy.logdebug("Drive command callback received: {}".format(cmd))
while self.mutex and not rospy.is_shutdown():
r.sleep()
self.mutex = True
props = self.roboclaw_mapping["drive_left_front"]
vel_cmd = self.velocity2qpps(cmd.left_front_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_middle"]
vel_cmd = self.velocity2qpps(cmd.left_middle_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_back"]
vel_cmd = self.velocity2qpps(cmd.left_back_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_back"]
vel_cmd = self.velocity2qpps(cmd.right_back_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_middle"]
vel_cmd = self.velocity2qpps(cmd.right_middle_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_front"]
vel_cmd = self.velocity2qpps(cmd.right_front_vel, props["ticks_per_rev"], props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
self.mutex = False
def send_position_cmd(self, address, channel, target_tick):
"""
Wrapper around one of the send position commands
:param address:
:param channel:
:param target_tick: int
"""
cmd_args = [self.corner_accel, self.corner_max_vel, self.corner_accel, target_tick, 1]
if channel == "M1":
return self.rc.SpeedAccelDeccelPositionM1(address, *cmd_args)
elif channel == "M2":
return self.rc.SpeedAccelDeccelPositionM2(address, *cmd_args)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
def read_encoder_position(self, address, channel):
"""Wrapper around self.rc.ReadEncM1 and self.rcReadEncM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadEncM1(address)
elif channel == "M2":
val = self.rc.ReadEncM2(address)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
assert val[0] == 1
return val[1]
def read_encoder_limits(self, address, channel):
"""Wrapper around self.rc.ReadPositionPID and returns subset of the data
:return: (enc_min, enc_max)
"""
if channel == "M1":
result = self.rc.ReadM1PositionPID(address)
elif channel == "M2":
result = self.rc.ReadM2PositionPID(address)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
assert result[0] == 1
return (result[-2], result[-1])
def send_velocity_cmd(self, address, channel, target_qpps):
"""
Wrapper around one of the send velocity commands
:param address:
:param channel:
:param target_qpps: int
"""
# clip values
target_qpps = max(-self.roboclaw_overflow, min(self.roboclaw_overflow, target_qpps))
accel = self.accel_pos
if target_qpps < 0:
accel = self.accel_neg
if channel == "M1":
return self.rc.DutyAccelM1(address, accel, target_qpps)
elif channel == "M2":
return self.rc.DutyAccelM2(address, accel, target_qpps)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
def read_encoder_velocity(self, address, channel):
"""Wrapper around self.rc.ReadSpeedM1 and self.rcReadSpeedM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadSpeedM1(address)
elif channel == "M2":
val = self.rc.ReadSpeedM2(address)
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
assert val[0] == 1
return val[1]
def read_encoder_current(self, address, channel):
"""Wrapper around self.rc.ReadCurrents to simplify code"""
if channel == "M1":
return self.rc.ReadCurrents(address)[0]
elif channel == "M2":
return self.rc.ReadCurrents(address)[1]
else:
raise AttributeError("Received unknown channel '{}'. Expected M1 or M2".format(channel))
def tick2position(self, tick, enc_min, enc_max, ticks_per_rev, gear_ratio):
"""
Convert the absolute position from ticks to radian relative to the middle position
:param tick:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return tick / ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
# positive values correspond to the wheel turning left (z-axis points up)
return -(tick - mid) / ticks_per_rad * gear_ratio
def position2tick(self, position, enc_min, enc_max, ticks_per_rev, gear_ratio):
"""
Convert the absolute position from radian relative to the middle position to ticks
Clip values that are outside the range [enc_min, enc_max]
:param position:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
# positive values correspond to the wheel turning left (z-axis points up)
position *= -1
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return position * ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
tick = int(mid + position * ticks_per_rad / gear_ratio)
return max(enc_min, min(enc_max, tick))
def qpps2velocity(self, qpps, ticks_per_rev, gear_ratio):
"""
Convert the given quadrature pulses per second to radian/s
:param qpps: int
:param ticks_per_rev:
:param gear_ratio:
:return:
"""
return qpps / (2 * math.pi * gear_ratio * ticks_per_rev)
def velocity2qpps(self, velocity, ticks_per_rev, gear_ratio):
"""
Convert the given velocity to quadrature pulses per second
:param velocity: rad/s
:param ticks_per_rev:
:param gear_ratio:
:return: int
"""
return int(velocity * 2 * math.pi * gear_ratio * ticks_per_rev)
def getBattery(self):
return self.rc.ReadMainBatteryVoltage(self.address[0])[1]
def getTemp(self):
temp = [None] * 5
for i in range(5):
temp[i] = self.rc.ReadTemp(self.address[i])[1]
return temp
def getCurrents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
currents[2*i] = currs[1]
currents[(2*i) + 1] = currs[2]
return currents
def getErrors(self):
return self.err
def killMotors(self):
"""Stops all motors on Rover"""
for i in range(5):
self.rc.ForwardM1(self.address[i], 0)
self.rc.ForwardM2(self.address[i], 0)
def errorCheck(self):
"""Checks error status of each motor controller, returns 0 if any errors occur"""
for i in range(len(self.address)):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
rospy.logerr("Motor controller Error: \n'{}'".format(error))
import time
from roboclaw import Roboclaw
#Windows comport name
#rc = Roboclaw("COM9",38400)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",38400)
rc = Roboclaw("/dev/serial0", 38400)
rc.Open()
address = 0x80
while (1):
rc.ForwardM1(address, 0)
rc.ForwardM1(address, 60) #1/4 power forward
rc.BackwardM2(address, 32) #1/4 power backward
time.sleep(2)
rc.BackwardM1(address, 60) #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)
from roboclaw import Roboclaw
from time import sleep
if __name__ == "__main__":
address = 0x80
roboclaw = Roboclaw("/dev/ttyS0", 38400)
roboclaw.Open()
while True:
roboclaw.ForwardM1(address,64)
sleep(2)
roboclaw.ForwardM1(address,0)
sleep(2)
roboclaw.ForwardM2(address, 64)
sleep(2)
roboclaw.ForwardM2(address,0)
sleep(2)
class RoboclawWrapper(object):
"""Interface between the roboclaw motor drivers and the higher level rover code"""
def __init__(self):
rospy.loginfo("Initializing motor controllers")
# initialize attributes
self.rc = None
self.err = [None] * 5
self.address = []
self.current_enc_vals = None
self.corner_cmd_buffer = None
self.drive_cmd_buffer = None
self.roboclaw_mapping = rospy.get_param('~roboclaw_mapping')
self.encoder_limits = {}
self.establish_roboclaw_connections()
self.stop_motors() # don't move at start
self.setup_encoders()
# save settings to non-volatile (permanent) memory
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
self.corner_max_vel = 1000
# corner motor acceleration
# Even though the actual method takes longs (2*32-1), roboclaw blog says 2**15 is 100%
accel_max = 2**15 - 1
accel_rate = rospy.get_param('/corner_acceleration_factor', 0.8)
self.corner_accel = int(accel_max * accel_rate)
self.roboclaw_overflow = 2**15 - 1
# drive motor acceleration
accel_max = 2**15 - 1
accel_rate = rospy.get_param('/drive_acceleration_factor', 0.5)
self.drive_accel = int(accel_max * accel_rate)
self.velocity_timeout = rospy.Duration(
rospy.get_param('/velocity_timeout', 2.0))
self.time_last_cmd = rospy.Time.now()
self.stop_motors()
# set up publishers and subscribers
self.corner_cmd_sub = rospy.Subscriber("/cmd_corner",
CommandCorner,
self.corner_cmd_cb,
queue_size=1)
self.drive_cmd_sub = rospy.Subscriber("/cmd_drive",
CommandDrive,
self.drive_cmd_cb,
queue_size=1)
self.enc_pub = rospy.Publisher("/encoder", JointState, queue_size=1)
self.status_pub = rospy.Publisher("/status", Status, queue_size=1)
def run(self):
"""Blocking loop which runs after initialization has completed"""
rate = rospy.Rate(8)
status = Status()
counter = 0
while not rospy.is_shutdown():
# Check to see if there are commands in the buffer to send to the motor controller
if self.drive_cmd_buffer:
drive_fcn = self.send_drive_buffer_velocity
drive_fcn(self.drive_cmd_buffer)
self.drive_cmd_buffer = None
if self.corner_cmd_buffer:
self.send_corner_buffer(self.corner_cmd_buffer)
self.corner_cmd_buffer = None
# read from roboclaws and publish
try:
self.read_encoder_values()
self.enc_pub.publish(self.current_enc_vals)
except AssertionError as read_exc:
rospy.logwarn("Failed to read encoder values")
# Downsample the rate of less important data
if (counter >= 5):
status.battery = self.read_battery()
status.temp = self.read_temperatures()
status.current = self.read_currents()
status.error_status = self.read_errors()
counter = 0
# stop the motors if we haven't received a command in a while
now = rospy.Time.now()
if now - self.time_last_cmd > self.velocity_timeout:
# rather than a hard stop, send a ramped velocity command
self.drive_cmd_buffer = CommandDrive()
self.send_drive_buffer_velocity(self.drive_cmd_buffer)
self.time_last_cmd = now # so this doesn't get called all the time
self.status_pub.publish(status)
counter += 1
rate.sleep()
def establish_roboclaw_connections(self):
"""
Attempt connecting to the roboclaws
:raises Exception: when connection to one or more of the roboclaws is unsuccessful
"""
self.rc = Roboclaw(
rospy.get_param('/motor_controller/device', "/dev/serial0"),
rospy.get_param('/motor_controller/baud_rate', 115200))
self.rc.Open()
address_raw = rospy.get_param('motor_controller/addresses')
address_list = (address_raw.split(','))
self.address = [None] * len(address_list)
for i in range(len(address_list)):
self.address[i] = int(address_list[i])
# initialize connection status to successful
all_connected = True
for address in self.address:
rospy.logdebug(
"Attempting to talk to motor controller ''".format(address))
version_response = self.rc.ReadVersion(address)
connected = bool(version_response[0])
if not connected:
rospy.logerr(
"Unable to connect to roboclaw at '{}'".format(address))
all_connected = False
else:
rospy.logdebug(
"Roboclaw version for address '{}': '{}'".format(
address, version_response[1]))
if all_connected:
rospy.loginfo(
"Sucessfully connected to RoboClaw motor controllers")
else:
raise Exception(
"Unable to establish connection to one or more of the Roboclaw motor controllers"
)
def setup_encoders(self):
"""Set up the encoders"""
for motor_name, properties in self.roboclaw_mapping.iteritems():
if "corner" in motor_name:
enc_min, enc_max = self.read_encoder_limits(
properties["address"], properties["channel"])
self.encoder_limits[motor_name] = (enc_min, enc_max)
else:
self.encoder_limits[motor_name] = (None, None)
self.rc.ResetEncoders(properties["address"])
def read_encoder_values(self):
"""Query roboclaws and update current motors status in encoder ticks"""
enc_msg = JointState()
enc_msg.header.stamp = rospy.Time.now()
for motor_name, properties in self.roboclaw_mapping.iteritems():
enc_msg.name.append(motor_name)
position = self.read_encoder_position(properties["address"],
properties["channel"])
velocity = self.read_encoder_velocity(properties["address"],
properties["channel"])
current = self.read_encoder_current(properties["address"],
properties["channel"])
enc_msg.position.append(
self.tick2position(position,
self.encoder_limits[motor_name][0],
self.encoder_limits[motor_name][1],
properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.velocity.append(
self.qpps2velocity(velocity, properties['ticks_per_rev'],
properties['gear_ratio']))
enc_msg.effort.append(current)
self.current_enc_vals = enc_msg
def corner_cmd_cb(self, cmd):
"""
Takes the corner command and stores it in the buffer to be sent
on the next iteration of the run() loop.
"""
rospy.logdebug("Corner command callback received: {}".format(cmd))
self.time_last_cmd = rospy.Time.now()
self.corner_cmd_buffer = cmd
def send_corner_buffer(self, cmd):
"""
Sends the corner command to the motor controller.
"""
# convert position to tick
encmin, encmax = self.encoder_limits["corner_left_front"]
left_front_tick = self.position2tick(
cmd.left_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_front"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_left_back"]
left_back_tick = self.position2tick(
cmd.left_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_left_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_left_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_back"]
right_back_tick = self.position2tick(
cmd.right_back_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_back"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_back"]["gear_ratio"])
encmin, encmax = self.encoder_limits["corner_right_front"]
right_front_tick = self.position2tick(
cmd.right_front_pos, encmin, encmax,
self.roboclaw_mapping["corner_right_front"]["ticks_per_rev"],
self.roboclaw_mapping["corner_right_front"]["gear_ratio"])
self.send_position_cmd(
self.roboclaw_mapping["corner_left_front"]["address"],
self.roboclaw_mapping["corner_left_front"]["channel"],
left_front_tick)
self.send_position_cmd(
self.roboclaw_mapping["corner_left_back"]["address"],
self.roboclaw_mapping["corner_left_back"]["channel"],
left_back_tick)
self.send_position_cmd(
self.roboclaw_mapping["corner_right_back"]["address"],
self.roboclaw_mapping["corner_right_back"]["channel"],
right_back_tick)
self.send_position_cmd(
self.roboclaw_mapping["corner_right_front"]["address"],
self.roboclaw_mapping["corner_right_front"]["channel"],
right_front_tick)
def drive_cmd_cb(self, cmd):
"""
Takes the drive command and stores it in the buffer to be sent
on the next iteration of the run() loop.
"""
rospy.logdebug("Drive command callback received: {}".format(cmd))
self.drive_cmd_buffer = cmd
self.time_last_cmd = rospy.Time.now()
def send_drive_buffer_velocity(self, cmd):
"""
Sends the drive command to the motor controller, closed loop velocity commands
"""
props = self.roboclaw_mapping["drive_left_front"]
vel_cmd = self.velocity2qpps(cmd.left_front_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_middle"]
vel_cmd = self.velocity2qpps(cmd.left_middle_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_left_back"]
vel_cmd = self.velocity2qpps(cmd.left_back_vel, props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_back"]
vel_cmd = self.velocity2qpps(cmd.right_back_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_middle"]
vel_cmd = self.velocity2qpps(cmd.right_middle_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
props = self.roboclaw_mapping["drive_right_front"]
vel_cmd = self.velocity2qpps(cmd.right_front_vel,
props["ticks_per_rev"],
props["gear_ratio"])
self.send_velocity_cmd(props["address"], props["channel"], vel_cmd)
def send_position_cmd(self, address, channel, target_tick):
"""
Wrapper around one of the send position commands
:param address:
:param channel:
:param target_tick: int
"""
cmd_args = [
self.corner_accel, self.corner_max_vel, self.corner_accel,
target_tick, 1
]
if channel == "M1":
return self.rc.SpeedAccelDeccelPositionM1(address, *cmd_args)
elif channel == "M2":
return self.rc.SpeedAccelDeccelPositionM2(address, *cmd_args)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
def read_encoder_position(self, address, channel):
"""Wrapper around self.rc.ReadEncM1 and self.rcReadEncM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadEncM1(address)
elif channel == "M2":
val = self.rc.ReadEncM2(address)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
assert val[0] == 1
return val[1]
def read_encoder_limits(self, address, channel):
"""Wrapper around self.rc.ReadPositionPID and returns subset of the data
:return: (enc_min, enc_max)
"""
if channel == "M1":
result = self.rc.ReadM1PositionPID(address)
elif channel == "M2":
result = self.rc.ReadM2PositionPID(address)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
assert result[0] == 1
return (result[-2], result[-1])
def send_velocity_cmd(self, address, channel, target_qpps):
"""
Wrapper around one of the send velocity commands
:param address:
:param channel:
:param target_qpps: int
"""
# clip values
target_qpps = max(-self.roboclaw_overflow,
min(self.roboclaw_overflow, target_qpps))
if channel == "M1":
return self.rc.SpeedAccelM1(address, self.drive_accel, target_qpps)
elif channel == "M2":
return self.rc.SpeedAccelM2(address, self.drive_accel, target_qpps)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
def read_encoder_velocity(self, address, channel):
"""Wrapper around self.rc.ReadSpeedM1 and self.rcReadSpeedM2 to simplify code"""
if channel == "M1":
val = self.rc.ReadSpeedM1(address)
elif channel == "M2":
val = self.rc.ReadSpeedM2(address)
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
assert val[0] == 1
return val[1]
def read_encoder_current(self, address, channel):
"""Wrapper around self.rc.ReadCurrents to simplify code"""
if channel == "M1":
return self.rc.ReadCurrents(address)[0]
elif channel == "M2":
return self.rc.ReadCurrents(address)[1]
else:
raise AttributeError(
"Received unknown channel '{}'. Expected M1 or M2".format(
channel))
def tick2position(self, tick, enc_min, enc_max, ticks_per_rev, gear_ratio):
"""
Convert the absolute position from ticks to radian relative to the middle position
:param tick:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return tick / ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
return (tick - mid) / ticks_per_rad * gear_ratio
def position2tick(self, position, enc_min, enc_max, ticks_per_rev,
gear_ratio):
"""
Convert the absolute position from radian relative to the middle position to ticks
Clip values that are outside the range [enc_min, enc_max]
:param position:
:param enc_min:
:param enc_max:
:param ticks_per_rev:
:return:
"""
ticks_per_rad = ticks_per_rev / (2 * math.pi)
if enc_min is None or enc_max is None:
return position * ticks_per_rad
mid = enc_min + (enc_max - enc_min) / 2
tick = int(mid + position * ticks_per_rad / gear_ratio)
return max(enc_min, min(enc_max, tick))
def qpps2velocity(self, qpps, ticks_per_rev, gear_ratio):
"""
Convert the given quadrature pulses per second to radian/s
:param qpps: int
:param ticks_per_rev:
:param gear_ratio:
:return:
"""
return qpps * 2 * math.pi / (gear_ratio * ticks_per_rev)
def velocity2qpps(self, velocity, ticks_per_rev, gear_ratio):
"""
Convert the given velocity to quadrature pulses per second
:param velocity: rad/s
:param ticks_per_rev:
:param gear_ratio:
:return: int
"""
return int(velocity * gear_ratio * ticks_per_rev / (2 * math.pi))
def read_battery(self):
"""Read battery voltage from one of the roboclaws as a proxy for all roboclaws"""
# roboclaw reports value in 10ths of a Volt
return self.rc.ReadMainBatteryVoltage(self.address[0])[1] / 10.0
def read_temperatures(self):
temp = [None] * 5
for i in range(5):
# reported by roboclaw in 10ths of a Celsius
temp[i] = self.rc.ReadTemp(self.address[i])[1] / 10.0
return temp
def read_currents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
# reported by roboclaw in 10ths of an Ampere
currents[2 * i] = currs[1] / 100.0
currents[(2 * i) + 1] = currs[2] / 100.0
return currents
def stop_motors(self):
"""Stops all motors on Rover"""
for i in range(5):
self.rc.ForwardM1(self.address[i], 0)
self.rc.ForwardM2(self.address[i], 0)
def read_errors(self):
"""Checks error status of each motor controller, returns 0 if no errors reported"""
err = [0] * 5
for i in range(len(self.address)):
err[i] = self.rc.ReadError(self.address[i])[1]
if err[i] != 0:
rospy.logerr(
"Motor controller '{}' reported error code {}".format(
self.address[i], err[i]))
return err
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()
from roboclaw import Roboclaw
import time
rc = Roboclaw("/dev/ttyS0", 115200)
i = rc.Open()
ver = rc.ReadVersion(0x80)
print(ver)
ver = rc.ReadVersion(0x81)
print(ver)
ver = rc.ReadVersion(0x82)
print(ver)
rc.ForwardM1(0x81, 100)
time.sleep(3)
rc.ForwardM1(0x81, 0)
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')
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
#Windows comport name
#rc = Roboclaw("COM9",115200)
#Linux comport name
#rc = Roboclaw("/dev/ttyACM0",115200)
rc = Roboclaw("/dev/serial0", 38400)
rc.Open()
address = 0x80
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
##rc.ForwardM1(address,60)
while (1):
rc.ForwardM1(address, 60)
time.sleep(2)
##rc.BackwardMixed(address, 60)
##time.sleep(2)
##rc.TurnRightMixed(address, 32)
##time.sleep(2)
##rc.TurnLeftMixed(address, 32)
##time.sleep(2)
##rc.ForwardMixed(address, 0)
##rc.TurnRightMixed(address, 32)
##time.sleep(2)
##rc.TurnLeftMixed(address, 32)
##time.sleep(2)
##rc.TurnRightMixed(address, 0)
##time.sleep(2)
if (recordmode): # record waypoints
temp = raw_input(
"Hit l, r, f, s or w to go left, right, forward, stop or save waypoint"
)
if (temp == "l"):
print("Going left")
drive(cruise_speed, -30)
if (temp == "r"):
print("Going right")
drive(cruise_speed, 20)
if (temp == "f"):
print("Going forward")
drive(cruise_speed, 0)
if (temp == "s"):
print("Stopping")
rc.ForwardM1(address, 0) # kill motors
rc.ForwardM2(address, 0)
if (temp == "w"):
print("Saving waypoint #", waypoint_num)
frames = pipe.wait_for_frames()
pose = frames.get_pose_frame()
if pose:
data = pose.get_pose_data()
x = data.translation.x
y = -1.000 * data.translation.z # don't ask me why, but in "VR space", y is z and it's reversed
if (waypoint_num == 0):
with open(record_file,
'w') as csvfile: # overwrite original file
recordwriter = csv.writer(csvfile,
delimiter=',',
quotechar='|',
import time
roboclaw = Roboclaw("/dev/ttyACM0", 115200)
print(roboclaw.Open())
address = 0x80
roboclaw.ResetEncoders(address)
# while 1:
# #Get version string
# version = roboclaw.ReadVersion(0x80)
# if version[0]==False:
# print "GETVERSION Failed"
# else:
# print repr(version[1])
# time.sleep(1)
print roboclaw.ReadEncM1(address)[1]
roboclaw.ForwardM1(address, 40)
roboclaw.ForwardM2(address, 40)
time.sleep(3)
roboclaw.ForwardM1(address, 0)
roboclaw.ForwardM2(address, 0)
print roboclaw.ReadEncM1(address)[1]
xbee_radio.open()
try:
while True:
line = ' '
if xbee_radio.in_waiting:
line = str(xbee_radio.readline())[0]
print('------------Input--------')
print(line) # Connected to Radio
print(line[0] == 'w')
print('------------Input--------')
#motor_drive(line)
if line == 'w':
rc.ForwardM1(128, 20)
rc.ForwardM2(128, 20)
rc.BackwardM1(129, 20)
rc.ForwardM2(129, 20)
rc.ForwardM1(130, 20)
rc.ForwardM2(130, 20)
time.sleep(2)
rc.ForwardM1(128, 20)
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':
termios.tcsetattr(file_descriptor, termios.TCSADRAIN, old_settings)
return character
if __name__ == '__main__':
print("Searching for roboclaw, this may take a few seconds...\n")
roboclaw = Roboclaw("/dev/ttyACM0", 38400)
roboclaw.Open()
print("Robot is ready to control, use WASD to control direction and Q to quit")
try:
while True:
char = get_char()
char_val = ord(char)
if char.lower() == "q":
break
elif char.lower() == "w":
roboclaw.ForwardM1(0x80, 127)
#roboclaw.ForwardM2(0x80, 127)
elif char.lower() == "s":
roboclaw.BackwardM1(0x80, 127)
#roboclaw.BackwardM2(0x80, 127)
# elif char.lower() == "x":
# roboclaw.ForwardM1(0x80, 0)
finally:
roboclaw.ForwardM1(0x80, 0)
#roboclaw.ForwardM2(0x80, 0)
print("------")
print("Program ending")
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()
# EVENT PROCESSING STEP
for event in pygame.event.get(): # User did something
if event.type == pygame.QUIT: # If user clicked close
done = True # Flag that we are done so we exit this loop
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
if event.type == pygame.JOYBUTTONDOWN:
print("Joystick button pressed.")
if event.type == pygame.JOYBUTTONUP:
print("Joystick button released.")
if joystick.get_button(2) == 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)
class MotorControllers(object):
'''
Motor class contains the methods necessary to send commands to the motor controllers
for the corner and drive motors. There are many other ways of commanding the motors
from the RoboClaw, we suggest trying to write your own Closed loop feedback method for
the drive motors!
'''
def __init__(self):
## MAKE SURE TO FIX CONFIG.JSON WHEN PORTED TO THE ROVER!
#self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'],
# config['CONTROLLER_CONFIG']['baud_rate']
# )
rospy.loginfo("Initializing motor controllers")
#self.rc = Roboclaw( rospy.get_param('motor_controller_device', "/dev/serial0"),
# rospy.get_param('baud_rate', 115200))
sdev = "/dev/ttyAMA0"
sdev = "/dev/serial0"
self.rc = Roboclaw(sdev, 115200)
self.rc.Open()
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
# PSW
address_raw = "128,129,130,131,132"
#address_raw = rospy.get_param('motor_controller_addresses')
address_list = (address_raw.split(','))
self.address = [None] * len(address_list)
for i in range(len(address_list)):
self.address[i] = int(address_list[i])
version = 1
for address in self.address:
print("Attempting to talk to motor controller", address)
version = version & self.rc.ReadVersion(address)[0]
print version
if version != 0:
print "[Motor__init__] Sucessfully connected to RoboClaw motor controllers"
else:
raise Exception(
"Unable to establish connection to Roboclaw motor controllers")
self.killMotors()
self.enc_min = []
self.enc_max = []
for address in self.address:
#self.rc.SetMainVoltages(address, rospy.get_param('battery_low', 11)*10), rospy.get_param('battery_high', 18)*10))
if address == 131 or address == 132:
#self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
#self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
self.enc_min.append(self.rc.ReadM1PositionPID(address)[-2])
self.enc_min.append(self.rc.ReadM2PositionPID(address)[-2])
self.enc_max.append(self.rc.ReadM1PositionPID(address)[-1])
self.enc_max.append(self.rc.ReadM2PositionPID(address)[-1])
else:
#self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
#self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
self.rc.ResetEncoders(address)
rospy.set_param('enc_min', str(self.enc_min)[1:-1])
rospy.set_param('enc_max', str(self.enc_max)[1:-1])
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
#'''
voltage = self.rc.ReadMainBatteryVoltage(0x80)[1] / 10.0
lvolts = rospy.get_param('low_voltage', 11)
lvolts = rospy.get_param('low_voltage', 9)
if voltage >= lvolts:
print "[Motor__init__] Voltage is safe at: ",
print voltage,
print "V"
else:
print "[Motor__init__] Voltage is unsafe at: ", voltage, "V ( low = ", lvolts, ")"
raise Exception("Unsafe Voltage of" + voltage + " Volts")
#'''
i = 0
for address in self.address:
self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4]
self.accel[i] = int(self.qpps[i] * 2)
self.qpps[i + 1] = self.rc.ReadM2VelocityPID(address)[4]
self.accel[i + 1] = int(self.qpps[i] * 2)
i += 2
accel_max = 655359
accel_rate = 0.5
self.accel_pos = int((accel_max / 2) + accel_max * accel_rate)
self.accel_neg = int((accel_max / 2) - accel_max * accel_rate)
self.errorCheck()
mids = [None] * 4
self.enc = [None] * 4
for i in range(4):
mids[i] = (self.enc_max[i] + self.enc_min[i]) / 2
#self.cornerToPosition(mids)
time.sleep(2)
self.killMotors()
def cornerToPosition(self, tick):
'''
Method to send position commands to the corner motor
:param list tick: A list of ticks for each of the corner motors to
move to, if tick[i] is 0 it instead stops that motor from moving
'''
speed, accel = 1000, 2000 #These values could potentially need tuning still
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if tick[i] != -1:
if (i % 2):
self.rc.SpeedAccelDeccelPositionM2(self.address[index],
accel, speed, accel,
tick[i], 1)
else:
self.rc.SpeedAccelDeccelPositionM1(self.address[index],
accel, speed, accel,
tick[i], 1)
else:
if not (i % 2): self.rc.ForwardM1(self.address[index], 0)
else: self.rc.ForwardM2(self.address[index], 0)
def sendMotorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the drive motors,
sends open-loop commands to the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
#speed = speed/100.0
#speed *= 0.5
addr = self.address[int(motorID / 2)]
if speed > 0:
if not motorID % 2: command = self.rc.ForwardM1
else: command = self.rc.ForwardM2
else:
if not motorID % 2: command = self.rc.BackwardM1
else: command = self.rc.BackwardM2
speed = abs(int(speed * 127))
return command(addr, speed)
def sendSignedDutyAccel(self, motorID, speed):
addr = self.address[int(motorID / 2)]
if speed > 0: accel = self.accel_pos
else: accel = self.accel_neg
if not motorID % 2: command = self.rc.DutyAccelM1
else: command = self.rc.DutyAccelM2
speed = int(32767 * speed / 100.0)
return command(addr, accel, speed)
def getCornerEnc(self):
enc = []
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if not (i % 2):
enc.append(self.rc.ReadEncM1(self.address[index])[1])
else:
enc.append(self.rc.ReadEncM2(self.address[index])[1])
self.enc = enc
return enc
@staticmethod
def tick2deg(tick, e_min, e_max):
'''
Converts a tick to physical degrees
:param int tick : Current encoder tick
:param int e_min: The minimum encoder value based on physical stop
:param int e_max: The maximum encoder value based on physical stop
'''
return (tick - (e_max + e_min) / 2.0) * (90.0 / (e_max - e_min))
def getCornerEncAngle(self):
if self.enc[0] == None:
return -1
deg = [None] * 4
for i in range(4):
deg[i] = int(
self.tick2deg(self.enc[i], self.enc_min[i], self.enc_max[i]))
return deg
def getDriveEnc(self):
enc = [None] * 6
for i in range(6):
if not (i % 2):
enc[i] = self.rc.ReadEncM1(self.address[int(math.ceil(i /
2))])[1]
else:
enc[i] = self.rc.ReadEncM2(self.address[int(math.ceil(i /
2))])[1]
return enc
def getBattery(self):
return self.rc.ReadMainBatteryVoltage(self.address[0])[1]
def getTemp(self):
temp = [None] * 5
for i in range(5):
temp[i] = self.rc.ReadTemp(self.address[i])[1]
return temp
def getCurrents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
currents[2 * i] = currs[1]
currents[(2 * i) + 1] = currs[2]
return currents
def getErrors(self):
return self.err
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(5):
self.rc.ForwardM1(self.address[i], 0)
self.rc.ForwardM2(self.address[i], 0)
def errorCheck(self):
'''
Checks error status of each motor controller, returns 0 if any errors occur
'''
for i in range(len(self.address)):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
#self.writeError()
rospy.loginfo("Motor controller Error", error)
return 1
def writeError(self):
'''
Writes the list of errors to a text file for later examination
'''
f = open('errorLog.txt', 'a')
errors = ','.join(str(e) for e in self.err)
f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' +
str(datetime.datetime.now()))
f.close()
class MotorControllersV2(object):
'''
Motor class contains the methods necessary to send commands to the motor controllers
for the corner and drive motors. There are many other ways of commanding the motors
from the RoboClaw, we suggest trying to write your own Closed loop feedback method for
the drive motors!
'''
def __init__(self):
## MAKE SURE TO FIX CONFIG.JSON WHEN PORTED TO THE ROVER!
#self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'],
# config['CONTROLLER_CONFIG']['baud_rate']
# )
rospy.loginfo("Initializing motor controllers")
#self.rc = Roboclaw( rospy.get_param('motor_controller_device', "/dev/serial0"),
# rospy.get_param('baud_rate', 115200))
self.rc = Roboclaw("/dev/ttyS0", 115200)
self.rc.Open()
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
address_raw = rospy.get_param('motor_controller_addresses')
address_list = (address_raw.split(','))
self.address = [None] * len(address_list)
for i in range(len(address_list)):
self.address[i] = int(address_list[i])
version = 1
for address in self.address:
rospy.loginfo("Attempting to talk to motor controller: " +
str(address))
version = version & self.rc.ReadVersion(address)[0]
rospy.loginfo("Motor controller version: " + str(version))
if version != 0:
rospy.loginfo(
"Sucessfully connected to RoboClaw motor controllers")
else:
rospy.logerr(
"Unable to establish connection to Roboclaw motor controllers")
raise Exception(
"Unable to establish connection to Roboclaw motor controllers")
self.killMotors()
for address in self.address:
self.rc.ResetEncoders(address)
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
'''
voltage = self.rc.ReadMainBatteryVoltage(0x80)[1]/10.0
if voltage >= rospy.get_param('low_voltage',11):
print "[Motor__init__] Voltage is safe at: ",voltage, "V"
else:
raise Exception("Unsafe Voltage of" + voltage + " Volts")
'''
i = 0
for address in self.address:
self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4]
self.accel[i] = int(self.qpps[i] * 2)
self.qpps[i + 1] = self.rc.ReadM2VelocityPID(address)[4]
self.accel[i + 1] = int(self.qpps[i] * 2)
i += 2
accel_max = 655359
accel_rate = 0.5
self.accel_pos = int((accel_max / 2) + accel_max * accel_rate)
self.accel_neg = int((accel_max / 2) - accel_max * accel_rate)
self.errorCheck()
self.drive_enc = [None] * 10
time.sleep(2)
self.killMotors()
def sendMotorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the drive motors,
sends open-loop commands to the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
#speed = speed/100.0
#speed *= 0.5
addr = self.address[int(motorID / 2)]
if speed > 0:
if not motorID % 2: command = self.rc.ForwardM1
else: command = self.rc.ForwardM2
else:
if not motorID % 2: command = self.rc.BackwardM1
else: command = self.rc.BackwardM2
speed = abs(int(speed * 127))
return command(addr, speed)
def sendSignedDutyAccel(self, motorID, speed):
addr = self.address[int(motorID / 2)]
if speed > 0: accel = self.accel_pos
else: accel = self.accel_neg
if not motorID % 2: command = self.rc.DutyAccelM1
else: command = self.rc.DutyAccelM2
speed = int(32767 * speed / 100.0)
return command(addr, accel, speed)
def getDriveEnc(self):
drive_enc = []
for i in range(10):
index = int(math.ceil((i + 1) / 2.0) - 1)
if not (i % 2):
drive_enc.append(self.rc.ReadEncM1(self.address[index])[1])
else:
drive_enc.append(self.rc.ReadEncM2(self.address[index])[1])
self.drive_enc = drive_enc
return drive_enc
def getBattery(self):
return self.rc.ReadMainBatteryVoltage(self.address[0])[1]
def getTemp(self):
temp = [None] * 5
for i in range(5):
temp[i] = self.rc.ReadTemp(self.address[i])[1]
return temp
def getCurrents(self):
currents = [None] * 10
for i in range(5):
currs = self.rc.ReadCurrents(self.address[i])
currents[2 * i] = currs[1]
currents[(2 * i) + 1] = currs[2]
return currents
def getErrors(self):
return self.err
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(5):
self.rc.ForwardM1(self.address[i], 0)
self.rc.ForwardM2(self.address[i], 0)
def errorCheck(self):
'''
Checks error status of each motor controller, returns 0 if any errors occur
'''
for i in range(len(self.address)):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
#self.writeError()
rospy.loginfo("Motor controller Error: " + str(error))
return 1
def writeError(self):
'''
Writes the list of errors to a text file for later examination
'''
f = open('errorLog.txt', 'a')
errors = ','.join(str(e) for e in self.err)
f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' +
str(datetime.datetime.now()))
f.close()
import time
from roboclaw import Roboclaw
address = 0x80
#Windows comport name
#rc = Roboclaw("COM11",115200)
#Linux comport name
rc = Roboclaw("/dev/serial/by-id/usb-03eb_USB_Roboclaw_2x60A-if00", 115200)
rc.ForwardMixed(address, 0)
rc.TurnRightMixed(address, 0)
while 1:
#Get version string
#rc.ForwardMixed(address, 0)
rc.ForwardM1(address, 0)
#rc.TurnRightMixed(address, 0)
time.sleep(0.02)
'''
version = rc.ReadVersion(address)
if version[0]==False:
print ("GETVERSION Failed")
else:
print (repr(version[1]))
time.sleep(1)
'''
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)
turn_right(steer)
elif cmd == 'K':
print('RIGHT')
if steer < right_limit + 3:
steer += 3
turn_right(steer)
elif cmd == ' ':
print('space')
steer = 0
fl_motor.angle = 90
fr_motor.angle = 90
bl_motor.angle = 90
br_motor.angle = 90
rc.ForwardM1(0x80, 0)
rc.ForwardM2(0x80, 0)
rc.ForwardM1(0x81, 0)
rc.ForwardM2(0x81, 0)
rc.ForwardM1(0x82, 0)
rc.ForwardM2(0x82, 0)
elif cmd == 'f':
rc.ForwardM1(0x80, 100)
rc.ForwardM2(0x80, 100)
rc.ForwardM1(0x81, 100)
rc.ForwardM2(0x81, 100)
rc.ForwardM1(0x82, 100)
rc.ForwardM2(0x82, 100)
elif cmd == 'q' or cmd == 'Q':
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()
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)
class Motor(object):
'''
Motor class contains the methods necessary to send commands to the motor controllers
for the corner and drive motors. There are many other ways of commanding the motors
from the RoboClaw, we suggest trying to write your own Closed loop feedback method for
the drive motors!
'''
def __init__(self,config):
super(Motor,self).__init__(config)
self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'],
config['CONTROLLER_CONFIG']['baud_rate']
)
self.rc.Open()
self.address = config['MOTOR_CONFIG']['controller_address']
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
version = 1
for address in self.address:
version = version & self.rc.ReadVersion(address)[0]
print(self.rc.ReadVersion(address)[0])
if version != 0:
print("[Motor__init__] Sucessfully connected to RoboClaw motor controllers")
else:
print("-----")
raise Exception("Unable to establish connection to Roboclaw motor controllers")
self.enc_min =[]
self.enc_max =[]
for address in self.address:
self.rc.SetMainVoltages(address,
int(config['BATTERY_CONFIG']['low_voltage']*10),
int(config['BATTERY_CONFIG']['high_voltage']*10)
)
if address == 131 or address == 132:
self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100))
self.enc_min.append(self.rc.ReadM1PositionPID(address)[-2])
self.enc_min.append(self.rc.ReadM2PositionPID(address)[-2])
self.enc_max.append(self.rc.ReadM1PositionPID(address)[-1])
self.enc_max.append(self.rc.ReadM2PositionPID(address)[-1])
else:
self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100))
self.rc.ResetEncoders(address)
for address in self.address:
self.rc.WriteNVM(address)
for address in self.address:
self.rc.ReadNVM(address)
voltage = self.rc.ReadMainBatteryVoltage(0x80)[1]/10.0
if voltage >= config['BATTERY_CONFIG']['low_voltage']:
print("[Motor__init__] Voltage is safe at: ",voltage, "V")
else:
raise Exception("Unsafe Voltage of" + voltage + " Volts")
i = 0
for address in self.address:
self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4]
self.accel[i] = int(self.qpps[i]*2)
self.qpps[i+1] = self.rc.ReadM2VelocityPID(address)[4]
self.accel[i+1] = int(self.qpps[i]*2)
i+=2
self.errorCheck()
def cornerToPosition(self,tick):
'''
Method to send position commands to the corner motor
:param list tick: A list of ticks for each of the corner motors to
move to, if tick[i] is 0 it instead stops that motor from moving
'''
speed, accel = 1000,2000 #These values could potentially need tuning still
self.errorCheck()
for i in range(4):
index = int(math.ceil((i+1)/2.0)+2)
if tick[i]:
if (i % 2): self.rc.SpeedAccelDeccelPositionM2(self.address[index],accel,speed,accel,tick[i],1)
else: self.rc.SpeedAccelDeccelPositionM1(self.address[index],accel,speed,accel,tick[i],1)
else:
if not (i % 2): self.rc.ForwardM1(self.address[index],0)
else: self.rc.ForwardM2(self.address[index],0)
def sendMotorDuty(self, motorID, speed):
'''
Wrapper method for an easier interface to control the drive motors,
sends open-loop commands to the motors
:param int motorID: number that corresponds to each physical motor
:param int speed: Speed for each motor, range from 0-127
'''
self.errorCheck()
addr = self.address[int(motorID/2)]
if speed > 0:
if not motorID % 2: command = self.rc.ForwardM1
else: command = self.rc.ForwardM2
else:
if not motorID % 2: command = self.rc.BackwardM1
else: command = self.rc.BackwardM2
speed = abs(int(speed * 127))
return command(addr,speed)
def killMotors(self):
'''
Stops all motors on Rover
'''
for i in range(5):
self.rc.ForwardM1(self.address[i],0)
self.rc.ForwardM2(self.address[i],0)
def errorCheck(self):
'''
Checks error status of each motor controller, returns 0 if any errors occur
'''
for i in range(5):
self.err[i] = self.rc.ReadError(self.address[i])[1]
for error in self.err:
if error:
self.killMotors()
self.writeError()
raise Exception("Motor controller Error", error)
return 1
def writeError(self):
'''
Writes the list of errors to a text file for later examination
'''
f = open('errorLog.txt','a')
errors = ','.join(str(e) for e in self.err)
f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' + str(datetime.datetime.now()))
f.close()
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
class comm:
def __init__(self):
self.rc = Roboclaw("/dev/ttyS0", 115200)
self.rc.Open()
self.accel = [0] * 10
self.qpps = [None] * 10
self.err = [None] * 5
self.address = [128, 129, 130, 131, 132]
'''
version = 1
for address in self.address:
print("Attempting to talk to motor controller",address)
version = version & self.rc.ReadVersion(0x80)
print(version)
if version != 0:
print("[Motor__init__] Sucessfully connected to RoboClaw motor controllers")
else:
raise Exception("Unable to establish connection to Roboclaw motor controllers")
'''
def accellDriveMotors(self, speeds):
for i in range(3):
self.rc.SpeedAccelM1(self.address[i], 7000,
speeds[i * 2] * 32767 / 500)
self.rc.SpeedAccelM2(self.address[i], 7000,
speeds[(i * 2 + 1)] * 32767 / 500)
def getCornerEncoders(self):
enc = []
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if not (i % 2):
enc.append(self.rc.ReadEncM1(self.address[index])[1])
else:
enc.append(self.rc.ReadEncM2(self.address[index])[1])
return enc
def moveCorners(self, tick):
enc = []
speed, accel = 1000, 2000
for i in range(4):
index = int(math.ceil((i + 1) / 2.0) + 2)
if not (i % 2):
enc.append(self.rc.ReadEncM1(self.address[index])[1])
else:
enc.append(self.rc.ReadEncM2(self.address[index])[1])
if (abs(tick[i] - enc[i]) < 25):
tick[i] = -1
if (tick[i] != -1):
if (i % 2):
self.rc.SpeedAccelDeccelPositionM2(self.address[index],
accel, speed, accel,
tick[i], 1)
else:
self.rc.SpeedAccelDeccelPositionM1(self.address[index],
accel, speed, accel,
tick[i], 1)
else:
if (i % 2): self.rc.ForwardM2(self.address[index], 0)
else: self.rc.ForwardM1(self.address[index], 0)