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)
