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 __init__(self,
DUAL,
PORT,
RATE=38400,
ADDRESSES=[128, 129],
MAX_SPEED=127,
NODE_NAME='Motors'):
# Definition of constants.
self.ADDRESSES = ADDRESSES
self.DUAL = DUAL
self.PORT = PORT
# Definition of useful tool objects.
self.tank = tank.tank_steering(MAX_SPEED)
self.joysticks = joysticks_data()
# Opening port for communication.
self.open()
# Opening comunication between drivers and program.
self.drivers = Roboclaw('/dev/' + self.PORT, RATE)
self.drivers.Open()
# Enable Ros Comunication and it's suscribe frequency.
rospy.init_node(NODE_NAME)
rospy.Subscriber('joy', Joy, self.ros_suscribe)
r = rospy.Rate(200)
# Control loop for comunication with the drivers.
while not rospy.is_shutdown():
self.tank_drive()
r.sleep()
def main():
global speed
speed = 64 #half the [0-127] to get 0 speed in ForwardBackward command
msgAckermann = AckermannDrive()
rospy.init_node('mobile_base_node', anonymous=True)
rospy.Subscriber("/ackermann", AckermannDrive, callbackAckermann)
rate = rospy.Rate(10)
rc = Roboclaw("/dev/ttyACM1", 115200)
rc.Open()
address = 0x80
br = tf.TransformBroadcaster()
autobot_x = 0
while not rospy.is_shutdown():
autobot_x += speed * 0.001
msgAckermann.speed = speed
rc.ForwardBackwardM1(address,
int(msgAckermann.speed)) #0 power forward = 64
rc.ForwardBackwardM2(address,
int(-msgAckermann.speed)) #0 power backward = 64
br.sendTransform((autobot_x, 0, 0),
tf.transformations.quaternion_from_euler(0, 0, 0),
rospy.Time.now(), "base_link", "odom")
rate.sleep()
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 root_menu():
global rc
if rc is None:
for device in potentialDevices():
newrc = Roboclaw("/dev/" + device, 115200, 0.01, 3)
if newrc.Open():
rc = newrc
break
# Failed to connect to USB, fall back to test stub.
if rc is None:
rc = Roboclaw_stub()
rcAddr = tryParseAddress(request.args.get('address'), default=128)
displayMenu = False
if rcAddr is not None:
try:
verString = readResult(rc.ReadVersion(rcAddr))
flash(
"Roboclaw at address {0} ({0:#x}) version: {1}".format(
rcAddr, verString), successCategory)
displayMenu = True
except ValueError as ve:
flash(
"No Roboclaw response from address {0} ({0:#x})".format(
rcAddr), errorCategory)
else:
roboResponse = None
return render_template("root_menu.html",
display=displayMenu,
address=rcAddr)
def connect_menu():
global rc
if request.method == 'GET':
return render_template("connect_menu.html",
potentialDevices=potentialDevices())
elif request.method == 'POST':
# TODO sanity validation of these values from the HTML form
portName = request.form['port']
baudrate = int(request.form['baudrate'])
interCharTimeout = float(request.form['interCharTimeout'])
retries = int(request.form['retries'])
if portName == 'Test_Stub':
# No RoboClaw - use the test stub.
newrc = Roboclaw_stub()
else:
# Create the Roboclaw object against the specified serial port
newrc = Roboclaw(portName, baudrate, interCharTimeout, retries)
if newrc.Open():
rc = newrc
flash("Roboclaw API connected to " + portName, successCategory)
return redirect(url_for('root_menu', address="0x80"))
else:
flash("Roboclaw API could not open " + portName, errorCategory)
return redirect(url_for('connect_menu'))
else:
flash("Unexpected request method on connect", errorCategory)
return redirect(url_for('connect_menu'))
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 __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 stop_motors():
print('shutting down motors...')
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
address = 0x80
rc.SpeedM1(address, 0) # M1 for linear movement
rc.SpeedM2(address, 0) # M2 for turning
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()
def connect(self):
"""
Read all configuration parameters (not just connect) and use the connect
parameters to create new RoboClaw API handle.
"""
# First load configuration file
config = configuration.configuration("roboclaw")
allparams = config.load()
self.velocityparams = allparams['velocity']
self.angleparams = allparams['angle']
# Use connect configuration to create a RoboClaw API handle
portname = allparams['connect']['port']
if portname == 'TEST':
self.roboclaw = Roboclaw_stub()
else:
baudrate = allparams['connect']['baudrate']
timeout = allparams['connect']['timeout']
retries = allparams['connect']['retries']
newrc = Roboclaw(portname, baudrate, timeout, retries)
if newrc.Open():
self.roboclaw = newrc
else:
raise ValueError("Could not connect to RoboClaw. {} @ {}".format(portname, baudrate))
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]
'''
def init(uart='/dev/TTYTHS0', baud=115200):
global rc
rc = Roboclaw(uart, baud)
rc.Open()
global pub
pub = rospy.Publisher('arm_motion', String, queue_size=10)
rospy.Subscriber('joy', Joy, manual_control)
rospy.init_node('base_motors', anonymous=True)
rospy.spin()
def run_motors(M1_counts, M2_counts):
### send motor commands M1 forward, M2 turning###
rc = Roboclaw("/dev/ttyACM0", 115200)
rc.Open()
address = 0x80
print('running motors')
rc.SpeedM1(address, int(M1_counts)) # M1 for linear movement
rc.SpeedM2(address, int(M2_counts)) # M2 for turning
display_speed()
display_power_values()
def __init__(self):
'''
Initialization of communication parameters for the Motor Controllers
'''
self.rc = Roboclaw("/dev/ttyS0", 115200)
self.rc.Open()
self.address = [0x80, 0x81, 0x82, 0x83, 0x84]
self.rc.ResetEncoders(self.address[0])
self.rc.ResetEncoders(self.address[1])
self.rc.ResetEncoders(self.address[2])
self.err = [None] * 5
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")
def __init__(self,
serial_port='/dev/ttyACM0',
baudrate=115200,
mode='PWM',
motors_connected={
"left": [],
"right": []
},
index=0):
self.name = "mtr_" + str(index)
self._serial_port = serial_port
self._baudrate = baudrate
self._address = 0x80
self._rc = Roboclaw(self._serial_port, self._baudrate)
self._rc.Open()
self.stop_service = rospy.Service(self.name + '_stop_motors', stop,
self.stop_srv)
self._mode = mode
self.current_speed = 0
if len(motors_connected["left"]) + len(motors_connected["right"]) > 2:
raise ValueError(
"You can only connect two motors to these controllers!")
self.motors_connected = {"left": [], "right": []}
self.motor_number = 0
for side in motors_connected.keys():
for motor in motors_connected[side]:
if motor:
if self.motor_number == 0:
self.motors_connected[side].append(
Motor(name=str(motor), side=side))
self.motor_number += 1
elif self.motor_number == 1:
self.motors_connected[side].append(
Motor(name=str(motor), side=side))
else:
raise ValueError('Too many motors, numer is: ' +
str(self.motor_number))
## ROS Interfaces:
self.r = rospy.Rate(10)
self.encoder_publisher = rospy.Publisher(self.name + '_enc_val',
encoder_values,
queue_size=1)
self.speed_publisher = rospy.Publisher(self.name + '_speed_val',
speed_values,
queue_size=1)
self.mode_service = rospy.Service(self.name + "_mode_service",
set_controller_mode,
self.set_controller_mode)
def __init__(self, log):
self.lfbias = 48 # experimentally determined for 'Spot 2'
self.lrbias = 44
self.rrbias = 69
self.rfbias = 40
self.pan_bias = 83
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.speedfactor = 35 # 8000 counts at 100%
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.pan = kit.servo[15]
self.tilt = kit.servo[14]
#pan_bias = 0 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.crc = 0
self.port = serial.Serial("/dev/ttyS0", baudrate=115200, timeout=0.1)
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()
ver = self.rc.ReadVersion(0x80)
print(ver[0], ver[1])
ver = self.rc.ReadVersion(0x81)
print(ver[0], ver[1])
ver = self.rc.ReadVersion(0x82)
print(ver[0], ver[1])
def __init__(self):
print "Trying to connect to motors..."
self.motor_status = 0
try: # First step: connect to Roboclaw controller
self.port = "/dev/ttyACM0"
self.argo = Roboclaw(self.port, 115200)
self.argo.Open()
self.address = 0x80 # Roboclaw address
self.version = self.argo.ReadVersion(
self.address
) # Test connection by getting the Roboclaw version
except:
print "Unable to connect to Roboclaw port: ", self.port, "\nCheck your port and setup then try again.\nExiting..."
self.motor_status = 1
return
# Follow through with setup if Roboclaw connected successfully
print "Roboclaw detected! Version:", self.version[1]
print "Setting up..."
# Set up publishers and subscribers
self.cmd_sub = rospy.Subscriber("/argo/wheel_speeds", Twist,
self.get_wheel_speeds)
self.encoder = rospy.Publisher("/argo/encoders", Encoder, queue_size=5)
# Set class variables
self.radius = 0.0728 # Wheel radius (m)
self.distance = 0.372 # Distance between wheels (m)
self.max_speed = 13000 # Global max speed (in QPPS)
self.session_max = 13000 # Max speed for current session (in QPPS)
self.rev_counts = 3200 # Encoder clicks per rotation
self.circ = .4574 # Wheel circumference (m)
self.counts_per_m = int(self.rev_counts /
self.circ) # Encoder counts per meter
self.conv = self.rev_counts / (2 * pi) # Wheel speed conversion factor
self.Lref = 0 # Left wheel reference speed
self.Rref = 0 # Right wheel reference speed
self.Lprev_err = 0 # Previous error value for left wheel
self.Rprev_err = 0 # Previous error value for right wheel
self.Kp = .004 # Proportional gain
self.Kd = .001 # Derivative gain
self.Ki = .0004 # Integral gain
self.LEint = 0 # Left wheel integral gain
self.REint = 0 # Right wheel integral gain
print "Setup complete, let's roll homie ;)\n\n"
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=10)
except FileNotFoundError:
fh = logging.handlers.TimedRotatingFileHandler('agitator.log',
when='D',
interval=1,
utc=True,
backupCount=10)
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 main():
global serverSocket
serverSocket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
serverSocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
serverSocket.bind(("192.168.1.186", 6666))
global roboclaw
roboclaw = Roboclaw("/dev/serial0", 38400)
roboclaw.Open()
global LeftDrive, RightDrive, LinActuators, Chainsaw
LeftDrive = 0x80
RightDrive = 0x81
LinActuators = 0x82
Chainsaw = 0x83
while True:
loop()
def setup(ip=c.pi_ip):
global inter
global address1
global address2
global address3
global roboclaw
inter = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
inter.bind((ip, 8080))
inter.listen(5)
address1 = 0x80 #front motors
address2 = 0x81 #mid motors
address3 = 0x82 #back motors
roboclaw = Roboclaw("/dev/ttyS0", 38400)
roboclaw.Open()
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
def find_controllers(self):
addressList = [0x80, 0x81, 0x82, 0x83, 0x84, 0x85, 0x86, 0x87]
controller_dictionary = {}
speed_factors = {}
counter = 0
for i in range(0, 20):
rc = Roboclaw(self.baseStr + str(i), self.rate)
if rc.Open() == 0:
for a in addressList:
if rc.GetConfig(a) != (0, 0):
controller_dictionary[a] = rc
speed_factors[a] = [1, 1, 1]
counter += 1
break
if counter == 3:
break
return controller_dictionary, speed_factors
def init_rc():
global rc
global rc_address
# Initialise the roboclaw motorcontroller
print("Initialising roboclaw driver...")
from roboclaw import Roboclaw
rc_address = 0x80
rc = Roboclaw("/dev/roboclaw", 115200)
rc.Open()
# Get roboclaw version to test if is attached
version = rc.ReadVersion(rc_address)
if version[0] is False:
print("Roboclaw get version failed")
sys.exit()
else:
print(repr(version[1]))
# Set motor controller variables to those required by K9
rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS)
rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS)
rc.SetMainVoltages(rc_address,232,290) # 23.2V min, 29V max
rc.SetPinFunctions(rc_address,2,0,0)
# Zero the motor encoders
rc.ResetEncoders(rc_address)
# Print Motor PID Settings
m1pid = rc.ReadM1VelocityPID(rc_address)
m2pid = rc.ReadM2VelocityPID(rc_address)
print("M1 P: " + str(m1pid[1]) + ", I:" + str(m1pid[2]) + ", D:" + str(m1pid[3]))
print("M2 P: " + str(m2pid[1]) + ", I:" + str(m2pid[2]) + ", D:" + str(m2pid[3]))
# Print Min and Max Main Battery Settings
minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts
print ("Min Main Battery: " + str(int(minmaxv[1])/10) + "V")
print ("Max Main Battery: " + str(int(minmaxv[2])/10) + "V")
# Print S3, S4 and S5 Modes
S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined']
S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home']
S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home']
pinfunc = rc.ReadPinFunctions(rc_address)
print ("S3 pin: " + S3mode[pinfunc[1]])
print ("S4 pin: " + S4mode[pinfunc[2]])
print ("S5 pin: " + S5mode[pinfunc[3]])
print("Roboclaw motor controller initialised...")
def __init__(self, serport=DEFAULT_PORT):
self._rc = Roboclaw(serport, 115200)
self._rc_polling_thread = Thread(target=self._polling_loop,
name="Encoders Polling Thread")
self.axes = []
self.addrs = []
for (name, addr, m_nr, def_spd, def_acc) in self.axis_map:
self.axes.append(Axis(self._rc, name, addr, m_nr, def_spd,
def_acc))
if addr not in self.addrs:
self.addrs.append(addr)
self.diff_axes = [
DiffTransmission(self.axes[4], self.axes[5]),
DiffTransmission(self.axes[5], self.axes[4])
]
def __init__(self, configPath):
print 'reading config from path...' + configPath
parser = SafeConfigParser()
parser.read(configPath)
#Reading the configurations
self._P = parser.get('pid_coeff', 'P')
self._I = parser.get('pid_coeff', 'I')
self._D = parser.get('pid_coeff', 'D')
self._PR = parser.get('pid_coeff', 'PR')
self._PL = parser.get('pid_coeff', 'PL')
self._speed = parser.get('motion', 'speed')
self._accel = parser.get('motion', 'accel')
self._samplingRate = parser.get('pid_coeff', 'samplingRate')
self._port = parser.get('systems', 'port')
self._robot = Robot()
#setup the roboclaw here
self._rc = Roboclaw(self._port, 115200)
self._rc.Open()
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
#***Before using this example the motor/controller combination must be
#***tuned and the settings saved to the Roboclaw using IonMotion.
#***The Min and Max Positions must be at least 0 and 50000
import time
from roboclaw import Roboclaw
#Windows comport name
#rc = Roboclaw("COM3",115200)
#Linux comport name
rc = Roboclaw("/dev/ttyACM0", 115200)
f = open("final.csv", "w+")
def displayspeed(ti):
enc1 = rc.ReadEncM1(address)
enc2 = rc.ReadEncM2(address)
speed1 = rc.ReadSpeedM1(address)
speed2 = rc.ReadSpeedM2(address)
f.write(str(ti) + ",")
print("Encoder1:"),
if (enc1[0] == 1):
print enc1[1],
print format(enc1[2], '02x'),
f.write(str(enc1[1] * 0.18))
f.write(",")
else:
print "failed",
print "Encoder2:",
if (enc2[0] == 1):
print enc2[1],
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