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