def TBCheck(image):
TB1 = ThunderBorg.ThunderBorg()
TB1.i2cAddress = 10
TB1.Init()
image = mdlPil.TB(TB1.foundChip, image, 1)
TB2 = ThunderBorg.ThunderBorg()
TB2.i2cAddress = 11
TB2.Init()
image = mdlPil.TB(TB1.foundChip, image, 2)
return image
def TB_Startup():
# Setup the ThunderBorg
TB = ThunderBorg.ThunderBorg()
# TB.i2cAddress = 0x15 # Uncomment and change the value if you have changed the board address
TB.Init()
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
print 'No ThunderBorg found, check you are attached :)'
else:
print 'No ThunderBorg at address %02X, but we did find boards:' % TB.i2cAddress
for board in boards:
print ' %02X (%d)' % (board, board)
print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
print 'TB.i2cAddress = 0x%02X' % (boards[0])
sys.exit()
# Ensure the communications failsafe has been enabled!
failsafe = False
for i in range(5):
TB.SetCommsFailsafe(True)
failsafe = TB.GetCommsFailsafe()
if failsafe:
break
if not failsafe:
print 'Board %02X failed to report in failsafe mode!' % TB.i2cAddress
sys.exit()
return TB
def init(self):
# Init ThunderBorg
self.TB = ThunderBorg.ThunderBorg()
self.TB.Init()
if not self.TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
print('No ThunderBorg found, check you are attached :)')
else:
print(
'No ThunderBorg at address %02X, but we did find boards:' %
(TB.i2cAddress))
for board in boards:
print(' %02X (%d)' % (board, board))
print(
'If you need to change the IC address change the setup line so it is correct, e.g.'
)
print('TB.i2cAddress = 0x%02X' % (boards[0]))
sys.exit()
self.TB.SetCommsFailsafe(False) # Disable the communications failsafe
self.TB.SetLedShowBattery(False)
# Init action values
self.Forward = False
self.Backward = False
self.Left = False
self.Right = False
# Init the Camera
self.camera = cv2.VideoCapture(0)
def __init__(self, address=ThunderBorg.I2C_ID_THUNDERBORG):
self._tb = ThunderBorg.ThunderBorg()
self._tb.i2cAddress = address
self._tb.Init()
self.m0 = ThunderBorgMotorChannel(self._tb, 0)
self.m1 = ThunderBorgMotorChannel(self._tb, 1)
self.led = ThunderBorgLED(self._tb)
def __init__(self, address=ThunderBorg.I2C_ID_THUNDERBORG):
self._tb = ThunderBorg.ThunderBorg()
self._tb.i2cAddress = address
self._tb.Init()
if not self._tb.foundChip:
raise Exception("No ThunderBorg at {}".format(address))
self.m0 = ThunderBorgMotorChannel(self._tb, 0)
self.m1 = ThunderBorgMotorChannel(self._tb, 1)
self.led = ThunderBorgLED(self._tb)
def __init__(self):
self.TB = ThunderBorg.ThunderBorg()
self.TB.i2cAddress = 10
self.motorControlEnabled = False
self.TB.Init()
if not self.TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
print 'no ThunderBorg found. check you are attached..'
else:
print 'no ThunderBorg at address %02X. but we did find boards:' % (
TB.i2cAddress)
for board in boards:
print ' %02X (%d)' % (board, board)
print 'if you need to change the I2C address change the setup line so it is correct, e.g.'
print 'TB.i2cAddress = 0x%02X' % (boards[0])
raise Exception('init motor control failed')
self.enable()
def __init__(self, i2cAddress, maxPower, holdingPower, stepDelay): # Order for stepping when moving self.sequence = [ [+maxPower, +maxPower], [+maxPower, -maxPower], [-maxPower, -maxPower], [-maxPower, +maxPower]] # Order for stepping when holding self.sequenceHold = [ [+holdingPower, +holdingPower], [+holdingPower, -holdingPower], [-holdingPower, -holdingPower], [-holdingPower, +holdingPower]] # Init motor self.motor = ThunderBorg.ThunderBorg() self.motor.i2cAddress = i2cAddress self.motor.Init() # Exit if motor initialization failed if not self.motor.foundChip: boards = ThunderBorg.ScanForThunderBorg() if len(boards) == 0: print 'No ThunderBorg found, check you are attached :)' else: print 'No ThunderBorg at address %02X, but we did find boards:' % (self.motor.i2cAddress) for board in boards: print ' %02X (%d)' % (board, board) print 'If you need to change the IC address change the setup line so it is correct, e.g.' print 'TB.i2cAddress = 0x%02X' % (boards[0]) sys.exit(-1) # Init rest variables self.degPerStep = 1.8 self.stepDelay = stepDelay self.step = -1
# Movement settings (worked out from our MonsterBorg on carpet tiles)
timeForward1m = 0.85 # Number of seconds needed to move about 1 meter
timeSpin360 = 1.2 # Number of seconds needed to make a full left / right spin
testMode = False # True to run the motion tests, False to run the normal sequence
# Power settings
voltageIn = 12.0 # Total battery voltage to the ThunderBorg
voltageOut = 12.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 = 1.0
else:
maxPower = voltageOut / float(voltageIn)
# Connect to the Brain client
broker_url, broker_port = "192.168.43.210", 1883
client = mqtt.Client()
client.connect(broker_url, broker_port)
# Create ThunderBorg
TB = ThunderBorg.ThunderBorg()
TB.Init()
# Subscribe to request topic
client.subscribe("motor_request", qos=0)
client.message_callback_add("motor_request", parse_move_command)
client.loop_forever()
#!/usr/bin/env python
# coding: latin-1
# Import library functions we need
import ThunderBorg
import Tkinter
import sys
# Setup the ThunderBorg
global TB
TB = ThunderBorg.ThunderBorg() # Create a new ThunderBorg object
#TB.i2cAddress = 0x15 # Uncomment and change the value if you have changed the board address
TB.Init() # Set the board up (checks the board is connected)
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
print 'No ThunderBorg found, check you are attached :)'
else:
print 'No ThunderBorg at address %02X, but we did find boards:' % (
TB.i2cAddress)
for board in boards:
print ' %02X (%d)' % (board, board)
print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
print 'TB.i2cAddress = 0x%02X' % (boards[0])
sys.exit()
# Class representing the GUI dialog
class ThunderBorg_tk(Tkinter.Tk):
# Constructor (called when the object is first created)
def __init__(self, parent):
try:
tty.setraw(fd)
ch = sys.stdin.read(1)
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
return ch
return _getch
getch = _find_getch()
# Setup the library ready for use
import ThunderBorg # Load the library
TB = ThunderBorg.ThunderBorg() # Create a board object
TB.Init() # Setup the board
# Setting motor speeds
power = 0.5
rotatepower = 0.45
def forward():
TB.SetMotor1(power)
TB.SetMotor2(-power)
def backward():
TB.SetMotor1(-power)
control_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
control_socket.bind(('0.0.0.0', port))
print("waiting on port:", port)
#Number the RPI IO pins using BCM
GPIO.setmode(GPIO.BCM)
#Set the first pin as an output
GPIO.setup(18,GPIO.OUT)
#Create a PWM instance
sampler = GPIO.PWM(1,50)
#Need to set the board address of each Thunderborg separately to addresses e.g. 10 11
# Board #1, address 10, two left motors, Motor1: Front, Motor2: Rear
TB1 = ThunderBorg.ThunderBorg()
TB1.i2cAddress = 10
TB1.Init()
# Board #2, address 11, two right motors, Motor1: Front, Motor2: Rear
TB2 = ThunderBorg.ThunderBorg()
TB2.i2cAddress = 11
TB2.Init()
while True:
# Receive control strings over the socket from the PC.
data, addr = socket_control.recvfrom(1024)
# Print the messages
print("Message: ", data)
def CommandStream():
port = 8005
ip_address = "192.168.0.1" # IP Address of PC
control_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
control_socket.bind(('0.0.0.0', port))
print("waiting on port:", port)
# Number the RPI IO pins using BCM
GPIO.setmode(GPIO.BCM)
# Set the first pin as an output
GPIO.setup(18, GPIO.OUT)
# GPIO.setup(1,GPIO.OUT)
# Create a PWM instance
sampler = GPIO.PWM(18, 50)
sampler.start(0)
# claw = GPIO.PWM(,50) #enter pin number
# Need to set the board address of each Thunderborg separately to addresses e.g. 10 11
# Board #1, address 10, two left motors, Motor1: Front, Motor2: Rear
TB1 = ThunderBorg.ThunderBorg()
TB1.i2cAddress = 10
TB1.Init()
# Board #2, address 11, two right motors, Motor1: Front, Motor2: Rear
TB2 = ThunderBorg.ThunderBorg()
TB2.i2cAddress = 11
TB2.Init()
TB3 = ThunderBorg.ThunderBorg()
TB3.i2cAddress = 12
TB3.Init()
while True:
print(TB1.GetBatteryReading())
print(TB2.GetBatteryReading())
print(TB3.GetBatteryReading()) # prints battery voltage
# Receive control strings over the socket from the PC.
data, addr = control_socket.recvfrom(1024)
# Print the messages
print("Message: ", data)
# Use the data to control motors
# Extract throttle and steer_angle from data
throttle = int(data[0:3])
steer_angle = int(data[3:6])
sampler_position = int(data[6:9])
claw_position = int(data[9:12])
# scale throttle, steer angle, and sampler position to range [-1,1]
# old_sampler_position = sampler_position
scaled_sampler_position = (
90 -
sampler_position) / 90 # 1 is max clockise, -1 max anticlockwise
scaled_throttle = (throttle - 90) / 90
scaled_steer_angle = (steer_angle - 90) / 90
claw_position_scaled = (90 - claw_position) / 90
left_power = scaled_throttle + scaled_steer_angle
right_power = scaled_throttle - scaled_steer_angle
# Scale left_power and right_power so both stay in range [-1,1]
if (left_power >= 1) or (right_power <= -1):
left_power *= 1 / 2
right_power *= 1 / 2
TB1.SetMotors(left_power)
TB2.SetMotors(right_power)
# if claw_position == 1:
# claw.ChangeDutyCycle(10)
# else:
# claw.ChangeDutyCycle(5)
# Sampler Control Code
# Servo is neutral at 1.5ms, >1.5 Counter-Clockwise, <1.5 Clockwise
print(scaled_sampler_position)
TB3.SetMotors(scaled_sampler_position)
pulse_width = 1.5 - claw_position_scaled * 1 / 4
duty_cycle = pulse_width * 5 # this is the simplification of D = PW/T * 100 with f=1/T = 50Hz
sampler.ChangeDutyCycle(duty_cycle)
control_socket.close()
GPIO.cleanup() #Check if neccessary
def setupTB(address=0x15):
"""Function for setting up a thunderborg. Tries to setup a TB instance from the given I2C address,
informs if set up failed"""
# Setup a thunderborg instance
TB = ThunderBorg.ThunderBorg()
# Set the I2C address
TB.i2cAddress = address
# Initialize
TB.Init()
# Inform if no board at address
if not TB.foundChip:
boards = ThunderBorg.ScanForThunderBorg()
if len(boards) == 0:
print 'No ThunderBorg found, check you are attached'
else:
print 'No ThunderBorg at address %02X, but we did find boards:' % (
TB.i2cAddress)
for board in boards:
print ' %02X (%d)' % (board, board)
print 'If you need to change the I²C address change the setup line so it is correct, e.g.'
print 'TB.i2cAddress = 0x%02X' % (boards[0])
sys.exit()
# Ensure the communcations failsafe had been enabled
failsafe = False
for i in range(5):
TB.SetCommsFailsafe(True)
failsafe = TB.GetCommsFailsafe()
if failsafe:
break
if not failsafe:
print 'Board %02X failed to report in failsafe mode!' % (
ThunderBorg.i2cAddress)
sys.exit()
# # Power settings
# voltageIn = 1.2 * 10 # Total battery voltage to the ThunderBorg
# voltageOut = 12.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 = 1.0
# else:
# maxPower = voltageOut / float(voltageIn)
# # Show battery monitoring settings
# battMin, battMax = TB.GetBatteryMonitoringLimits()
# print 'Battery monitoring settings:'
# print ' Minimum (red) %02.2f V' % (battMin)
# print ' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2)
# print ' Maximum (green) %02.2f V' % (battMax)
# print
battCurrent = TB.GetBatteryReading()
print ' Current voltage %02.2f V' % (battCurrent)
print
# Turn off battery mode for leds
TB.SetLedShowBattery(False)
TB.SetLeds(0, 0, 1)
# Make sure the motors are off
TB.MotorsOff()
return TB