#select drive controller based on name
# this means using a different controller will require the hardcoded name to change
elif joysticks[-1].get_name() == arm_stick_name:
print("Detected arm controller '%s'" % joysticks[-1].get_name())
arm_stick = joysticks[-1]
else:
print("Detected unbound controller '%s'" % joysticks[-1].get_name())
#start main loop
rover = phoenix.Phoenix(master_ip)
rover.open()
"""rover.killAll()"""
#every update, run the below
while not BSExit:
#main menu screen
while mainMenu == True:
for event in pygame.event.get():
#quit program
if event.type == pygame.QUIT:
BSExit = True
mainMenu = False
# This code runs the arm on the Phoenix rover.
#initialize and import
import pygame #for controller
import math
import socket #for sockets
import sys #for exit
import phoenix
pygame.init()
pygame.joystick.init()
joysticks = []
rover = phoenix.Phoenix('192.168.1.11')
ip = "192.168.1.11"
port = 9002
drive_stick_name = ""
shoulder_stick_draw_pos = (150, 350)
wrist_stick_draw_pos = (350, 350)
elbow_pitch_draw_pos = (50, 350)
claw_actuation_draw_pos = (450, 350)
temp_label_draw_pos = (100, 100)
#open window
screen = pygame.display.set_mode((500, 500))
pygame.display.set_caption("Rover Arm Driving Code")
#find xbox controller
def test():
global channel_vals
rover = phoenix.Phoenix('192.168.1.11')
rospy.init_node('DrivetrainSW')
for i in range(3):
topic = '/Drive_Board_HW/PWM_Channels/PulseTimeControl/CH' + str(i)
print(topic)
subscriptions.append(
rospy.Subscriber(topic, Float64, callback, i, queue_size=speed))
for i in range(4, 7):
topic = '/Drive_Board_HW/PWM_Channels/PulseTimeControl/CH' + str(i)
print(topic)
subscriptions.append(
rospy.Subscriber(topic, Float64, callback, i - 1,
queue_size=speed))
rover.setWheels(speed, speed)
time.sleep(0.5)
rover.setWheels(0, 0)
time.sleep(0.5)
for channel in range(6):
assert channel_vals[channel] == 0.0015, channel_vals[channel]
rover.setWheels(+speed, +speed)
channel_directions = [0, 0, 0, 0, 0, 0]
time.sleep(0.5)
for channel in range(6):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
channel_directions[channel] = -1
else:
channel_directions[channel] = 1
rover.setWheels(-speed, -speed)
time.sleep(0.5)
for channel in range(6):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == 1
else:
assert channel_directions[channel] == -1
rover.setWheels(+speed, 0)
time.sleep(0.5)
for channel in range(3):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == -1
else:
assert channel_directions[channel] == 1
for channel in range(3, 6):
assert channel_vals[channel] == 0.0015
rover.setWheels(-speed, 0)
time.sleep(0.5)
for channel in range(3):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == 1
else:
assert channel_directions[channel] == -1
for channel in range(3, 6):
assert channel_vals[channel] == 0.0015
rover.setWheels(0, +speed)
time.sleep(0.5)
for channel in range(3):
assert channel_vals[channel] == 0.0015
for channel in range(3, 6):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == -1
else:
assert channel_directions[channel] == 1
rover.setWheels(0, -speed)
time.sleep(0.5)
for channel in range(3):
assert channel_vals[channel] == 0.0015
for channel in range(3, 6):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == 1
else:
assert channel_directions[channel] == -1
rover.setWheels(speed, -speed)
time.sleep(0.5)
for channel in range(3):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == -1
else:
assert channel_directions[channel] == 1
for channel in range(3, 6):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == 1
else:
assert channel_directions[channel] == -1
rover.setWheels(-speed, speed)
time.sleep(0.5)
for channel in range(3):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == 1
else:
assert channel_directions[channel] == -1
for channel in range(3, 6):
assert channel_vals[channel] != 0.0015
if (channel_vals[channel] < 0.0015):
assert channel_directions[channel] == -1
else:
assert channel_directions[channel] == 1
rover.setWheels(0, 0)
for subscription in subscriptions:
subscription.unregister()
return