def joyDrive():
global FLeftAddress
global FRightAddress
global BLeftAddress
global BRightAddress
FLeftAddress = 0x80 # note that this assumes that the frontleft controller is in mode 7,
FRightAddress = 0x81 # and the the frontright is in mode 8
BLeftAddress = 0x82
BRightAddress = 0x83
#PID coefficients
Kp = 35.0
Ki = 10.0
Kd = 0
qpps = 44000
#Set PID Coefficients
roboclaw.Open("/dev/ttyACM0", 115200)
roboclaw.SetM1VelocityPID(FLeftAddress, Kp, Kd, Ki, qpps)
roboclaw.SetM2VelocityPID(BRightAddress, Kp, Kd, Ki, qpps)
roboclaw.Open("/dev/ttyACM1", 115200)
roboclaw.SetM1VelocityPID(FRightAddress, Kp, Kd, Ki, qpps)
roboclaw.SetM2VelocityPID(BLeftAddress, Kp, Kd, Ki, qpps)
# global pub
# pub = rospy.Publisher('joyDrive', String, queue_size=10)
rospy.Subscriber("cmd_vel", Twist, velCallback)
rospy.init_node('joyDrive', anonymous=True)
rospy.spin()
def __init__(self):
# Roboclaw parameters
self.m1Duty = 0
self.m2Duty = 0
#largest absolute value that the motors can be set to
self.dutyMax = 6000
self.address = 0x80
roboclawPort = '/dev/ttyACM0'
roboclaw.Open(roboclawPort,115200)
def __init__(self):
#Get the ADC fired up.
i2c_helper = ABEHelpers()
bus = i2c_helper.get_smbus()
self.adc = ADCPi(bus, 0x68, 0x69, 12)
#Open the serial port for use.
roboclaw.Open("/dev/ttyAMA0", 38400)
print("Serial port open!")
self.address = 0x80
#Networking vars. Currently set to Christian's IP and Laptop
client_IP = "10.42.0.87"
port = 6969
self.server_address = (client_IP, port)
#Set the max speed of the motors. Max/min is +32767/-32767
self.max_speed = 20000
self.min_speed = -20000
#Scaling params. Experimentally obtained.
self.adc_min = 0.600453
self.adc_max = 4.6000
self.mapped_max = 0 #Yes, max is zero and min is 100.
self.mapped_min = 100 #This is due to backward wiring in TVCBey and on my part.
#Vars needed for mapping method.
self.adc_span = self.adc_max - self.adc_min
self.mapped_span = self.mapped_max - self.mapped_min
#Initializing feedbacks
self.x_feedback = 0
self.y_feedback = 0
#PID Params. They're currently same for x and y
kp = 2500
ki = 50
kd = 0
sample_time = 0.001
#Objects from the PID.py import. See documentation there.
self.x_PID = PID.PID(kp, ki, kd)
self.y_PID = PID.PID(kp, ki, kd)
self.x_PID.setSampleTime(sample_time)
self.y_PID.setSampleTime(sample_time)
#Zero point for actuators to balance motor
self.x_zero = 54 #Experimentally defined
self.y_zero = 51
self.x_output_error_range = 1500
self.y_output_error_range = 1000
def velCallback(cmd_vel):
CONVERSION = 60000 # Conversion factor from the -1...1 input to the total velocity
ANGLE_DIM = 0.1 # Conversion factor to make the twists solwer
netVel = (cmd_vel.linear.x**2 + cmd_vel.linear.y**2)**0.5
angle = atan2(cmd_vel.linear.y, cmd_vel.linear.x)
y = cmd_vel.linear.y
x = cmd_vel.linear.x
roboclaw.Open("/dev/ttyACM0", 115200)
roboclaw.SpeedM1(
FLeftAddress,
int(CONVERSION *
(netVel * sin(angle + pi / 4) + cmd_vel.angular.z * ANGLE_DIM)))
roboclaw.SpeedM2(
FLeftAddress,
int(CONVERSION *
(netVel * sin(angle + pi / 4) - cmd_vel.angular.z * ANGLE_DIM)))
time.sleep(.01)
roboclaw.Open("/dev/ttyACM1", 115200)
roboclaw.SpeedM1(
FRightAddress,
int(-1 * CONVERSION *
(netVel * cos(angle + pi / 4) + cmd_vel.angular.z * ANGLE_DIM)))
roboclaw.SpeedM2(
FRightAddress,
int(-1 * CONVERSION *
(netVel * cos(angle + pi / 4) - cmd_vel.angular.z * ANGLE_DIM)))
# Print speeds of motors
rospy.loginfo("-----------------------------------")
rospy.loginfo("Front right " + str(
int(CONVERSION * (cmd_vel.linear.y - cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
rospy.loginfo("Front left " + str(
int(CONVERSION * (cmd_vel.linear.y + cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
rospy.loginfo("Back right " + str(
int(CONVERSION * (cmd_vel.linear.y - cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
rospy.loginfo("Back left " + str(
int(CONVERSION * (cmd_vel.linear.y + cmd.vel.linear.x) /
((cmd_vel.linear.y**2 + cmd_vel.linear.x**2)**(0.5)))))
def __init__(self):
# Roboclaw parameters
self.m1Duty = 0 # stop running
self.m2Duty = 0 # stop running
# largest absolute value that the motors can be set to
self.dutyMax = 6000
self.address = 0x80
roboclawPort = '/dev/ttyACM0'
roboclaw.Open(roboclawPort,115200)
self.pub_left = rospy.Publisher('left_voltage_pwm', Int16, queue_size = 1)
self.pub_right = rospy.Publisher('right_voltage_pwm', Int16, queue_size = 1)
def connect(self):
"""
If there are multiple roboclaws connected to the computer then there is no good way to tell which one is which
without connecting to them all. That's basically what this function does. It gets a list of all attached
Roboclaws then connects to them one-by-one and reads their device ID.
We distinguish between Roboclaws through their user-settable address.
If another instance of motornode.py is currently connected to a roboclaw then no other instances can connect
to that specific roboclaw until the other instance closes the connect. That's why this function needs to run
in a loop, another instance might be connected to this instance's roboclaw so it'll get skipped.
"""
ports = serial.tools.list_ports.comports()
random.shuffle(ports)
for usb in ports:
try:
if usb.product != 'USB Roboclaw 2x60A':
continue
rospy.logerr('about to open ' + str(usb.device))
# Open up the serial port and see if data is available at the desired address
roboclaw.Open(usb.device, 38400)
c1, c2 = roboclaw.GetConfig(self.address)
print(c1)
print(c2)
print(' ')
if (c1 is not 0) or (c2 is not 0):
self.device = usb.device
rospy.logerr(
'%s (%s) has connected to roboclaw with address %s',
self.device, self.name, self.address)
#print(self.name + ' ' + self.device)
return True
else:
roboclaw.port.close()
except IOError:
rospy.logerr('IOError')
continue
return False
#!/usr/bin/env python
import roboclaw
import rospy
from geometry_msgs.msg import Twist
from nomad.srv import RoboclawDiagnostics, RoboclawDiagnosticsResponse
roboclaw.Open("/dev/ttyACM0", 115200)
def drive_wheels(msg):
x = msg.linear.x
z = msg.angular.z
#127 is max speed
m1 = int((x - z) * 127)
m2 = int((x + z) * 127)
print str(m1) + " " + str(m2)
if m1 > 0:
roboclaw.ForwardM1(0x80, m1)
else:
roboclaw.BackwardM1(0x80, abs(m1))
if m2 > 0:
roboclaw.ForwardM2(0x80, m2)
else:
roboclaw.BackwardM2(0x80, abs(m2))
import roboclaw
#Windows comport name
roboclaw.Open("/dev/cu.usbmodem1411",115200)
#Linux comport name
#roboclaw.Open("/dev/ttyACM0",115200)
address = 0x80
print roboclaw.ReadM1PositionPID(address)
server = "localhost"
port = 1883
vhost = "/"
username = "******"
password = "******"
topic = "commands/#"
sense = SenseHat()
sense.clear()
sense.set_rotation(90)
sense.set_imu_config(False, True, False)
#Yellow TX - S1
#Orange RX - S2
roboclaw.Open("/dev/serial0", 115200)
address = 0x80
roboclaw.ForwardMixed(address, 0)
roboclaw.TurnRightMixed(address, 0)
def onConnect(client, userdata, rc): #event on connecting
client.subscribe([(topic, 1)]) #subscribe
w = threading.Thread(target=joystick_MQTT)
w2 = threading.Thread(target=slow_sensors_MQTT)
w3 = threading.Thread(target=fast_sensors_MQTT)
selectedDevice = None
w4 = threading.Thread(target=connect_controller, args=(selectedDevice, ))
w.start()
_kd = 0
_ki = .1
_kw = 1
timing = 10
lastError1 = 0
lastError2 = 0
PWM1 = 0
PWM2 = 0
increment1 = 0
increment2 = 0
GPIO.setup(FALL_BUTTON,GPIO.IN)
rc.Open("COM5",115200)
address = 0x80
# Toggle switch to activate mode where system will attempt fall once fall button is released
while FALL_BUTTON == 0 & toggle == 0:
beginning = time.clock()
# to shift targets down initialize count as something else
count = 0
index = 0
# Beginning of fall to end of fall all within 1 second, else cut motors
while time.clock-beginning < 1:
targets = instructions.targets(index)
print
speed1[1],
else:
print
"failed",
print("Speed2:"),
if (speed2[0]):
print
speed2[1]
else:
print
"failed "
# Windows comport name
roboclaw.Open("COM3", 38400)
# Linux comport name
# roboclaw.Open("/dev/ttyACM0",115200)
address = 0x80
while (1):
print
"Pos 50000"
roboclaw.SpeedAccelDeccelPositionM1(address, 32000, 12000, 32000, 50000, 0)
for i in range(0, 80):
displayspeed()
time.sleep(0.1)
time.sleep(2)
import time
import roboclaw
#Windows comport name
#roboclaw.Open("COM3",115200)
#Linux comport name
roboclaw.Open("/dev/roboclaw", 115200)
while 1:
#Get version string
version = roboclaw.ReadVersion(0x80)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
time.sleep(1)
# State Machine: Preparing to fall
# Very raw, will not compile,
import roboclaw as rc
#rc.Open("/dev/cu.usbmodem1411",115200) # LEFT USB
rc.Open("/dev/cu.usbmodem1421",115200) # RIGHT USB
address = 0x80
# start setup
# user input --> zero positions of limbs
def zero_func():
"""
Finds values for each encoder to be used as position reference.
Arguments:
(None)
Returns:
ref -- list of reference encoder values
"""
ref = [0]*3
print format(enc2[2], '02x'),
else:
print "failed ",
print "Speed1:",
if (speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
#Windows comport name
#roboclaw.Open("COM3",38400)
#Linux comport name
roboclaw.Open("/dev/roboclaw", 1000000)
address = 0x80
version = roboclaw.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while (1):
displayspeed()
def connect(self):
try:
roboclaw.Open(self.device, 38400)
return True
except IOError:
return False
else:
print "failed ",
print "Speed1:",
if (speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
#Windows comport name
roboclaw.Open("/dev/cu.usbmodem1411",
115200) #2400,9600,19200,38400,57600,115200,230400,460800
#Linux comport name
#roboclaw.Open("/dev/ttyACM0",115200)
address = 0x80
version = roboclaw.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
while (1):
displayspeed()
#!/usr/bin/env python
import roboclaw as r
r.Open('/dev/ttySAC0', 38400)
r.ForwardM1(128,0)
r.ForwardM2(128,0)
r.ForwardM1(129,0)
#r.SpeedM1M2(128,0,0)
#r.SpeedM1(129,0)
import time
import os
import numpy as np
# ELECTRICAL LIBRARIES
import roboclaw as rc
import RPi.GPIO as GPIO
######################### Setup Electrical Interfacing #########################
motorDetected = False
for comPort in config.comPorts:
try:
motorDetected = rc.Open(comPort, config.baudRate)
break
except:
continue
if not motorDetected:
print('Motor port could not be opened.')
print('Ports checked:')
for i in config.comPorts:
print('\t'+i)
print('Exiting...')
exit()
# INITIALIZE GPIO PORTS
GPIO.setmode(GPIO.BCM)
GPIO.setup(config.SPIMOSI, GPIO.OUT)
import time
import roboclaw
#Windows comport name
roboclaw.Open("COM3", 115200)
#Linux comport name
#roboclaw.Open("/dev/ttyACM0",115200)
address = 0x80
while (1):
roboclaw.ForwardM1(address, 64)
roboclaw.BackwardM2(address, 64)
time.sleep(2)
roboclaw.BackwardM1(address, 64)
roboclaw.ForwardM2(address, 64)
time.sleep(2)
roboclaw.ForwardBackwardM1(address, 96)
roboclaw.ForwardBackwardM2(address, 32)
time.sleep(2)
roboclaw.ForwardBackwardM1(address, 32)
roboclaw.ForwardBackwardM2(address, 96)
time.sleep(2)
print "Speed1:",
if (speed1[0]):
print speed1[1],
else:
print "failed",
print("Speed2:"),
if (speed2[0]):
print speed2[1]
else:
print "failed "
#Windows comport name
#roboclaw.Open("COM3",38400)
#Linux comport name
roboclaw.Open("/dev/ttySAC0", 38400)
address = 0x80
version = roboclaw.ReadVersion(address)
if version[0] == False:
print "GETVERSION Failed"
else:
print repr(version[1])
roboclaw.SpeedM1(address, 10)
roboclaw.SpeedM2(address, 10)
time.sleep(2)
roboclaw.SpeedM1(address, 0)
roboclaw.SpeedM2(address, 0)
