Esempio n. 1
0
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
Esempio n. 2
0
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
Esempio n. 3
0
 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)
Esempio n. 4
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)
Esempio n. 5
0
    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)
Esempio n. 6
0
    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()
Esempio n. 7
0
	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
Esempio n. 8
0
# 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()
Esempio n. 9
0
#!/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):
Esempio n. 10
0
        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)
Esempio n. 11
0
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)
Esempio n. 12
0
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
Esempio n. 13
0
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