def ascii(): # saber = Sabertooth(port, baudrate=115200) saber = Sabertooth(port, baudrate=38400) try: print('temperature [C]: {}'.format(saber.textGet(b'm2:gett'))) print('battery [mV]: {}'.format(saber.textGet(b'm2:getb'))) saber.text(b'm1:startup') saber.text(b'm2:startup') for speed in range(-2000, 2000, 500): # print('.') # def independentDrive(self, dir_left="fwd", speed_left=0, dir_right="fwd", speed_right=0) # saber.independentDrive('fwd', 50, 'fwd', 50) saber.text(b'm1:{}'.format(speed)) saber.text(b'm2:{}'.format(speed)) # format returned text m1 = saber.textGet(b'p1:get').split()[1] m2 = saber.textGet(b'p2:get').split()[1] c1 = saber.textGet(b'm1:getc')[:-2].split('C')[1] c2 = saber.textGet(b'm2:getc')[:-2].split('C')[1] # print(c1) print('M1: {:6} {:>6} mA M2: {:6} {:>6} mA'.format( m1, c1, m2, c2)) time.sleep(1) except KeyboardInterrupt: print('keyboard interrupt ... ') saber.text(b'm1:0') saber.text(b'm2:0') time.sleep(0.1) saber.stop()
class MotorController(): port = '/dev/ttyTHS2' baud = 9600 address = 128 timeout = 0.1 def __init__(self, use_hardware=False): self.use_hardware = use_hardware if self.use_hardware: print("Setting up motor controller") self.saber = Sabertooth(port=self.port, baudrate=self.baud, address=self.address, timeout=self.timeout) else: print("Hardware controller is running in 'mock' model") def drive(self, wheel, value): if self.use_hardware: print("Sending %.1f to motor %i"%(value, wheel)) self.saber.drive(wheel, value) else: print("Would send %f to motor %i"%(value, wheel)) def stop(self, wheel, value): self.drive(1, 0) self.drive(2, 0) if self.use_hardware: print("Sending stop to motors") self.saber.stop() else: print("Would stop to motors")
def packet(): # saber = Sabertooth(port, baudrate=115200) saber = Sabertooth(port, baudrate=38400) try: print('temperature [C]: {}'.format(saber.textGet('m2:gett'))) print('battery [mV]: {}'.format(saber.textGet('m2:getb'))) saber.text('m1:startup') saber.text('m2:startup') for speed in range(-100, 100, 20): saber.drive(1, speed) saber.drive(2, speed) # format returned text m1 = saber.textGet('m1:get').split()[1] m2 = saber.textGet('m2:get').split()[1] print('M1: {:6} M2: {:6}'.format(m1, m2)) time.sleep(1) except KeyboardInterrupt: print('keyboard interrupt ... ') saber.drive(1, 0) saber.drive(2, 0) time.sleep(0.1) saber.stop()
class Motors: def __init__(self): self.saber = Sabertooth('/dev/ttyS0') # Stop the motors just in case a previous run as resulted in an infinite loop. self.saber.stop() def follow(self, area_buffer, pan): # Change this variable to increase the reverse speed while following, lower is faster (min = -100) saber_rmin_speed = -50 # Change this variable to reduce the max forward speed, lower is slower (0 = stop) saber_fmax_speed = 100 # Change this variable for more or less aggression while turning (the lower the value, the more aggressive the # turns). This should be determined while tuning to your environment. Weight of the rover, floor type, and wheel # traction are the some of the determining factors. turn_aggression = 64 # Change this variable to adjust speed from the area given, if you're attempting to follow bigger objects in the # frame, set the value higher. proportional_area = 200 while True: # If the area buffer is empty, skip over the code and start at the beginning to check again. if area_buffer.empty(): self.saber.stop() continue area = area_buffer.get() pan_angle = pan.value # Determine how far off the center location (typically, the angle at which the Pan Servo points directly # forward). In this case, the Pan Servo starts at angle 100 degrees. follow_error = 100 - pan_angle # The forward speed is determined by the area of the object passed in via the area_buffer. This equation # results in faster speeds as the area gets smaller and slower speeds as the area gets bigger (eventually # reversing if the area is too large). forward_speed = constrain(100 - (area // proportional_area), saber_rmin_speed, saber_fmax_speed) # This equations sets the speed differential based on how far the object/pan angle is from the original # center location. differential = (follow_error + (follow_error * forward_speed)) / turn_aggression left_speed = constrain(forward_speed - differential, saber_rmin_speed, saber_fmax_speed) right_speed = constrain(forward_speed + differential, saber_rmin_speed, saber_fmax_speed) print("area: {} follow_error: {} forward speed: {} differential: {} left_speed: {} right_speed: {}" .format(area, follow_error, forward_speed, differential, left_speed, right_speed)) self.saber.drive(1, left_speed) self.saber.drive(2, right_speed)
def main(): saber = Sabertooth('/dev/tty.usbserial-A6033KY3', baudrate=9600, address=128, timeout=0.1) # drive(number, speed) # number: 1-2 # speed: -100 - 100 saber.drive(1, 50) saber.drive(2, -75) saber.stop()
def run(self): smc = SMC('/dev/tty.usbserial0') smc.init() smc.stop() # make sure we are stopped? saber = Sabertooth('/dev/tty.usbserial1') saber.stop() # make sure we are stopped? # pub = zmq.Pub() # do i need to feedback motor errors? sub = zmq.Sub(['cmd', 'dome']) while True: topic, msg = sub.recv() if msg: if topic == 'cmd': print('got cmd') elif topic == 'dome': print('got dome') else: time.sleep(0.5)
import time from pysabertooth import Sabertooth saber = Sabertooth('/dev/ttyS0') # Stop the motors just in case a previous run as resulted in an infinite loop. saber.stop() saber_min_speed = -100 saber_max_speed = 100 print('Running motors at max forward speed for 2 seconds...') saber.drive(1, saber_max_speed) saber.drive(2, saber_max_speed) time.sleep(2) saber.stop() print('Running motors at max reverse speed for 2 seconds...') saber.drive(1, saber_min_speed) saber.drive(2, saber_min_speed) time.sleep(2) saber.stop() print('Running left motors at half forward speed for 2 seconds...') saber.drive(1, saber_max_speed // 2) time.sleep(2) saber.stop() print('Running right motors at half forward speed for 2 seconds...') saber.drive(2, saber_max_speed // 2) time.sleep(2) saber.stop()
def main(cam_state, pt_state, mot_state): processes = [] # Pan and Tilt Servo angles are determined by the HS-422 Servos and Adafruit ServoKit library used for this project. start_pan_angle = 100 start_tilt_angle = 140 try: with Manager() as manager: # Initialize the multiprocessing queue buffers. Setting a maxsize of 1 for the buffers currently results in # optimal performance as there is some latency due to the Raspberry Pi environment. cam_buffer = Queue(maxsize=1) detection_buffer = Queue(maxsize=1) area_buffer = Queue(maxsize=1) center_buffer = Queue(maxsize=1) # Initialize the multiprocessing manager values for the pan and tilt process to provide input to the # pan/tilt and motor processes. pan = manager.Value("i", start_pan_angle) tilt = manager.Value("i", start_tilt_angle) if cam_state == 1: cam = Camera() det = Detect(myriad=True) camera_process = Process(target=cam.start, args=(cam_buffer, detection_buffer, center_buffer, area_buffer), daemon=True) camera_process.start() processes.append(camera_process) detection_process = Process(target=det.start, args=(cam_buffer, detection_buffer), daemon=True) detection_process.start() processes.append(detection_process) if pt_state == 1: servo = Servos(start_pan_angle, start_tilt_angle) pan_tilt_process = Process(target=servo.follow, args=(center_buffer, pan, tilt), daemon=True) pan_tilt_process.start() processes.append(pan_tilt_process) if mot_state == 1: mot = Motors() motor_process = Process(target=mot.follow, args=(area_buffer, pan), daemon=True) motor_process.start() processes.append(motor_process) for process in processes: process.join() except: print("Unexpected error: ", sys.exc_info()[0]) finally: for p in range(len(processes)): processes[p].terminate() # Ensure the motors are stopped before exiting. saber = Sabertooth('/dev/ttyS0') saber.stop() sys.exit(0)