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)
