def __init__(self): # if you want to switch to hardware PWM, remember: # GPIO12(Board 32) & GPIO18(Board 12) share a setting as do GPIO13(Board 33) & GPIO19(Board 35) # NOTE: all gpio pin numbers are BCM self.gpioPin_SabertoothS1 = 18 self.gpioPin_SabertoothS2 = 12 self.gpioPin_Syren10 = 13 # board pins 16 (BCM 23) and 18 (BCM 24) initialize to LOW at boot self.gpioPin_2_leg_mode = 23 self.gpioPin_3_leg_mode = 24 self.pi = pigpio.pi() self.pi.set_mode(self.gpioPin_2_leg_mode, pigpio.OUTPUT) self.pi.write(self.gpioPin_2_leg_mode, 0) self.pi.set_mode(self.gpioPin_3_leg_mode, pigpio.OUTPUT) self.pi.write(self.gpioPin_3_leg_mode, 0) self.pi.set_mode(self.gpioPin_Syren10, pigpio.OUTPUT) self.pi.set_PWM_frequency(self.gpioPin_Syren10, 50) self.pi.set_servo_pulsewidth(self.gpioPin_Syren10, 0) # to find the usb port of the sabertooth run this: cd /dev # and ls to see the list of usb ports, then plug in your usb to the sabertooth and ls again to see the new port self.saber = Sabertooth('/dev/ttyACM0', baudrate=115200, address=128, timeout=0.1) self.initializeXboxController() self.initializeSoundController() self.initializePeekabooController() self.running = True
class DCBrushed: def __init__(self, address, motor_num): """ A DC brushed motor class for sabertooth motor controllers in packetized serial Dependencies: pysabertooth (https://github.com/MomsFriendlyRobotCompany/pysabertooth) Instance variables: address = address of the sabertooth motor controller in packetized serial mode motor_num = 1 or 2; the port number of the motor controller of the sabertooth; there are two motors available for each controller """ self.address = address self.motor_num = motor_num self.motor = Sabertooth('/dev/serial0', baudrate=9600, address=self.address) def drive(self, speed): """ Sets the motor to a given power speed = a value in the range [-100, 100] """ self.motor.drive(self.motor_num, speed)
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 __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")
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)
def initialize(): global saber PATH = 'direct2642_noisy_resnet2.pt' #direct2642_noisy_resnet2.pt, direct2642_noisy.pt quality = "qhd" print("\n Initializing Kinect ...") # command="roslaunch kinect2_bridge kinect2_bridge.launch" # os.system("gnome-terminal -e 'bash -c \"roslaunch kinect2_bridge kinect2_bridge.launch; exec bash\"'") imgtopic = "/kinect2/{}/image_color".format(quality) print("\nDetecting sabertooth....\n") pl = list(port.comports()) print(pl) address = '' for p in pl: print(p) if 'Sabertooth' in str(p): address = str(p).split(" ") print("\nAddress found @") print(address[0]) saber = Sabertooth(address[0], baudrate=9600, address=128, timeout=0.1) if torch.cuda.is_available(): device = torch.device("cuda") use_gpu = 1 print("CUDA Available") else: device = torch.device("cpu") model = cnn_finetune.make_model('resnet18', num_classes=1, pretrained=True, input_size=(227, 227)) if use_gpu == 1: model = nn.DataParallel(model).cuda() if path.exists(PATH): model.load_state_dict(torch.load(PATH)) model = model.to(device) print("\n Initialization complete") return model, saber, imgtopic
# Contains methods for the control and monitoring of various motors #from board import SCL, SDA #from adafruit_pca9685 import PCA9685 from pysabertooth import Sabertooth # from analogio import AnalogIn from enum import Enum import board import busio import time # import rotaryio # each Sabertooth controls 2 motors f_saber = Sabertooth('/dev/ttyS1', baudrate=9600, address=128, timeout=1000) b_saber = Sabertooth('/dev/ttyS1', baudrate=9600, address=129, timeout=1000) TD_saber = Sabertooth('/dev/ttyS1', baudrate=9600, address=130, timeout=1000) # Motor Numbers class Motor(Enum): FRONT_LEFT = 0 BACK_LEFT = 1 FRONT_RIGHT = 2 BACK_RIGHT = 3 @staticmethod def is_front(motor): return motor == Motor.FRONT_LEFT or motor == Motor.FRONT_RIGHT @staticmethod def is_back(motor): return motor == Motor.BACK_LEFT or motor == Motor.BACK_RIGHT
from pysabertooth import Sabertooth import pygame, time, sys, os motor = Sabertooth("/dev/serial0", baudrate=9600, address=128) os.environ["SDL_VIDEODRIVER"] = "dummy" #initialize dummy video screen os.putenv('DISPLAY', ':0.0') #set display, pygame requires this pygame.init() #initialize pygame pygame.joystick.init() #initialzie joystick pygame.display.init() #initialize display while True: pygame.event.pump() time.sleep(0.1)
print "\n Initialization complete" print "\nDetecting sabertooth....\n" pl = list(port.comports()) print pl address = '' for p in pl: print p if 'Sabertooth' in str(p): address = str(p).split(" ") print "\nAddress found @" print address[0] speed1 = 0 speed2 = 0 saber = Sabertooth(address[0], baudrate=9600, address=128, timeout=0.1) def translate(value, leftMin, leftMax, rightMin, rightMax): # Figure out how 'wide' each range is leftSpan = leftMax - leftMin rightSpan = rightMax - rightMin # Convert the left range into a 0-1 range (float) valueScaled = float(value - leftMin) / float(leftSpan) # Convert the 0-1 range into a value in the right range. return rightMin + (valueScaled * rightSpan) def callback(data): #print data
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 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()
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)
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()
class R2PY: def __init__(self): # if you want to switch to hardware PWM, remember: # GPIO12(Board 32) & GPIO18(Board 12) share a setting as do GPIO13(Board 33) & GPIO19(Board 35) # NOTE: all gpio pin numbers are BCM self.gpioPin_SabertoothS1 = 18 self.gpioPin_SabertoothS2 = 12 self.gpioPin_Syren10 = 13 # board pins 16 (BCM 23) and 18 (BCM 24) initialize to LOW at boot self.gpioPin_2_leg_mode = 23 self.gpioPin_3_leg_mode = 24 self.pi = pigpio.pi() self.pi.set_mode(self.gpioPin_2_leg_mode, pigpio.OUTPUT) self.pi.write(self.gpioPin_2_leg_mode, 0) self.pi.set_mode(self.gpioPin_3_leg_mode, pigpio.OUTPUT) self.pi.write(self.gpioPin_3_leg_mode, 0) self.pi.set_mode(self.gpioPin_Syren10, pigpio.OUTPUT) self.pi.set_PWM_frequency(self.gpioPin_Syren10, 50) self.pi.set_servo_pulsewidth(self.gpioPin_Syren10, 0) # to find the usb port of the sabertooth run this: cd /dev # and ls to see the list of usb ports, then plug in your usb to the sabertooth and ls again to see the new port self.saber = Sabertooth('/dev/ttyACM0', baudrate=115200, address=128, timeout=0.1) self.initializeXboxController() self.initializeSoundController() self.initializePeekabooController() self.running = True def initializePeekabooController(self): self.peekabooCtrlr = PeekabooController() self.peekabooCtrlr.start() def initializeSoundController(self): self.soundCtrlr = SoundController() self.soundCtrlr.start() def initializeXboxController(self): try: # setup controller values self.xValueLeft = 0 self.yValueLeft = 0 self.xValueRight = 0 self.yValueRight = 0 self.dpadValue = (0, 0) self.lbValue = 0 self.xboxCtrlr = XboxController(deadzone=0.3, scale=1, invertYAxis=True) # setup call backs self.xboxCtrlr.setupControlCallback( self.xboxCtrlr.XboxControls.LTHUMBX, self.leftThumbX) self.xboxCtrlr.setupControlCallback( self.xboxCtrlr.XboxControls.LTHUMBY, self.leftThumbY) self.xboxCtrlr.setupControlCallback( self.xboxCtrlr.XboxControls.RTHUMBX, self.rightThumbX) self.xboxCtrlr.setupControlCallback( self.xboxCtrlr.XboxControls.RTHUMBY, self.rightThumbY) # self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.LTRIGGER, self.leftTrigger) #triggers don't work # self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.RTRIGGER, self.rightTrigger) #triggers don't work self.xboxCtrlr.setupControlCallback( self.xboxCtrlr.XboxControls.DPAD, self.dpadButton) self.xboxCtrlr.setupControlCallback( self.xboxCtrlr.XboxControls.BACK, self.backButton) self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.A, self.aButton) self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.B, self.bButton) self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.X, self.xButton) self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.Y, self.yButton) self.xboxCtrlr.setupControlCallback(self.xboxCtrlr.XboxControls.LB, self.lbButton) # start the controller self.xboxCtrlr.start() except: print("ERROR: could not connect to Xbox controller") def steering(self, x, y): # assumes the initial (x,y) coordinates are in the -1.0/+1.0 range print("x = {}".format(x)) print("y = {}".format(y)) # convert to polar r = math.hypot(x, y) t = math.atan2(y, x) # rotate by 45 degrees t += math.pi / 4 # back to cartesian left = r * math.cos(t) right = r * math.sin(t) # rescale the new coords left = left * math.sqrt(2) right = right * math.sqrt(2) # clamp to -1/+1 left = max(-1, min(left, 1)) right = max(-1, min(right, 1)) print("left = {}".format(left)) print("right = {}".format(right)) # rotate 90 degrees counterclockwise back returnLeft = right * -1 returnRight = left print("returnLeft = {}".format(returnLeft)) print("returnRight = {}".format(returnRight)) return returnLeft, returnRight def translate(self, value, leftMin, leftMax, rightMin, rightMax): # figure out how 'wide' each range is leftSpan = leftMax - leftMin rightSpan = rightMax - rightMin # convert the left range into a 0-1 range (float) valueScaled = float(value - leftMin) / float(leftSpan) # convert the 0-1 range into a value in the right range. return rightMin + (valueScaled * rightSpan) # call back functions for left thumb stick def leftThumbX(self, xValue): self.xValueLeft = xValue self.updateFeet() def xValueLeftValue(self): return self.xValueLeft def leftThumbY(self, yValue): self.yValueLeft = yValue self.updateFeet() # call back functions for right thumb stick def rightThumbX(self, xValue): self.xValueRight = xValue self.updateDome() def xValueRightValue(self): return self.xValueRight def rightThumbY(self, yValue): self.yValueRight = yValue self.updateDome() def dpadButton(self, value): print("dpadButton = {}".format(value)) self.dpadValue = value self.transitionLegs() def lbButton(self, value): print("lbButton = {}".format(value)) self.lbValue = value def backButton(self, value): print("backButton = {}".format(value)) self.stop() def aButton(self, value): print("aButton = {}".format(value)) if ((value == 1) & (self.lbValue == 1)): PeekabooController.toggleRecord() if value == 1: self.annoyed() def xButton(self, value): print("xButton = {}".format(value)) if value == 1: self.worried() if (self.lbValue == 1): self.peekabooCtrlr.resume() def bButton(self, value): print("bButton = {}".format(value)) if value == 1: self.whistle() if (self.lbValue == 1): self.peekabooCtrlr.stop() def yButton(self, value): print("yButton = {}".format(value)) if value == 1: self.scream() # behavioral functions def annoyed(self): print("sound annoyed") SoundController.annoyed(self.soundCtrlr) def worried(self): print("sound worried") SoundController.worried(self.soundCtrlr) def whistle(self): print("sound whistle") SoundController.whistle(self.soundCtrlr) def scream(self): print("sound scream") SoundController.scream(self.soundCtrlr) def transitionLegs(self): # up if ((self.dpadValue == (0, -1)) & (self.lbValue == 1)): print("3 legged mode started") self.pi.write(self.gpioPin_3_leg_mode, 1) sleep(0.1) self.pi.write(self.gpioPin_3_leg_mode, 0) # down elif ((self.dpadValue == (0, 1)) & (self.lbValue == 1)): print("2 legged mode started") self.pi.write(self.gpioPin_2_leg_mode, 1) sleep(0.1) self.pi.write(self.gpioPin_2_leg_mode, 0) def updateDome(self): # debug print("xValueRight {}".format(self.xValueRight)) print("yValueRight {}".format(self.yValueRight)) # x,y values coming from XboxController are rotated 90 degrees, # so rotate 90 degrees counterclockwise back (x,y) = (-y, x) x1 = self.yValueRight * -1 y1 = self.xValueRight # debug print("x1 {}".format(x1)) print("y1 {}".format(x1)) # i.e. if i push left, motor should be spinning left # if i push right, motor should be spinning right # assuming RC, then you need to generate pulses about 50 times per second # where the actual width of the pulse controls the speed of the motors, # with a pulse width of about 1500 is stopped # and somewhere around 1000 is full reverse and 2000 is full forward. dutyCycleSyren10 = self.translate(x1, -1, 1, 1000, 2000) # debug print("---------------------") print("dutyCycleSyren10 {}".format(dutyCycleSyren10)) print("---------------------") self.pi.set_servo_pulsewidth(self.gpioPin_Syren10, dutyCycleSyren10) def updateFeet(self): # debug print("xValueLeft {}".format(self.xValueLeft)) print("yValueLeft {}".format(self.yValueLeft)) # i.e. if i push left, left motor should be spinning backwards, right motor forwards # if i push right, left motor should be spinning forwards, right motor backwards left, right = self.steering(self.xValueLeft, self.yValueLeft) dutyCycleS1 = self.translate(left, -1, 1, -100, 100) dutyCycleS2 = self.translate(right, -1, 1, -100, 100) # debug print("---------------------") print("dutyCycleS1 {}".format(dutyCycleS1)) print("dutyCycleS2 {}".format(dutyCycleS2)) print("---------------------") # drive(number, speed) # number: 1-2 # speed: -100 - 100 self.saber.drive(1, dutyCycleS1) self.saber.drive(2, dutyCycleS2) def stop(self): self.pi.set_servo_pulsewidth(self.gpioPin_Syren10, 0) saber.stop() self.xboxCtrlr.stop() self.peekabooCtrlr.stop() self.peekabooCtrlr.stopVideo() self.running = False
#!/usr/bin/env python from pysabertooth import Sabertooth from std_msgs.msg import String from geometry_msgs.msg import Twist, Pose import rospy, time saber = Sabertooth('/dev/ttyACM0', baudrate=38400, address=128, timeout=0.1) def callback(data): temperature = ('T [C]: {}'.format(saber.textGet('m2:gett'))) battery = ('battery [mV]: {}'.format(saber.textGet('m2:getb'))) current = ('current [mA]: {}'.format(saber.textGet('m2:getc'))) print temperature print battery print current def listener(): pub = rospy.Publisher('/Junkbot/info', String, queue_size=10) rospy.init_node('talker', anonymous=True) rospy.Subscriber("/junkbot/cmd_vel", Twist, callback) rospy.spin() if __name__ == '__main__':
noMovementFlag = 0 # Flag to track if no movement is desired by press of freeWheel button, toggled lastEvent = time.time( ) # Time of last event, just in case no input - stop after 2 seconds of no input noEventTimeout = 1 # seconds until timeout with no event, set back to 0 speed. # Power settings voltageIn = 24.0 # Total battery voltage to the Sabertooth voltageOut = 24.0 * 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power # Setup the power limits if voltageOut > voltageIn: maxPower = 100 else: maxPower = voltageOut / float(voltageIn) # Setup pygame and wait for the joystick to become available saber = Sabertooth(port, baudrate=9600) 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') print('Pygame os display settings') os.environ[ "SDL_VIDEODRIVER"] = "dummy" # Removes the need to have a GUI window os.putenv('SDL_VIDEODRIVER', 'fbcon') print('Pygame init') pygame.init() print('Pygame display init') pygame.display.init() print('Pygame display set')
from pysabertooth import Sabertooth saber = Sabertooth('/dev/ttyACM0', 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()
# Program: basic.py # Descrption: tests serial port communication, # basic forward/reverse commands to sabertooth. # from pysabertooth import Sabertooth import time saber = Sabertooth("/dev/serial0", baudrate=9600, address=128, timeout=0.1) def forward(speed): saber.driveBoth(speed, speed) def reverse(speed): saber.driveBoth(speed, speed) # mixed mode for turning saber.drive(1, 10) # forward saber.drive(2, 10) saber.stop() time.sleep(15) saber.drive(1, -10) # reverse saber.drive(2, -10) time.sleep(5) saber.driveBoth(0, 5) # slight right turn
# Program: elavator.py # Description: Implements elevator motion i.e. get in position # and enter elevator, get in position and exit. # from pysabertooth import Sabertooth import time saber = Sabertooth("/dev/", baudrate=9600, address=128, timeout=0.1) def getInPosition(): "Gets robot in position" def enterElevator(): "Enters elevator" def getOutPosition(): "Robot gets in position to exit elevator" def exitElevator(): "Exits elevator"
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()
# Contains methods for the control and monitoring of various motors from board import SCL, SDA from adafruit_pca9685 import PCA9685 from pysabertooth import Sabertooth import busio # i2c_bus = busio.I2C(SCL, SDA) pca = PCA9685(i2c_bus, address=0x40) pca.frequency = 1600 # motor0 = Sabertooth(pca.channels[0], 0.1) motor1 = Sabertooth(pca.channels[1], 0.1) motor2 = Sabertooth(pca.channels[2], 0.1) motor3 = Sabertooth(pca.channels[3], 0.1) motor_speeds = [0, 0, 0, 0] chassis_speed = 0 class motors: @staticmethod def set_motor_speed(motor, percent): if (motor == 0): motor0.drive(1, percent) elif (motor == 1): motor1.drive(1, percent) elif (motor == 2): motor2.drive(1, percent) elif (motor == 3):
from pysabertooth import Sabertooth saber = Sabertooth('/dev/serial0', baudrate=115200, address=125, timeout=0.1) saber.drive(1, 50) saber.drive(2, -75)
import socket import time from pysabertooth import Sabertooth from models.robot import Robot from models.camera import Camera from multiprocessing import Process PORT = "/dev/serial/by-id/usb-FTDI_FT232R_USB_UART_A105QI4I-if00-port0" saber = Sabertooth(PORT, baudrate=9600, address=128, timeout=0.1) robot = Robot(saber) camera = Camera('test_1', "./data/") client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) client_socket.connect(("192.168.0.9", 8000)) button_delay = 0.2 try: while True: from_server = client_socket.recv(4096) char = from_server.decode("utf-8") if char == "p": print("Stoping robot") robot.stop() time.sleep(1) camera.done() exit(0) if char == "c": robot.stop() time.sleep(button_delay)