def __init__(self):
self.lcd = LS027B4DH01(spi=SPI(2,
baudrate=2_000_000,
polarity=0,
phase=0,
bits=8,
firstbit=SPI.LSB,
sck=Pin(18),
mosi=Pin(23),
miso=Pin(19)),
scs=Pin(32, Pin.OUT),
extcomin=Pin(33, Pin.OUT),
disp=Pin(25, Pin.OUT))
self.button = Joystick(x=Pin(34), y=Pin(35), s=Pin(26, Pin.IN))
self.mpu9250 = MPU9250(I2C(scl=Pin(21), sda=Pin(22), freq=100000))
self.mpu9250.setting(self.mpu9250.GFS_1000, self.mpu9250.AFS_16G)
self.states = {
'home': Ui_home(self),
'acceleration': Ui_acceleration(self),
'gyro': Ui_gyro(self),
'geomagnetism': Ui_geomagnetism(self)
}
self.state = 'home'
def __init__(self):
self.lcd = LS027B4DH01(
SPI(2,
baudrate=2_000_000,
polarity=0,
phase=0,
bits=8,
firstbit=SPI.LSB,
sck=Pin(18),
mosi=Pin(23),
miso=Pin(19)), Pin(32, Pin.OUT), Pin(33, Pin.OUT),
Pin(25, Pin.OUT))
self.button = Joystick(Pin(34), Pin(35), Pin(26, Pin.IN))
self.mpu9250 = MPU9250(I2C(scl=Pin(21), sda=Pin(22), freq=100000))
self.mpu9250.setting(self.mpu9250.GFS_1000, self.mpu9250.AFS_16G)
self.states = {
'home': Home(self),
'record': Record(self),
'settings': Settings(self)
}
self.state = 'home'
def __init__(self):
# create simulation (GUI)
self.urdfRootPath = pybullet_data.getDataPath()
p.connect(p.GUI)
p.setGravity(0, 0, -9.81)
# set up camera
self._set_camera()
# load some scene objects
p.loadURDF(os.path.join(self.urdfRootPath, "plane.urdf"),
basePosition=[0, 0, -0.65])
p.loadURDF(os.path.join(self.urdfRootPath, "table/table.urdf"),
basePosition=[0.5, 0, -0.65])
# example YCB object
obj1 = YCBObject('003_cracker_box')
obj1.load()
p.resetBasePositionAndOrientation(obj1.body_id, [0.7, -0.2, 0.1],
[0, 0, 0, 1])
# load a panda robot
self.panda = Panda()
# connect to joystick for control
self.joystick = Joystick(scale=0.01)
def get_joystick():
while True:
try:
joystick = Joystick()
break
except Exception:
print('Error happened while recognizing the joystick. Retrying in 1 second...')
Joystick.reinit()
sleep(1)
return joystick
def main(amybot=True, camjambot=False):
joystick = Joystick()
# if elsing this because the init methods of both classes do stuff with hardware, so best to only intiailise deliberately
if amybot:
bot = AmyBot()
else:
bot = CamJamBot()
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
interval = 0.0
# Power settings
voltage_in = 6.0 # Total battery voltage to the PicoBorg Reverse
voltage_out = 5.0 #* 0.95 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power
# Setup the power limits
if voltage_out > voltage_in:
max_power = 1.0
else:
max_power = voltage_out / float(voltage_in)
# Setup pygame and wait for the joystick to become available
try:
LOGGER.info('Press CTRL+C to quit')
running = True
# Loop indefinitely
while running:
# Get the latest events from the system
had_event = False
events = pygame.event.get()
# Handle each event individually
for event in events:
if event.type == pygame.QUIT:
# User exit
running = False
break
elif event.type == pygame.JOYBUTTONDOWN:
# A button on the joystick just got pushed down
had_event = True
elif event.type == pygame.JOYAXISMOTION:
# A joystick has been moved
had_event = True
if had_event:
# Read axis positions (-1 to +1) + determine how much to move by
left_drive, right_drive = joystick.get_reading()
bot.move(left_drive, right_drive)
time.sleep(interval)
except KeyboardInterrupt:
# CTRL+C exit, disable all drives
#LeftMotor.speed(0)
#RightMotor.speed(0)
LOGGER.debug('Motors off')
def __init__(self) -> None:
self._joystick = Joystick(FORWARD_PIN, REVERSE_PIN, LEFT_PIN,
RIGHT_PIN)
self._left_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._right_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._left_motor_controler = MotorControler(LEFT_MOTOR_PWM,
LEFT_MOTOR_A, LEFT_MOTOR_B)
self._right_motor_controler = MotorControler(RIGHT_MOTOR_PWM,
RIGHT_MOTOR_A,
RIGHT_MOTOR_B)
def __init__(self,
joystick_port=0,
serial_port='/dev/ttyACM0',
publish_rate=10.0):
self.joystick = Joystick(joystick_port)
self.serial_port = serial_port
self.publish_rate = publish_rate
self.xbee = Packet()
self.arm_car = False
self.last_data = None
self.arm_display_count = 0
self.safety_check_done = False
def main():
# global exit_flag
js_lock = threading.Lock()
arm_lock = threading.Lock()
js = Joystick(1, "Thread - Joystick", js_lock)
arm = None
screen = Screen()
hotas = HOTAS_Arm(1, "Thread - HOTAS", js, arm, js_lock, arm_lock, screen)
js.start()
hotas.start()
screen.start()
screen.join()
js.stop()
print("Joystick asked to stop...")
hotas.stop()
print("Program asked to stop...")
hotas.join()
print("Main Ended")
print("Please press a key on the joystick to end the program...")
js.join()
print("Program ended...")
def __init__(self):
#channel numbers
self.errorLightOut = 4
self.errorLightLow = 18
self.switchOut = 13
self.switchDetect = 20
#constants
self.rs_stopped = 0
self.rs_forward = 1
self.rs_reverse = 2
self.ss_nothing = 0 #nothing has happened yet
self.ss_start = 1 #both eyes open complete
self.ss_forward1 = 2 #one eye open complete
self.ss_forward2 = 3 #both eyes closed complete
self.ss_reverse1 = 4 #both eyes closed complete
self.redFac = .75
self.addFac = 1 - self.redFac
self.maxSwitchTime = 7 #leave room for waiting and messing up
self.maxReverseTime = 5
self.defaultPrevX = 0
self.defaultPrevY = -.5
#vars
self.runState = self.rs_stopped
self.runTime = 0
self.switchState = self.ss_nothing
self.switchTime = 0
self.prevX = self.defaultPrevX
self.prevY = self.defaultPrevY
self.joystick = Joystick()
io.setmode(io.BCM)
io.setup(self.errorLightOut, io.OUT, initial=io.LOW)
io.setup(self.errorLightLow, io.OUT, initial=io.LOW)
io.setup(self.switchOut, io.OUT, initial=io.HIGH)
io.setup(self.switchDetect, io.IN, pull_up_down=io.PUD_DOWN)
time.sleep(.2) #wait for io to definitely turn on before attempting to sense
if self.should_shut_down():
print("waiting for switch on")
io.wait_for_edge(self.switchDetect, io.RISING) #wait for switch to turn on.
self.set_led(io.HIGH) #indicate success
time.sleep(.3) #wait so people know for sure that its working
def __init__(self, master=None):
super().__init__(master)
self.joystick = Joystick()
self.master = master
self.grid()
self.acelerador = tk.StringVar()
self.re = tk.StringVar()
self.esquerda = tk.StringVar()
self.direita = tk.StringVar()
self.cam_v = tk.StringVar()
self.cam_h = tk.StringVar()
self.criar_componentes()
self.txt_acelerador.focus_set()
self.atual = self.acelerador
def __init__(self, device: Device) -> None:
self.device = device
self.type = device.type
self.binds: Dict[str, Callable] = dict()
if device.type == "keyboard":
self.keyboard = Keyboard()
for action, how in device.actions.items():
if type(how) is str:
self.keyboard.bind(Input.str_to_key(how), action)
elif type(how) is list and "repeat" in how:
self.keyboard.bind(Input.str_to_key(how[0]), action, True)
else:
raise Exception("invalid binding: {}".format(how))
elif device.type == "joystick":
self.joystick = Joystick(device.joystick)
for action, how in device.actions.items():
invalid = True
if type(how) is int:
self.joystick.bind_button(how, action)
invalid = False
elif type(how) is list:
if (
len(how) >= 3
and type(how[0]) is str
and how[0] == "axis"
):
repeat = "repeat" in how
axis = int(how[1])
direction = 0
if how[2] in ("+", "-"):
if how[2] == "-":
direction = -1
elif how[2] == "+":
direction = 1
invalid = False
self.joystick.bind_axis(
axis, direction, action, repeat
)
if invalid:
raise Exception("invalid binding: {}".format(how))
def run(self):
self.joy = Joystick()
self.joy.init(self.devicePath)
while(1):
#time.sleep(1)
print "Updating Joystick"
self.joy.update()
def setup():
global joystick, mouse_control, scroll_control, distance_sensor
GPIO.setmode(GPIO.BOARD)
joystick = Joystick(7, 0, 1)
mouse_control = Potentiometer(2)
scroll_control = Potentiometer(3)
distance_sensor = DistanceSensor(35, 37)
class JoystickUpdate(threading.Thread):
'''
classdocs
'''
def setDevice(self, devicePath):
self.devicePath = devicePath
def getJoystick(self):
return self.joy
def run(self):
self.joy = Joystick()
self.joy.init(self.devicePath)
while(1):
#time.sleep(1)
print "Updating Joystick"
self.joy.update()
def init_ui(self):
QtGui.QMainWindow.__init__(self, None)
###Joystick Interpret StoryTelling and Nao_Manager ####
self.joystick = Joystick(self)
self.interpret1 = Interpret(1)
self.interpret2 = Interpret(2)
self.story = StoryTelling()
self.manager = Nao_manager()
#Timer, updating nao battery level, and connection
self.timerNao = QtCore.QTimer(self)
self.connect(self.timerNao, QtCore.SIGNAL("timeout()"), self.manager.nao_getStatus)
self.timerNao.start(3000)
self.timerStory = QtCore.QTimer(self)
self.connect(self.timerStory, QtCore.SIGNAL("timeout()"), self.story.update_clock)
self.timerStory.start(200)
########### Connect ######
self.joystick.joy_event1.connect(self.interpret1.translate)
self.joystick.view_event1.connect(self.interpret1.changeView)
self.interpret1.interpret_event.connect(self.story.transmit_msg)
self.joystick.joy_event2.connect(self.interpret2.translate)
self.joystick.view_event2.connect(self.interpret2.changeView)
self.interpret2.interpret_event.connect(self.story.transmit_msg)
self.story.storytelling_event.connect(self.manager.transmit_msg)
self.joystick.second_joy_detected.connect(self.add_joystick)
#### GUI######
self.setWindowTitle("Multiple Nao xbox controller")
self.layout_main = QtGui.QGridLayout()
self.group_ManagerStory = QtGui.QGroupBox("Nao Control Joystick")
self.layout_ManagerStory = QtGui.QVBoxLayout()
self.layout_ManagerStory.addWidget(self.manager)
self.layout_ManagerStory.addWidget(self.story)
self.group_ManagerStory.setLayout(self.layout_ManagerStory)
#set layout QGridBox
self.layout_main.addWidget(self.group_ManagerStory, 0, 1)
self.layout_main.addWidget(self.interpret1, 0,0)
self.setLayout(self.layout_main)
self.show()
#### Nao Manager #### LUCAS PUIS MAMA PUIS LUCY
#self.manager.addNao("Timide", "10.0.1.20", 9559, 1 )
self.manager.addNao("Lucas", "10.0.1.11", 9559, 2 )
#self.manager.addNao("Mama", "10.0.1.12", 9559 , 2)
#self.manager.addNao("Lucy", "10.0.1.13", 9559 , 2)
self.manager.init_manager()
def __init__(self,
amybot=True,
cambot=False,
fourtronix=False,
straight=False):
Sonar.__init__(self)
# Rainbow.__init__(self)
print("Hello!")
self.last_text = "Bleurgh!"
self.joystick = Joystick()
# if elsing this because the init methods of both classes do stuff with hardware, so best to only intiailise deliberately
if amybot:
self.bot = AmyBot()
LOGGER.info('Enable AmyBot')
elif cambot:
self.bot = CamJamBot()
LOGGER.info('Enable CamJamBot')
elif fourtronix:
self.bot = FourTronix()
LOGGER.info('Enable FourTronixBot')
else:
print("Unknown Robot Type")
sys.exit(0)
self.straight_line_start = False
if straight:
self.mode = R2_BUTTON
self.straight_line_start = True
# Re-direct our output to standard error, we need to ignore standard out to hide some nasty print statements from pygame
sys.stdout = sys.stderr
interval = 0.0
self.modes = {
# L1_BUTTON: self.rainbow,
R1_BUTTON: self.remote,
L2_BUTTON: self.maze,
R2_BUTTON: self.straight
}
super().__init__()
def enableJoystick(self):
self.joystick_enabled = True
try:
self.joystick = Joystick(1)
except:
self.joystick_enabled = False
self.rb_js_enable.set_active(True)
self.rb_js_disable.set_active(False)
self.statusbar.push(self.context_id, 'Joystick device error')
return
self.js_handler = self.joystick.connect('axis', self.axis_event)
self.factory.control.send_select_command(True)
def __init__(self, parent=None):
super(MainDialog, self).__init__(parent)
self.setupUi(self)
# set up the heartbeat callback
self.heartBeat = HeartBeat()
self.heartBeat.heartBeat.connect(self.ServiceHeartBeat)
self.heartBeat.start() # start the heartbeat thread!
# set up the joystick thread
self.lockJoystick = QtCore.QMutex()
self.joystick = Joystick(self.lockJoystick)
self.joystick.start()
# This is the embedcreativity API
self.API = PalmettoAPI()
self.status = -1
self.voltage = -1
self.current = -1
# Status variables to keep track of what we've sent the board
self.motorPowerOn = False
self.slowMo = True
self.ledDecButtonDown = False
self.ledIncButtonDown = False
self.pwrLED = ledDefault
# Security Cam Mode variables
self.hatXDown = False
self.hatYDown = False
self.securityCamMode = False
self.securityCamLeftPos = 650
self.securityCamRightPos = 690
self.securityCamDirectionIncreasing = True
self.securityCamPositionLast = 670
self.securityCamIncAmount = 2
self.securityCamIncAmountMin = 1
self.securityCamIncAmountMax = 10
self.chargerDockOpen = False
self.chargerDockOpenPos = 550
self.chargerDockClosedPos = 1200
def enableJoystick(self):
logging.debug('enableJoystick')
self.joystick_enabled = True
try:
self.joystick = Joystick(self.jsdevnum)
except:
self.joystick_enabled = False
#self.view['rb_js_enable'].set_active(False)
#self.view['rb_js_disable'].set_active(True)
logging.error('could not enable joystick')
return
self.jsAxisHandler = self.joystick.connect('axis', self.on_axis_event)
self.jsButtonHandler = self.joystick.connect('button', self.on_button_event)
class RemoteControl:
deadzone = 4000
def __init__(self):
self.udp_client = UDPClient()
self.joystick = Joystick()
self.auto = False
@classmethod
def adapt(cls, value):
if value > cls.deadzone:
return (value - cls.deadzone) * 100.0 / (32768 - cls.deadzone)
elif value < -cls.deadzone:
return (value + cls.deadzone) * 100.0 / (32768 - cls.deadzone)
else:
return 0.0
def update(self):
self.joystick.update()
left_drive = self.adapt(self.joystick.axis.get(1, 0))
right_drive = self.adapt(self.joystick.axis.get(4, 0))
self.auto = (self.auto or self.joystick.button.get(0, False)) and not self.joystick.button.get(1, False)
self.udp_client.write("%.2f,%.2f,%d" % (left_drive, right_drive, self.auto))
def main():
lm = LedMatrix()
pygame.init()
pygame.joystick.init()
joy = Joystick(0)
gameMain = LifeGameMain(lm, joy)
try:
gameMain.start()
except KeyboardInterrupt:
pass
except:
print(traceback.format_exc())
if lm != None:
lm.term()
def update_from_joystick(rov_movement):
print("Thrade oluşturuldu")
# Joystick variables are creating
joystick_values = {}
Joy_obj = Joystick()
joystick_values.update(Joy_obj.shared_obj.ret_dict)
# Joystick variables are created
global arayuz_running
while arayuz_running:
Joy_obj.while_initializer()
if Joy_obj.joystick_count:
Joy_obj.for_initializer()
Joy_obj.joysticks()
joystick_values = Joy_obj.shared_obj.ret_dict
if rov_movement:
# Robotik kol hareketi
arm_status = joystick_values["robotik_kol"]
arm_status = True if arm_status == 1 else (
False if arm_status == -1 else None)
rov_movement.toggle_arm(arm_status)
# XYZ hareketi
pf = joystick_values["power_factor"]
zpf = joystick_values["zpf"]
xy_power = joystick_values["xy_plane"]["magnitude"] * pf * 100
z_power = joystick_values["z_axes"] * pf * zpf * 60
turn_power = joystick_values["turn_itself"] * pf * 100
tilt_degree = joystick_values["tilt_degree"]
if tilt_degree:
rov_movement.go_xyz_with_tilt(xy_power,
z_power,
turn_power,
tilt_degree=tilt_degree)
else:
xy_angle = joystick_values["xy_plane"]["angel"]
rov_movement.go_xy_and_turn(xy_power, xy_angle, turn_power)
rov_movement.go_z_bidirectional(z_power)
sleep(0.04)
Joy_obj.clock.tick(50)
def enable_or_disable_cv_adjust_1(self):
toggle_char = '0'
if self.joystick_str != '':
values = Joystick.get_deserialized_info(self.joystick_str)
if self.CV_ADJUST1_DISABLE_ENABLE_BUTTON_INDEX < len(values.buttons):
toggle_char = values.buttons[self.CV_ADJUST1_DISABLE_ENABLE_BUTTON_INDEX]
else:
print('Button ' + str(self.CV_ADJUST1_DISABLE_ENABLE_BUTTON_INDEX) + '( used for toggling the '
'camera doesn\'t exits in '
'this joystick.')
if toggle_char == '1' and self.joystick_str_is_new:
if self.cv_adjust1 is None:
self.initialize_cv_adjust_1()
else:
self.cv_adjust1 = None
class Powertrain:
def __init__(self) -> None:
self._joystick = Joystick(FORWARD_PIN, REVERSE_PIN, LEFT_PIN,
RIGHT_PIN)
self._left_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._right_ramp = Ramp(RAMP_SIZE, RAMP_BREAK_FACTOR)
self._left_motor_controler = MotorControler(LEFT_MOTOR_PWM,
LEFT_MOTOR_A, LEFT_MOTOR_B)
self._right_motor_controler = MotorControler(RIGHT_MOTOR_PWM,
RIGHT_MOTOR_A,
RIGHT_MOTOR_B)
def loop(self):
start_time = time.ticks_ms()
while True:
try:
# delta = time.ticks_diff(time.ticks_ms(), start_time)
# print("delta", delta)
delta = 10
left_direction, right_direction = self._joystick.get()
print("direction", left_direction, right_direction)
self._left_ramp.update(left_direction, delta)
self._right_ramp.update(right_direction, delta)
left_consign = self._left_ramp.get_normalized(750)
self._left_motor_controler.set(left_consign)
right_consign = self._right_ramp.get_normalized(750)
self._right_motor_controler.set(right_consign)
print("")
time.sleep_ms(10)
except Exception as e:
print(e)
start_time = time.ticks_ms()
#from writer import UDP_Writer
from joystick import Joystick
import time
#writer = UDP_Writer("127.0.0.1")
joystick = Joystick(0)
m = [0, 0, 0]
while True:
# get each axis strength
s0, s1, s2, s3, brake = joystick.get_sticks()
print(s0)
print(s3)
# convert to thrust inputs
# vertical
m[0] = s0
# left
m[1] = s3
m[2] = brake
msg = ""
msg = msg + str(m[0]) + ","
msg = msg + (str(m[1])) + ","
msg = msg + (str(m[2]))
# writer.send(msg)
time.sleep(0.01)
class MainDialog(QDialog, QtUI.Ui_Dialog):
def __init__(self, parent=None):
super(MainDialog, self).__init__(parent)
self.setupUi(self)
# set up the heartbeat callback
self.heartBeat = HeartBeat()
self.heartBeat.heartBeat.connect(self.ServiceHeartBeat)
self.heartBeat.start() # start the heartbeat thread!
# set up the joystick thread
self.lockJoystick = QtCore.QMutex()
self.joystick = Joystick(self.lockJoystick)
self.joystick.start()
# This is the embedcreativity API
self.API = PalmettoAPI()
self.status = -1
self.voltage = -1
self.current = -1
# Status variables to keep track of what we've sent the board
self.motorPowerOn = False
self.slowMo = True
self.ledDecButtonDown = False
self.ledIncButtonDown = False
self.pwrLED = ledDefault
# Security Cam Mode variables
self.hatXDown = False
self.hatYDown = False
self.securityCamMode = False
self.securityCamLeftPos = 650
self.securityCamRightPos = 690
self.securityCamDirectionIncreasing = True
self.securityCamPositionLast = 670
self.securityCamIncAmount = 2
self.securityCamIncAmountMin = 1
self.securityCamIncAmountMax = 10
self.chargerDockOpen = False
self.chargerDockOpenPos = 550
self.chargerDockClosedPos = 1200
def ServiceHeartBeat(self, value):
# Get max speed for motors
if self.slowMo:
maxRate = 1000.0 * SlowMoRate
else:
maxRate = 1000.0
# Mix motor settings
Ymotors = int(maxRate * float(self.joystick.absY[0]) /
float(self.joystick.absY[2]))
Xmotors = int(maxRate * float(self.joystick.absX[0]) /
float(self.joystick.absX[2]))
# First add Ymotors to both sides equally
Rmotors = Ymotors
Lmotors = Ymotors
# Then split XMotors across each side (add to one side, subtract from other)
Rmotors += Xmotors
Lmotors -= Xmotors
# ceiling/floor limits
if Rmotors > maxRate:
Rmotors = maxRate
elif Rmotors < (-1 * maxRate):
Rmotors = (-1 * maxRate)
if Lmotors > maxRate:
Lmotors = maxRate
elif Lmotors < (-1 * maxRate):
Lmotors = (-1 * maxRate)
# Set Right Motors
self.Send('setmotor 1 {}'.format(-1 * Rmotors))
self.Send('setmotor 2 {}'.format(-1 * Rmotors))
# Set Left Motors
self.Send('setmotor 3 {}'.format(Lmotors))
self.Send('setmotor 4 {}'.format(-1 * Lmotors))
# Get Pan Servo setting
if self.securityCamMode: # automated panning
if self.securityCamDirectionIncreasing:
if self.securityCamPositionLast < self.securityCamRightPos:
self.securityCamPositionLast += self.securityCamIncAmount
else:
self.securityCamDirectionIncreasing = False # flip directions
self.securityCamPositionLast -= self.securityCamIncAmount
else:
if self.securityCamPositionLast > self.securityCamLeftPos:
self.securityCamPositionLast -= self.securityCamIncAmount
else:
self.securityCamDirectionIncreasing = True # flip directions
self.securityCamPositionLast += self.securityCamIncAmount
pan = self.securityCamPositionLast
else: # joystick controlled
pan = 670 + int(560.0 * float(self.joystick.absRx[0]) /
float(self.joystick.absRx[2]))
self.Send('setservo 1 {}'.format(pan))
# Get Tilt Servo setting
tilt = 750 + int(750.0 * float(self.joystick.absRy[0]) /
float(self.joystick.absRy[2]))
if tilt < 200: # don't allow it go too far up
tilt = 200
self.Send('setservo 2 {}'.format(tilt))
# Check Center Mode Button for Motor On/Off
if self.joystick.btnMode[
1]: # new button activity (either pressed or released)
self.joystick.btnMode[1] = False #reset flag
if 1 == self.joystick.btnMode[0]: # button pressed!
if self.motorPowerOn: # it's on, let's turn it off
self.motorPowerOn = False
self.lblMotorPower.setText('Motor Power Off')
self.Send('moff')
else: # it's off, let's turn it on
self.motorPowerOn = True
self.lblMotorPower.setText('Motor Power On')
self.Send('mon')
# Check Back Button for SlowMo
if self.joystick.btnA[
1]: # new button activity (either pressed or released)
self.joystick.btnA[1] = False # reset flag
if 1 == self.joystick.btnA[0]: # button pressed!
self.slowMo = False
else: # button released!
self.slowMo = True
# Toggle Security Cam Mode
if self.joystick.btnStart[
1]: # new button activity (either pressed or released)
self.joystick.btnStart[1] = False #reset flag
if 1 == self.joystick.btnStart[0]: # button pressed!
if self.securityCamMode: # it's on, let's turn it off
self.securityCamMode = False
self.lblSecurityCamMode.setText('Security Cam Mode Off')
else: # it's off, let's turn it on
self.securityCamMode = True
self.lblSecurityCamMode.setText('Security Cam Mode On')
# Toggle Charger Port
if self.joystick.btnSelect[
1]: # new button activity (either pressed or released)
self.joystick.btnSelect[1] = False #reset flag
if 1 == self.joystick.btnSelect[0]: # button pressed!
if self.chargerDockOpen: # it's open, let's close it
self.chargerDockOpen = False
self.lblChargerDoor.setText('Charger Door Closed')
self.Send('setservo 3 {}'.format(
self.chargerDockClosedPos))
else: # it's closed, let's open it
self.chargerDockOpen = True
self.lblChargerDoor.setText('Charger Door Open')
self.Send('setservo 3 {}'.format(self.chargerDockOpenPos))
# Right/Left Hat buttons
if self.joystick.absHatX[0] != 0 and self.hatXDown == False:
self.hatXDown = True
if self.joystick.absHatX[0] == self.joystick.absHatX[
1]: # left hat button pressed
self.securityCamLeftPos = pan # capture pan position
else: # right hat button pressed
self.securityCamRightPos = pan # capture pan position
elif self.joystick.absHatX[
0] == 0 and self.hatXDown == True: # button up - reset flag
self.hatXDown = False
# Up/Down Hat buttons
if self.joystick.absHatY[0] != 0 and self.hatYDown == False:
self.hatYDown = True
if self.joystick.absHatY[0] == self.joystick.absHatY[
1]: # up hat button pressed
if self.securityCamIncAmount < self.securityCamIncAmountMax:
self.securityCamIncAmount += 1
else: # down hat button pressed
if self.securityCamIncAmount > self.securityCamIncAmountMin:
self.securityCamIncAmount -= 1
elif self.joystick.absHatY[
0] == 0 and self.hatYDown == True: # button up - reset flag
self.hatYDown = False
# Check Button B for reset LED to 0
if self.joystick.btnB[
1]: # new button activity (either pressed or released)
self.joystick.btnB[1] = False # reset flag
if 1 == self.joystick.btnB[0]: # button pressed!
self.pwrLED = ledDefault
# Check Right Trigger for LED decrement
if self.joystick.btnTR[
1]: # new button activity (either pressed or released)
self.joystick.btnTR[1] = False # reset flag
if 1 == self.joystick.btnTR[0]: # button down
self.ledDecButtonDown = True
else:
self.ledDecButtonDown = False
# Check Left Trigger for LED increment
if self.joystick.btnTL[
1]: # new button activity (either pressed or released)
self.joystick.btnTL[1] = False # reset flag
if 1 == self.joystick.btnTL[0]: # button down
self.ledIncButtonDown = True
else:
self.ledIncButtonDown = False
# Check if buttons are still down and continually inc/dec while they are
if self.ledDecButtonDown:
if self.pwrLED > 0:
self.pwrLED -= 10
if self.ledIncButtonDown:
if self.pwrLED < 1000:
self.pwrLED += 10
self.Send('setled {}'.format(self.pwrLED))
self.heartBeat.lockHeartBeat.unlock(
) # allow heartbeat thread to continue
def UpdatePower(self):
self.lblPower.setText(str(self.sldPower.value()))
def Send(self, cmd):
self.ProcessResponse(self.API.send(cmd))
def ProcessResponse(self, response):
self.status = response[0]
self.voltage = response[1]
self.current = response[2]
if -1 == self.voltage or -1 == self.current:
self.lblVoltage.setText('?')
self.lblCurrent.setText('?')
else:
self.lblVoltage.setText('{:.2f}V'.format(self.voltage))
self.lblCurrent.setText('{:.2f}A'.format(self.current))
def __init__(self):
self.udp_client = UDPClient()
self.joystick = Joystick()
self.auto = False
class JsController(Controller):
def __init__(self, model, view, jsdevnum):
Controller.__init__(self, model, view)
self.jsdevnum = jsdevnum
self.joystick = None
self.joystick_x = 0
self.joystick_y = 0
def register_view(self, view):
pass
def register_adapters(self):
a = Adapter(self.model, 'joy_x')
a.connect_widget(self.view['label_js_x'], setter=self.joystick_setter)
self.adapt(a)
a = Adapter(self.model, 'joy_y')
a.connect_widget(self.view['label_js_y'], setter=self.joystick_setter)
self.adapt(a)
def enableJoystick(self):
logging.debug('enableJoystick')
self.joystick_enabled = True
try:
self.joystick = Joystick(self.jsdevnum)
except:
self.joystick_enabled = False
#self.view['rb_js_enable'].set_active(False)
#self.view['rb_js_disable'].set_active(True)
logging.error('could not enable joystick')
return
self.jsAxisHandler = self.joystick.connect('axis', self.on_axis_event)
self.jsButtonHandler = self.joystick.connect('button', self.on_button_event)
def disableJoystick(self):
self.joystick_enabled = False
#self.view['rb_js_enable'].set_active(True)
#self.view['rb_js_disable'].set_active(False)
if self.joystick is not None:
self.joystick.disconnect(self.jsAxisHandler)
self.joystick.disconnect(self.jsButtonHandler)
self.joystick = None
# signal handlers
def on_rb_js_enable_toggled(self, btn):
logging.debug('on_rb_js_enable_toggled')
if btn.get_active():
self.enableJoystick()
else:
self.disableJoystick()
def on_tb_calibrate_clicked(self, btn):
#TODO
pass
def on_axis_event(self, object, axis, value, init):
if init == 128:
# Ignore this event. One gets sent per axis when the joystick
# is initialized. I should really find out why.
return
if axis == 0:
# dividing by 256 scales the value to fit within a signed char
self.prev_x = self.joystick_x
self.joystick_x = value / 256
if self.joystick_x == self.prev_x:
return
elif axis == 1:
# this axis needs to be inverted
self.prev_y = self.joystick_y
self.joystick_y = -value / 256
if self.joystick_y == self.prev_y:
return
else: # ignore other axises
return
self.model.driveModel.joystickCommand(self.joystick_x, self.joystick_y)
def on_button_event(self, object, button, value, init):
pass
# special setters
def joystick_setter(self, wid, val):
if val != 0:
color = gtk.gdk.Color('#00FF00')
else:
color = None
wid.set_label(str(val))
if wid == self.view['label_js_x']:
self.view['eb_js_x'].modify_bg(gtk.STATE_NORMAL, color)
else:
self.view['eb_js_y'].modify_bg(gtk.STATE_NORMAL, color)
def ampMoveDiscon(input):
i = input * 2
return i**3 * -1
class Printer:
def start(self, topot):
topot.registerOutput("print", self.printer)
def printer(self, input, prefix = ""):
def prnt():
print prefix, input.value
return OutputSignal(prnt, input)
t.add(Printer())
t.add(Ticker(20))
t.add(Joystick("/dev/input/js0"), "j0_")
t.add(MidiInput())
t.add(MidiOutput())
t.connect("note", 48, t.get("j0_button",0))
t.connect("print", "note 48", t.get("note", 48))
t.connect("print", "escape:", t.get("key", 9))
t.connect("click", 1, t.get("j0_button", 4))
t.connect("click", 2, t.get("j0_button", 5))
t.connect("click", 3, t.get("j0_button", 6))
t.connect("mousemove", pair(transform(t.get("j0_axis", 2), ampMove),
transform(t.get("j0_axis", 3), ampMove)))
t.connect("key", 98, t.get("j0_axis", 4))
t.connect("print", primitive(repeat(t.get("tick"), transform(t.get("j0_axis", 1), ampMoveDiscon)), 0, 0, 127))
t.connect("note", 50, primitive(repeat(t.get("tick"), transform(t.get("j0_axis", 1), ampMoveDiscon)), 0, 0, 127))
class AzathothClient:
def __init__(self):
self.connected = False
self.builder = gtk.Builder()
self.builder.add_from_file('main.glade')
self.mainWindow = self.builder.get_object('window_main')
self.builder.connect_signals(self)
# let's just add widgets as members programatically.
widgets = ('btn_disconnect', 'btn_connect', 'statusbar', 'label_js_x',
'label_js_y', 'rb_js_enable', 'rb_js_disable', 'eb_js_x', 'eb_js_y',
'eb_drivestatus', 'label_drivestatus', 'label_drive_x', 'label_drive_y',
'label_raw_x', 'label_raw_y', 'label_select_cmd', 'label_select_act',
'eb_select_cmd', 'eb_select_act', 'label_estop_cmd', 'label_estop_act',
'eb_estop_cmd', 'eb_estop_act')
for wid in widgets:
setattr(self, wid, self.builder.get_object(wid))
self.context_id = self.statusbar.get_context_id("Azathoth")
self.prev_x = 0
self.prev_y = 0
self.joystick = None
self.joystick_x = 0
self.joystick_y = 0
self.joystick_enabled = False
self.mainWindow.show_all()
def connect(self, host, port=2024):
log.msg(format="Connecting to host %(host)s on port %(port)d", host=host, port=port)
self.statusbar.push(self.context_id, 'Connecting...')
self.factory = ControlFactory(self)
self.connection = reactor.connectTCP(host, port, self.factory)
def disconnect(self):
log.msg("Disconnecting")
self.connection.disconnect()
def setUiState(self, state):
connected_controls = ('btn_disconnect', 'rb_js_enable', 'rb_js_disable',
'btn_select', 'btn_deselect', 'tb_estop', 'tb_reset', 'tb_calibrate',
'imi_calibration')
if state == 'connecting':
self.btn_disconnect.set_sensitive(True)
self.btn_connect.set_sensitive(False)
elif state == 'connected':
for control in connected_controls:
self.builder.get_object(control).set_sensitive(True)
elif state == 'disconnected':
if self.joystick_enabled:
self.rb_js_enable.set_active(False)
self.rb_js_disable.set_active(True)
self.disableJoystick()
for control in connected_controls:
self.builder.get_object(control).set_sensitive(False)
self.btn_connect.set_sensitive(True)
def enableJoystick(self):
self.joystick_enabled = True
try:
self.joystick = Joystick(1)
except:
self.joystick_enabled = False
self.rb_js_enable.set_active(True)
self.rb_js_disable.set_active(False)
self.statusbar.push(self.context_id, 'Joystick device error')
return
self.js_handler = self.joystick.connect('axis', self.axis_event)
self.factory.control.send_select_command(True)
def disableJoystick(self):
self.joystick_enabled = False
self.rb_js_enable.set_active(True)
self.rb_js_disable.set_active(False)
if self.joystick is not None:
self.joystick.disconnect(self.js_handler)
self.joystick.shutdown()
self.joystick = None
self.factory.control.send_select_command(False)
def onStartConnection(self):
self.setUiState('connecting')
def onConnect(self):
self.statusbar.remove_all(self.context_id)
self.statusbar.push(self.context_id, 'Connected')
self.setUiState('connected')
def onConnectionLost(self):
self.setUiState('disconnected')
self.statusbar.remove_all(self.context_id)
self.statusbar.push(self.context_id, 'Disconnected')
if self.calDlg is not None:
self.calDlg.destroy()
def onConnectionFailed(self, reason):
self.setUiState('disconnected')
self.statusbar.remove_all(self.context_id)
self.statusbar.push(self.context_id, 'Connection failed!')
def onUpdateAxis(self):
if self.joystick_x != 0:
self.eb_js_x.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
elif self.joystick_x == 0:
self.eb_js_x.modify_bg(gtk.STATE_NORMAL, None)
if self.joystick_y != 0:
self.eb_js_y.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
elif self.joystick_y == 0:
self.eb_js_y.modify_bg(gtk.STATE_NORMAL, None)
self.label_js_x.set_label(str(self.joystick_x))
self.label_js_y.set_label(str(self.joystick_y))
self.factory.control.send_joystick_command(self.joystick_x, self.joystick_y)
def onStatusUpdate(self, status, xpos, ypos, xval, yval):
estop_act = (status & 0x01 == 0x01)
estop_cmd = (status & 0x02 == 0x02)
select_act = (status & 0x04 == 0x04)
select_cmd = (status & 0x08 == 0x08)
moving = (status & 0x10 == 0x10)
if moving:
self.label_drivestatus.set_label("MOVING")
self.eb_drivestatus.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
else:
self.label_drivestatus.set_label("STOPPED")
self.eb_drivestatus.modify_bg(gtk.STATE_NORMAL, None)
if estop_act:
self.label_estop_act.set_label("RUN")
self.eb_estop_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
else:
self.label_estop_act.set_label("STOP")
self.eb_estop_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#FF0000'))
if estop_cmd:
self.label_estop_cmd.set_label("RUN")
self.eb_estop_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
else:
self.label_estop_cmd.set_label("STOP")
self.eb_estop_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#FF0000'))
if select_act:
self.label_select_act.set_label("ROBOT")
self.eb_select_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
else:
self.label_select_act.set_label("CHAIR")
self.eb_select_act.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FFFF'))
if select_cmd:
self.label_select_cmd.set_label("ROBOT")
self.eb_select_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FF00'))
else:
self.label_select_cmd.set_label("CHAIR")
self.eb_select_cmd.modify_bg(gtk.STATE_NORMAL, gtk.gdk.Color('#00FFFF'))
self.label_drive_x.set_label(str(xpos))
self.label_drive_y.set_label(str(ypos))
self.label_raw_x.set_label(str(xval))
self.label_raw_y.set_label(str(yval))
def on_window_main_delete_event(self, win, event):
reactor.stop()
def on_imi_quit_activate(self, widget):
reactor.stop()
def on_imi_calibration_activate(self, widget):
self.calDlg = CalibrationDialog(self)
def on_btn_connect_clicked(self, widget):
host = self.builder.get_object('entry_host').get_text()
self.connect(host)
def on_btn_disconnect_clicked(self, widget):
self.disconnect()
def on_rb_js_enable_toggled(self, btn):
if btn.get_active():
self.enableJoystick()
else:
self.disableJoystick()
def on_btn_select_clicked(self, btn):
self.factory.control.send_select_command(True)
def on_btn_deselect_clicked(self, btn):
self.factory.control.send_select_command(False)
def on_tb_estop_clicked(self, btn):
self.factory.control.send_estop_command()
def on_tb_reset_clicked(self, btn):
self.factory.control.send_reset_command()
def on_tb_calibrate_clicked(self, btn):
self.calDlg = CalibrationDialog(self)
def axis_event(self, object, axis, value, init):
if init == 128:
# Ignore this event. One gets sent per axis when the joystick
# is initialized. I should really find out why.
return
if axis == 0:
# dividing by 256 scales the value to fit within a signed char
self.prev_x = self.joystick_x
self.joystick_x = value / 256
if self.joystick_x == self.prev_x:
return
if axis == 1:
# this axis needs to be inverted
self.prev_y = self.joystick_y
self.joystick_y = -value / 256
if self.joystick_y == self.prev_y:
return
self.onUpdateAxis()
def button_event(self, object, button, value, init):
pass
def __init__(self):
PM.pre_init(44100, -16, 1, 1024)
PG.init()
###(Screen stuff)####
Globals.SCREEN.fill((255, 255, 255))
PD.set_caption("Master Chef's wicked adventure " +
"with his ice cream buddies")
###(Declare interface)#####
self.font = PF.SysFont('Arial', 25)
#Win/Lose items
self.end_time = 100
self.win_image = PI.load("FPGraphics/" +
"specialEffects/UWIN.png").convert_alpha()
self.lose_image = PI.load("FPGraphics/" +
"specialEffects/ULOSE.png").convert_alpha()
self.MAX_LEVEL = 4
self.MAX_STAGE = 2
#items
self.pill_img = PI.load("FPGraphics/tiles/" +
"lactasePill.png").convert_alpha()
######(Initialize objects on screen)####
##draw map/background
##draw sprites
self.character = Player(Globals.DELTA)
self.INVINCIBILITY_TIME = Globals.DEFAULT_INVINCIBILITY
self.player_group = PS.GroupSingle(self.character)
# adding extra since cutscene bug deletes one
# self.remainingEnemies = self.num_enemies
#create icecream group
self.icecream_list = PS.Group()
self.burger_list = PS.Group()
self.egg_list = PS.Group()
self.lettuce_list = PS.Group()
self.cupcake_list = PS.Group()
self.enemy_list = PS.Group() # all enemies
self.pad_list = PS.Group()
self.trap_group = PS.Group()
self.item_group = PS.Group()
self.projectile_group = PS.Group()
self.enemies = None
#allsprites has all dirty sprites (player, enemies, traps, pads)
self.allsprites = PS.LayeredDirty(self.trap_group, self.pad_list,
self.item_group, self.player_group,
self.icecream_list, self.burger_list,
self.egg_list, self.lettuce_list,
self.cupcake_list,
self.projectile_group)
#variables to be handled in change_level method
self.objective = None
self.objectiveBlit = True
self.updated_obj = False
self.map = None
self.num_enemies = 0
self.background = None
self.end_time = 100
self.end_image_position = (100, 178)
self.block_group = None
####(Level variables)####
Globals.INVINCIBILITY_COUNT = 0 # player's invinicibility frame time
#what kind of enemy by ID (-1 means no enemy) used for collisions
self.enemy_ID = -1
#if true, tells map to redraw
self.map_modified = False
# self.level = 1
# self.stage = 1
self.level = 1
self.stage = 1
self.change_level(self.level, self.stage)
self.burn_player = False
####Joystick#########
self.joy = Joystick()
self.use_joy = str(inbx.ask(Globals.SCREEN, 'Joystick? y/n'))
self.score_health_background = PI.load(
"FPGraphics/specialEffects/ScoreHealth.png").convert_alpha()
self.items_table = PI.load(
"FPGraphics/specialEffects/ItemsTable.png").convert_alpha()
import time
from joystick import Joystick
g = Joystick(name='Test_Gamepad')
analogs = [x for x in g.actions if g.actions[x][1] == Joystick.ANALOG]
buttons = [x for x in g.actions if g.actions[x][1] == Joystick.BUTTON]
print("Press ENTER to begin test")
raw_input()
for i in range(3, 0, -1):
print(i)
time.sleep(1)
print("Testing analog actions:")
for a in analogs:
print(a)
for i in range(0, 256, 4):
g.update({a: i})
time.sleep(0.02)
for b in buttons:
print(b)
g.update({b: 1})
time.sleep(0.5)
for b in buttons:
for c in buttons:
if c != b:
print(b + " + " + c)
addresses = {
'Ball X': 12288,
'Ball Y': 12290,
'Joystick X': 12292,
'Joystick Y': 12294
}
# Main loop
if __name__ == '__main__':
cap = cv2.VideoCapture(1)
cap.set(propId=3, value=640)
cap.set(propId=4, value=480)
# Creating objects
client = ModbusClient()
js = Joystick()
ballTracking = BallTracking(capture=cap, watch=True, color='dark-green')
# Sends data over Modbus client for as long the connection is established
while client.isConnected():
ball_coordinates = ballTracking.getCoordinates()
js_coordinates = js.getEvents()
client.sendInt(value=ball_coordinates[0], address=addresses['Ball X'])
client.sendInt(value=ball_coordinates[1], address=addresses['Ball Y'])
client.sendInt(value=js_coordinates[0],
address=addresses['Joystick X'])
client.sendInt(value=js_coordinates[1],
address=addresses['Joystick Y'])
# Break loop with ESC-key
key = cv2.waitKey(5) & 0xFF
for j in range(p.getNumJoints(boxId)):
# p.changeDynamics(boxId, j, linearDamping=0, angularDamping=0)
info = p.getJointInfo(boxId, j)
print(info)
jointName = info[1]
jointType = info[2]
jointIds.append(j)
footFR_index = 3
footFL_index = 7
footBR_index = 11
footBL_index = 15
pybulletDebug = pybulletDebug()
robotKinematics = robotKinematics()
joystick = Joystick('/dev/input/event18') #need to specify the event route
arduino = ArduinoSerial('/dev/ttyACM1') #need to specify the serial port
trot = trotGait()
#robot properties
"""initial safe position"""
#angles
targetAngs = np.array([
0,
np.pi / 4,
-np.pi / 2,
0, #BR
0,
np.pi / 4,
-np.pi / 2,
0, #BL
0,
def pause(self):
self.__flag.clear() # set False
def resume(self):
self.__flag.set() # set True
def stop(self):
self.__flag.set() # resume from pause
self.__running.clear()
def running(self):
return self.__flag.isSet()
if __name__ == '__main__':
js = Joystick()
car = Car()
camera = Camera()
camera.start()
try:
while True:
map = js.run()
x = map.rx
car.turn(x)
# y = map.ly
# car.turn(y)
if map.menu == 1:
class WheelChairController:
def __init__(self):
#channel numbers
self.errorLightOut = 4
self.errorLightLow = 18
self.switchOut = 13
self.switchDetect = 20
#constants
self.rs_stopped = 0
self.rs_forward = 1
self.rs_reverse = 2
self.ss_nothing = 0 #nothing has happened yet
self.ss_start = 1 #both eyes open complete
self.ss_forward1 = 2 #one eye open complete
self.ss_forward2 = 3 #both eyes closed complete
self.ss_reverse1 = 4 #both eyes closed complete
self.redFac = .75
self.addFac = 1 - self.redFac
self.maxSwitchTime = 7 #leave room for waiting and messing up
self.maxReverseTime = 5
self.defaultPrevX = 0
self.defaultPrevY = -.5
#vars
self.runState = self.rs_stopped
self.runTime = 0
self.switchState = self.ss_nothing
self.switchTime = 0
self.prevX = self.defaultPrevX
self.prevY = self.defaultPrevY
self.joystick = Joystick()
io.setmode(io.BCM)
io.setup(self.errorLightOut, io.OUT, initial=io.LOW)
io.setup(self.errorLightLow, io.OUT, initial=io.LOW)
io.setup(self.switchOut, io.OUT, initial=io.HIGH)
io.setup(self.switchDetect, io.IN, pull_up_down=io.PUD_DOWN)
time.sleep(.2) #wait for io to definitely turn on before attempting to sense
if self.should_shut_down():
print("waiting for switch on")
io.wait_for_edge(self.switchDetect, io.RISING) #wait for switch to turn on.
self.set_led(io.HIGH) #indicate success
time.sleep(.3) #wait so people know for sure that its working
def sig_no_eye(self, now):
#print("sig no eye")
pass #probs just for debugging
def sig_direction(self, x, y):
#print("go ", x, ", ", y)
self.prevX = self.prevX * self.redFac + x * self.addFac
self.prevY = self.prevY * self.redFac + y * self.addFac
if self.runState == self.rs_forward:
self.joystick.forward(self.prevX, self.prevY)
def sig_error_reset(self):
#print("turn on error light")
self.joystick.stop()
self.set_led(io.HIGH)
def sig_turn_off_error_light(self):
self.set_led(io.LOW)
def process_blink_commands(self, useGroups, now):
temp = sorted(useGroups, key=lambda group: group.boxCenterX)
eyes = len(useGroups)
isLeft = True
left = False
right = False
for group in temp:
if group.lastOpen == now:
if isLeft:
left = True
isLeft = False
else:
right = True
self.joystick.showEyes(left, right)
counts1 = self.count_blinks(useGroups, 1, now) #be, bd, ne, nd
if now - self.switchTime > self.maxSwitchTime and not (self.switchState == self.ss_start and counts1[3] == 2): # don't timeout if during start (because that would be confusing) if eyes are mostly open still
if self.switchState != self.ss_nothing:
print("canceled switch due to timeout")
self.switchState = self.ss_nothing
if (self.runState == self.rs_reverse and now - self.runTime > self.maxReverseTime) or eyes == 0:
self.stop(now)
print("no eye stop")
elif eyes == 2: #other eyes == 1 case is just continue forward
countsHalf = self.count_blinks(useGroups, .5, now)
if self.runState == self.rs_forward:
if counts1[3] < 2 or countsHalf[1] == 2: #not both ~open1 or both =closed.5
self.stop(now)
print("forward stop")
elif self.runState == self.rs_reverse:
if counts1[3] == 2 or countsHalf[1] == 2: #both ~open1 or both =closed.5
self.stop(now)
print("reverse stop")
elif self.runState == self.rs_stopped:
if self.switchState == self.ss_nothing:
if counts1[2] == 2: #both =open1
self.switchState = self.ss_start
print("ss_start")
self.switchTime = now
else:
print("still nothing")
elif self.switchState == self.ss_start:
if counts1[0] == 2: #both =closed1
self.switchState = self.ss_reverse1
self.switchTime = now
print("ss_reverse1")
elif counts1[1] == 1 and counts1[3] == 1: #one ~open1, one ~closed1
self.switchState = self.ss_forward1
self.swtichTime = now
print("ss_forward1")
else:
print("still start")
elif self.switchState == self.ss_reverse1:
if counts1[1] == 1 and counts1[3] == 1: #one ~open1, one ~closed1
self.start_reverse(now)
elif counts1[2] == 2: #both =open1
self.switchState = self.ss_start
self.switchTime = now
print("cancel")
else:
print("still reverse1")
elif self.switchState == self.ss_forward1:
if counts1[0] == 2: #both =closed1
self.switchState = self.ss_forward2
self.switchTime = now
print("ss_forward2")
elif counts1[2] == 2: #both =open1
self.switchState = self.ss_start
self.switchTime = now
print("cancel")
else:
print("still forward1")
elif self.switchState == self.ss_forward2:
if counts1[2] == 2: #both =open1
self.start_forward(now)
elif counts1[0] == 1 and counts1[2] == 1: #one =open1, one =closed1
self.switchState = self.ss_start
self.switchTime = now
print("cancel")
else:
print("still forward2")
else:
#something went terribly wrong
print("aww nuts")
self.stop(now)
else:
#something went terribly wrong
print("awww nutz")
self.stop(now)
def count_blinks(self, useGroups, timeReq, now):
#return format: [blinking_exact, blinking_denoised, notBlinking_exact, notBlinking_denoised]
be = sum((now - group.lastOpen) > timeReq for group in useGroups)
bd = sum((now - group.lastDefOpen) > timeReq for group in useGroups)
ne = sum((now - group.lastClosed) > timeReq for group in useGroups)
nd = sum((now - group.lastDefClosed) > timeReq for group in useGroups)
return [be, bd, ne, nd]
def stop(self, now):
self.runState = self.rs_stopped
self.switchState = self.ss_nothing
self.joystick.stop()
self.runTime = now
def start_forward(self, now):
self.runState = self.rs_forward
self.switchState = self.ss_nothing
self.joystick.forward(self.prevX, self.prevY)
self.runTime = now
print("start forward")
def start_reverse(self, now):
self.runState = self.rs_reverse
self.switchState = self.ss_nothing
self.joystick.reverse()
self.runTime = now
print("start reverse")
def should_shut_down(self):
return io.input(self.switchDetect) == io.LOW
def set_led(self, state):
io.output(self.errorLightOut, state)
def release_assets(self):
print("releasing all assets")
self.joystick.stop()
self.joystick.shutdown()
io.cleanup()
from joystick import Joystick
# defines for what serial port motor interface is connected to + baud rate
motor_com_port = '/dev/ttyACM0'
sensor_com_port = '/dev/ttyACM1'
baud = 9600
motor_ser = serial.Serial(motor_com_port, baud)
motor_ser.close()
motor_ser.open()
sensor_ser = serial.Serial(sensor_com_port, baud)
sensor_ser.close()
sensor_ser.open()
joystick = Joystick()
prev_dir = -1
prev_speed = -1
while True:
#speed = int(input("speed:"))
#print("speed:", speed)
#dir_input = int(input("direction:"))
#direction = 0
#if dir_input == 1:
# direction = 1
#elif dir_input == 0:
# direction = 0
#print("dir:",direction)
from driveUnit import driveUnit
from joystick import Joystick
import time
drive=driveUnit()
joy=Joystick()
while True:
driveUnit.setMotorLeft((-joy.y_axis()-joy.x_axis())*512)
driveunit.setMotorRight((-joy.y_axis()+joy.x_axis())*512)
time.sleep(0.1)
