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)