Skip to content

Commit

Permalink
Seperated out checking buttons/axis/hats into seperate functions
Browse files Browse the repository at this point in the history 
Seperated checking Buttons/Hats/Joystick into seperate functions.  Also fixed a few bugs with mode switching.
  • Loading branch information
@rerdtsieck
rerdtsieck committed Nov 15, 2017
1 parent 7c2409e commit 4d16202
Show file tree
Hide file tree
Showing 3 changed files with 794 additions and 150 deletions.
335 changes: 185 additions & 150 deletions rover/core/servers/ArduinoSocketServer/Joystick_SocketCall.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,6 @@
GPIO.setup(greenLed, GPIO.OUT) #Green LED
GPIO.setup(blueLed, GPIO.OUT) #Blue LED


pygame.init()

# Initialize the joysticks
Expand All @@ -38,7 +37,7 @@
# Arduino address and connection info
address = ('192.168.1.177', 5000)
client_socket = socket(AF_INET, SOCK_DGRAM)
client_socket.settimeout(1)
#client_socket.settimeout(.5)

# Globals variables for data transmittion
global re_data
Expand All @@ -60,6 +59,8 @@
global modeWhenPaused
modeWhenPaused = ""

#global motor1, motor2, pauseInterval, pauseQuitInterval, modeWhenPause, motor_mult, arm1, arm2, joint5, joint6, joint7, mode

# Global variable for motor throttle
global motor_mult
motor_mult = .5
Expand Down Expand Up @@ -91,14 +92,18 @@ def stop():
joint1 = joint5 = joint6 = joint7 = 0
client_socket.sendto(data, address)
print("Sent Stop Command")
#GPIO.output(redLed,GPIO.HIGH)
#GPIO.output(greenLed,GPIO.LOW)
#GPIO.output(blueLed,GPIO.LOW)
except:
print("Failed to send Stop Command")
pass
return;

# This function changes the color of the LED on the Pi, based off of the current mode the rover is in
# Green meaning BOTH Mobility and Arm are active
# Blue meaning ONLY Mobility is active
# Purple meaning ONLY Arm is active
# Red meaning NEITHER Mobility or Arm is actice, we are in a paused state
# Input: NONE
# Output: NONE
def changeLedColor():
if mode == "both":
#LED Color = Green
Expand All @@ -121,12 +126,165 @@ def changeLedColor():
GPIO.output(greenLed,GPIO.LOW)
GPIO.output(blueLed,GPIO.LOW)

GPIO.output(greenLed,GPIO.HIGH)
GPIO.output(redLed,GPIO.LOW)
GPIO.output(blueLed,GPIO.LOW)
# This function takes in a joystick and reads the values of each joystick Axis.
# Based on the values of the joystick axis it sets the corresponding motor values
# to be sent to the ESC.
# Input: Joystick
# Output: None
def checkJoystickMovement(currentJoystick):
global motor1, motor2, pauseInterval, pauseQuitInterval, modeWhenPause, motor_mult, arm1, arm2, joint5, joint6, joint7, mode
axes = currentJoystick.get_numaxes()

# Check for axes usage
for i in range( axes ):
axis = joystick.get_axis( i )
if mode == "mobility" or mode == "both":
if i == 1:
motor1 = -int(127 * axis * motor_mult)
if i == 0:
motor2 = int(127 * axis * motor_mult)
if mode == "arm" or mode == "both":
if i == 2:
arm1 = int(127 * axis)
if i == 3:
arm2 = int(127 * axis)

# This function takes in a joystick and cycles through all of the buttons to see if any are pushed.
# If any are pushed it sets the corresponding command to the rover.
# Input: Joystick
# Output: None
# List of Buttons and what they move (Logitech Wireless Controller).
# Button 1: Joint 5.1 Rotate End Effector Left
# Button 2: Joint 4 Move Up
# Button 3: Joint 5.1 Rotate End Effector Right
# Button 4: Joint 4 Move Down
# Button 5: Joint 5.2 Close End Effector
# Button 6: Joint 5.2 Open End Effector
# Button 7: Decrease Motor Multiplier
# Button 8: Increase Motor Multiplier
# Button 9: Pause/Unpause Rover commands
# Button 10: Switch between modes
# Note: These button numbers are as they are on the controller.
# In the for loop the buttons go from 0-9 not 1-10
def checkButtons(currentJoystick):
global motor1, motor2, pauseInterval, pauseQuitInterval, modeWhenPause, motor_mult, arm1, arm2, joint5, joint6, joint7, mode, modeWhenPaused
#Get the number of buttons on the joystick
buttons = currentJoystick.get_numbuttons()

