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)
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 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")
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()
from pysabertooth import Sabertooth
saber = Sabertooth('/dev/serial0', baudrate=115200, address=125, timeout=0.1)
saber.drive(1, 50)
saber.drive(2, -75)
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()
#
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
saber.driveBoth(5, 0) # slight left turn
saber.stop()
saber.close()
running = False
elif event.type == pygame.JOYBUTTONDOWN:
# A button on the joystick just got pushed down
hadEvent = True
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
hadEvent = True
if hadEvent:
#track time and set noRecentEventFlag to 0 to prevent stop
noRecentEventFlag = 0
lastEvent = time.time()
# break out if PS button is pressed
if joystick.get_button(buttonPSExit):
stopButtonPSExitFlag = 1
saber.drive(1, 0)
saber.drive(2, 0)
if joystick.get_button(buttonFreeWheel):
if noMovementFlag == 0:
noMovementFlag = 1
saber.text(b'm1:0')
saber.text(b'm2:0')
saber.text(b'q1:1')
saber.text(b'q2:1')
else:
noMovementFlag = 0
saber.text(b'm1:0')
saber.text(b'm2:0')
saber.text(b'q1:0')
saber.text(b'q2:0')
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
