def connect(self):
try:
print "Connecting to Arduino on port", self.port, "..."
self.channel = PyMata(self.port)
# Note: In order to keep the firmata firmware simple and small we create a controller to handle the integration
# of the motors, encoders and pid control. Hopefully, the speed is not an issue because of the back-and-forth
# between the Arduino via firmata.
self.rab = RosArduinoBridgePyMataController(
self.channel, self.params)
print "Connected at", self.baudrate
print "Arduino is ready."
except:
print "Exception:"
print sys.exc_info()
print "Traceback follows:"
traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!"
os._exit(1)
def connect(self):
try:
print "Connecting to Arduino on port", self.port, "..."
self.channel = PyMata(self.port)
# Note: In order to keep the firmata firmware simple and small we create a controller to handle the integration
# of the motors, encoders and pid control. Hopefully, the speed is not an issue because of the back-and-forth
# between the Arduino via firmata.
self.rab = RosArduinoBridgePyMataController( self.channel, self.params )
print "Connected at", self.baudrate
print "Arduino is ready."
except:
print "Exception:"
print sys.exc_info()
print "Traceback follows:"
traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!"
os._exit(1)
class PymataDriver(ArduinoBase):
def __init__(self, params, *args, **kwargs):
super(PymataDriver, self).__init__(*args, **kwargs)
# Note: The PID Rate/Interval doesn't have to be fixed. It can be specified in the ROS parameters and passed
# into this class and onto the PyMata controller and the RAB controller (optionally)
self.channel = None
self.rab = None
self.params = params
def connect(self):
try:
print "Connecting to Arduino on port", self.port, "..."
self.channel = PyMata(self.port)
# Note: In order to keep the firmata firmware simple and small we create a controller to handle the integration
# of the motors, encoders and pid control. Hopefully, the speed is not an issue because of the back-and-forth
# between the Arduino via firmata.
self.rab = RosArduinoBridgePyMataController( self.channel, self.params )
print "Connected at", self.baudrate
print "Arduino is ready."
except:
print "Exception:"
print sys.exc_info()
print "Traceback follows:"
traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!"
os._exit(1)
def open(self):
pass
def close(self):
self.rab.Close()
def update_pid(self, Kp, Kd, Ki, Ko):
''' Set the PID parameters on the Arduino
'''
self.rab.UpdatePid( { 'Kp' : Kp, 'Kd' : Kd, 'Ki' : Ki, 'Ko' : Ko } )
def get_baud(self):
''' Get the current baud rate on the serial port.
'''
return self.baudrate
def get_encoder_counts(self):
encoders = self.rab.ReadEncoders()
return [ encoders['left'], encoders['right'] ]
def reset_encoders(self):
''' Reset the encoder counts to 0
'''
self.rab.ResetEncoders()
def drive(self, left, right):
''' Speeds are given in encoder ticks per PID interval
'''
# Remember: left, right are in units of ticks per PID interval
self.rab.DriveMotors( {'left' : left, 'right' : right } )
pass
def drive_m_per_s(self, right, left):
''' Set the motor speeds in meters per second.
'''
left_revs_per_second = float(left) / (self.wheel_diameter * PI)
right_revs_per_second = float(right) / (self.wheel_diameter * PI)
left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
self.drive(right_ticks_per_loop , left_ticks_per_loop )
def stop(self):
''' Stop both motors.
'''
self.drive(0, 0)
def analog_read(self, pin):
return self.channel.analog_read(pin)
def analog_write(self, pin, value):
self.channel.analog_write(pin, value)
def digital_read(self, pin):
return self.channel.digital_read(pin)
def digital_write(self, pin, value):
self.channel.digital_write(pin, value)
def pin_mode(self, pin, mode):
self.channel.set_pin_mode(pin, mode, params[pin]['type'])
def servo_write(self, id, pos):
''' Usage: servo_write(id, pos)
Position is given in radians and converted to degrees before sending
'''
self.channel.ServoWrite(id, pos)
def servo_read(self, id):
''' Usage: servo_read(id)
The returned position is converted from degrees to radians
'''
return radians(self.ServoRead(id))
def ping(self, pin):
''' The srf05/Ping command queries an SRF05/Ping sonar sensor
connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based.
'''
pass
class PymataDriver(ArduinoBase):
def __init__(self, params, *args, **kwargs):
super(PymataDriver, self).__init__(*args, **kwargs)
# Note: The PID Rate/Interval doesn't have to be fixed. It can be specified in the ROS parameters and passed
# into this class and onto the PyMata controller and the RAB controller (optionally)
self.channel = None
self.rab = None
self.params = params
def connect(self):
try:
print "Connecting to Arduino on port", self.port, "..."
self.channel = PyMata(self.port)
# Note: In order to keep the firmata firmware simple and small we create a controller to handle the integration
# of the motors, encoders and pid control. Hopefully, the speed is not an issue because of the back-and-forth
# between the Arduino via firmata.
self.rab = RosArduinoBridgePyMataController(
self.channel, self.params)
print "Connected at", self.baudrate
print "Arduino is ready."
except:
print "Exception:"
print sys.exc_info()
print "Traceback follows:"
traceback.print_exc(file=sys.stdout)
print "Cannot connect to Arduino!"
os._exit(1)
def open(self):
pass
def close(self):
self.rab.Close()
def update_pid(self, Kp, Kd, Ki, Ko):
''' Set the PID parameters on the Arduino
'''
self.rab.UpdatePid({'Kp': Kp, 'Kd': Kd, 'Ki': Ki, 'Ko': Ko})
def get_baud(self):
''' Get the current baud rate on the serial port.
'''
return self.baudrate
def get_encoder_counts(self):
encoders = self.rab.ReadEncoders()
return [encoders['left'], encoders['right']]
def reset_encoders(self):
''' Reset the encoder counts to 0
'''
self.rab.ResetEncoders()
def drive(self, left, right):
''' Speeds are given in encoder ticks per PID interval
'''
# Remember: left, right are in units of ticks per PID interval
self.rab.DriveMotors({'left': left, 'right': right})
pass
def drive_m_per_s(self, right, left):
''' Set the motor speeds in meters per second.
'''
left_revs_per_second = float(left) / (self.wheel_diameter * PI)
right_revs_per_second = float(right) / (self.wheel_diameter * PI)
left_ticks_per_loop = int(left_revs_per_second *
self.encoder_resolution * self.PID_INTERVAL *
self.gear_reduction)
right_ticks_per_loop = int(right_revs_per_second *
self.encoder_resolution *
self.PID_INTERVAL * self.gear_reduction)
self.drive(right_ticks_per_loop, left_ticks_per_loop)
def stop(self):
''' Stop both motors.
'''
self.drive(0, 0)
def analog_read(self, pin):
return self.channel.analog_read(pin)
def analog_write(self, pin, value):
self.channel.analog_write(pin, value)
def digital_read(self, pin):
return self.channel.digital_read(pin)
def digital_write(self, pin, value):
self.channel.digital_write(pin, value)
def pin_mode(self, pin, mode):
self.channel.set_pin_mode(pin, mode, params[pin]['type'])
def servo_write(self, id, pos):
''' Usage: servo_write(id, pos)
Position is given in radians and converted to degrees before sending
'''
self.channel.ServoWrite(id, pos)
def servo_read(self, id):
''' Usage: servo_read(id)
The returned position is converted from degrees to radians
'''
return radians(self.ServoRead(id))
def ping(self, pin):
''' The srf05/Ping command queries an SRF05/Ping sonar sensor
connected to the General Purpose I/O line pinId for a distance,
and returns the range in cm. Sonar distance resolution is integer based.
'''
pass