# Cycle through every button set corresponding values depending on whether button is pushed or not.
# Set the corresponding joint values if buttons 1-6
# Adjust the motor multiplier if buttons 7,8
# Pause/Unpause rover if Button 9
# Switch modes if Button 10
for i in range( buttons ):
#Gets whether button is pushed or not (0 = not pushed, 1 = pushed)
button = joystick.get_button( i )

#If arm is active set joint values
if mode == "both" or mode == "arm":
# Joint commands
if i == 1:
joint5 = button
elif i == 3 and joint5 == 0:
joint5 = -button

if i == 0:
joint6 = button
elif i == 2 and joint6 == 0:
joint6 = -button

if i == 4:
joint7 = button
elif i == 5 and joint7 == 0:
joint7 = -button

# If mobility is active change multiplier if buttons are pushed
if mode == "both" or mode == "mobility":
# Motor Multiplier Commands
if i == 6 and button == 1 and motor_mult > 0.31:
motor_mult = motor_mult - .1
print(motor_mult)

if i == 7 and button == 1 and motor_mult < .9:
motor_mult = motor_mult + .1
print(motor_mult)

# If Pause button is held down for atleast 3 seconds pause/unpause
if i == 9 and button == 1:
if pauseQuitInterval == 0:
pauseQuitInterval = datetime.now()
elif (datetime.now() - pauseQuitInterval).seconds > 3:
if mode != "pause":
print("Pausing Controls")

#Keeps mode when paused so we can return to the same mode
modeWhenPaused = mode
mode = "pause"
pauseQuitInterval = 0
stop()
#changeLedColor()
elif mode == "pause":
print("Resuming Controls")
mode = modeWhenPaused
modeWhenPaused = ""
pauseQuitInterval = 0
#changeLedColor()
elif i == 9 and button == 0 and pauseQuitInterval !=0:
print("Reseting Pause Interval")
pauseQuitInterval = 0

#This button switches between different Modes
#Green = Arm and Mobility
#Blue = Mobility
#Purple = Arm
#Red = None (Paused)
if i == 8 and button == 1:
#print(pauseInterval)
if pauseInterval == 0:
pauseInterval = datetime.now()
elif mode == "both":
if (datetime.now() - pauseInterval).seconds > 3:
print("Switching to MOBILITY ONLY mode")
mode = "mobility"
#stop()
pauseInterval = 0
#LED color = Blue
#changeLedColor()
elif mode == "mobility":
if (datetime.now() - pauseInterval).seconds > 3:
print("Switcching to ARM ONLY mode")
#stop()
mode = "arm"
pauseInterval = 0
#LED Color = Purple
#changeLedColor()
elif mode == "arm":
if (datetime.now() - pauseInterval).seconds > 3:
print("Switching to BOTH mode")
#stop()
mode = "both"
pauseInterval = 0
#LED Color = Green
#changeLedColor()
elif i == 8 and button == 0 and pauseInterval != 0:
print("reseting Pause Interval")
pauseInterval = 0

def checkHats(currentJoystick):
global motor1, motor2, pauseInterval, pauseQuitInterval, modeWhenPause, motor_mult, arm1, arm2, joint5, joint6, joint7, mode
hats = currentJoystick.get_numhats()

if mode == "arm" or mode == "both":
for i in range( hats ):
hat = joystick.get_hat( i )
if hat[0] != 0:
joint1 = hat[0]
else:
joint1 = 0

#Depending on which mode we are in only the correct buttons will be checked
while(1):
changeLedColor()
for event in pygame.event.get(): # User did something
# Possible joystick actions: JOYAXISMOTION JOYBALLMOTION JOYBUTTONDOWN JOYBUTTONUP JOYHATMOTION
if event.type == pygame.JOYBUTTONDOWN:
Expand All @@ -149,156 +307,33 @@ def changeLedColor():
# Get the name from the OS for the controller/joystick
name = joystick.get_name()

# Usually axis run in pairs, up/down for one, and left/right for
# the other.
axes = joystick.get_numaxes()
hats = joystick.get_numhats()

# Check for axes usage
for i in range( axes ):
axis = joystick.get_axis( i )
if mode == "mobility" or mode == "both":
if i == 1:
motor1 = -int(127 * axis * motor_mult)
if i == 0:
motor2 = int(127 * axis * motor_mult)
if mode == "arm" or mode == "both":
if i == 2:
arm1 = int(127 * axis)
if i == 3:
arm2 = int(127 * axis)




buttons = joystick.get_numbuttons()

# Check for button pushes
for i in range( buttons ):
button = joystick.get_button( i )
if (mode == "both" or mode == "arm"):
# Joint commands
if i == 1:
joint5 = button
elif i == 3 and joint5 == 0:
joint5 = -button

if i == 0:
joint6 = button
elif i == 2 and joint6 == 0:
joint6 = -button

if i == 4:
joint7 = button
elif i == 5 and joint7 == 0:
joint7 = -button

if (mode == "both" or mode == "mobility"):
# Motor Multiplier Commands
if i == 6 and button == 1 and motor_mult > 0.31:
#if button == 1:
motor_mult = motor_mult - .1
print(motor_mult)

if i == 7 and button == 1 and motor_mult < .9:
#if button == 1:
motor_mult = motor_mult + .1
print(motor_mult)

# Script timed quit command
if i == 9 and button == 1:
if pauseQuitInterval == 0:
pauseQuitInterval = datetime.now()
elif (datetime.now() - pauseQuitInterval).seconds > 3:
if mode != "pause":
print("Pausing Controls")
modeWhenPaused = mode
mode = "pause"
pauseQuitInterval = 0
stop()
changeLedColor()
elif mode == "pause":
print("Resuming Controls")
mode = modeWhenPaused
modeWhenPaused = ""
pauseQuitInterval = 0
changeLedColor()
elif i == 9 and button == 0 and pauseQuitInterval !=0:
print("Reseting Pause Interval")
pauseQuitInterval = 0

#This button switches between different Modes
#Green = Arm and Mobility
#Blue = Mobility
#Purple = Arm
#Red = None (Paused)
if i == 8 and button == 1:
#print(pauseInterval)
if pauseInterval == 0:
pauseInterval = datetime.now()
elif mode == "both":
if (datetime.now() - pauseInterval).seconds > 3:
print("Switching to MOBILITY ONLY mode")
mode = "mobility"
pauseInterval = 0
#LED color = Blue
changeLedColor()
elif mode == "mobility":
if (datetime.now() - pauseInterval).seconds > 3:
print("Switcching to ARM ONLY mode")
mode = "arm"
pauseInterval = 0
#LED Color = Purple
changeLedColor()
elif mode == "arm":
if (datetime.now() - pauseInterval).seconds > 3:
print("Switching to BOTH mode")
mode = "both"
pauseInterval = 0
#LED Color = Green
changeLedColor()
elif i == 8 and button == 0 and pauseInterval != 0:
print("reseting Pause Interval")
pauseInterval = 0
if mode == "arm" or mode == "both":
for i in range( hats ):
hat = joystick.get_hat( i )
if hat[0] != 0:
joint1 = hat[0]
else:
joint1 = 0

## # Joystick Pause command
## #if i == 9 and button == 1:
## # if pauseInterval == 0:
## # print("getting pause interval start")
## # pauseInterval = datetime.now()
## # elif (datetime.now() - pauseInterval).seconds > 3:
## # print("Longer than 3 seconds")
## if pauseFull == False:
## pauseFull = True
## pauseInterval = 0
## print "paused"
## stop()
## elif pauseFull == True:
## print "unpaused"
## pauseFull = False
## pauseInterval = 0
## GPIO.output(greenLed,GPIO.HIGH)
## GPIO.output(redLed,GPIO.LOW)
## GPIO.output(blueLed,GPIO.LOW)

checkJoystickMovement(joystick)
checkButtons(joystick)
checkHats(joystick)

# Command to Arduino
if not pauseFull:
if mode != 'pause':
print 'Sending Command to Arduino'
try:
if mode == 'both':
data = str(motor1) + ',' + str(motor2) + ',' + str(arm1) + ',' + str(arm2) + ',' + str(joint1) + ',' + str(joint5) + ',' + str(joint6) + ',' + str(joint7) + ',' + '0' + ',' + '0'
elif mode == 'arm':
data = '0' + ',' + '0' + ',' + str(arm1) + ',' + str(arm2) + ',' + str(joint1) + ',' + str(joint5) + ',' + str(joint6) + ',' + str(joint7) + ',' + '0' + ',' + '0'
elif mode == 'mobility':
data = str(motor1) + ',' + str(motor2) + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0'
elif mode == 'pause':
data = '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0' + ',' + '0'
print (data)
re_data, addr = client_socket.recvfrom(2048)
print (re_data)

if re_data == "ready":
data = str(motor1) + ',' + str(motor2) + ',' + str(arm1) + ',' + str(arm2) + ',' + str(joint1) + ',' + str(joint5) + ',' + str(joint6) + ',' + str(joint7) + ',' + '0' + ',' + '0'
#data = str(motor1) + ',' + str(motor2) + ',' + str(arm1) + ',' + str(arm2) + ',' + str(joint1) + ',' + str(joint5) + ',' + str(joint6) + ',' + str(joint7) + ',' + '0' + ',' + '0'
client_socket.sendto(data, address)
print (data)
except:
print 'Failed'
pass
# Safety catch to force new values or shutdown old ones
data = str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0) + ',' + str(0)
joint1 = joint5 = joint6 = joint7 = 0
joint1 = joint5 = joint6 = joint7 = motor1 = motor2 = arm1 = arm2 = 0

0 comments on commit 4d16202

Please sign in to comment.