-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathcreate3_dualshock4.py
117 lines (102 loc) · 3.62 KB
/
create3_dualshock4.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
import sys
import pygame
import serial
from irobot_edu_sdk.backend.bluetooth import Bluetooth
from irobot_edu_sdk.robots import event, Create3
# a script to control the create 3 robot with a Dualshock 4 controller
# set to create3 bluetooth name
create3_name = "chompions"
# initialize dependencies
robot = Create3(Bluetooth(name=create3_name))
pygame.init()
pygame.joystick.init()
ser = serial.Serial("COM7", 9600, timeout = 1)
# check for controller connection
if pygame.joystick.get_count() == 0:
print("No controller found. Please connect a controller.")
sys.exit(1)
controller = pygame.joystick.Joystick(0)
controller.init()
# write a data packet as serial
def sendData(packet):
ser.write(packet)
# set l/r motor values based on controller trigger and axis values
def set_motor_values(controller):
speed = 50
# raw data
left_joystick_x = controller.get_axis(0)
left_trigger = controller.get_axis(4)
right_trigger = controller.get_axis(5)
# normalized from 0-1
ltv = (left_trigger + 1) / 2
rtv = (right_trigger + 1) / 2
abs_trigger = rtv - ltv
# sharp turning
if abs_trigger == 0:
if left_joystick_x > 0.01 :
l_differential = abs(left_joystick_x)
r_differential = - abs(left_joystick_x)
elif left_joystick_x < -0.01:
l_differential = - abs(left_joystick_x)
r_differential = abs(left_joystick_x)
else:
l_differential = 0
r_differential = 0
left_motor = l_differential * speed * 0.5
right_motor = r_differential * speed * 0.5
# forward driving
elif abs_trigger > 0:
if left_joystick_x > 0.01 :
l_differential = 0
r_differential = abs(left_joystick_x)
elif left_joystick_x < -0.01:
l_differential = abs(left_joystick_x)
r_differential = 0
else:
l_differential = 0
r_differential = 0
left_motor = -(abs_trigger - (r_differential/1.1)) * speed
right_motor = -(abs_trigger - (l_differential/1.1)) * speed
# backwards driving
elif abs_trigger < 0:
if left_joystick_x > 0.01 :
l_differential = abs(left_joystick_x)
r_differential = 0
elif left_joystick_x < -0.01:
l_differential = 0
r_differential = abs(left_joystick_x)
else:
l_differential = 0
r_differential = 0
left_motor = -(abs_trigger + (l_differential/1.1)) * speed
right_motor = -(abs_trigger + (r_differential/1.1)) * speed
return left_motor, right_motor
# send data packets to arduino to control auxilary motors
def control_motors(controller):
if controller.get_button(0) == 1:
print("Starting spinner.")
sendData(b'1')
if controller.get_button(1) == 1:
print("Stopping spinner.")
sendData(b'2')
if controller.get_button(2) == 1:
print("Opening door.")
sendData(b'3')
if controller.get_button(3) == 1:
print("Closing Door.")
sendData(b'4')
if controller.get_button(9) == 1:
print("Decreasing Speed.")
sendData(b'5')
if controller.get_button(10) == 1:
print("Increasing Speed.")
sendData(b'6')
# main looping function
@event(robot.when_play)
async def play(robot):
print('Ready for control.')
pygame.event.pump()
control_motors(controller)
motor_values = set_motor_values(controller)
await robot.set_wheel_speeds(motor_values[0], motor_values[1])
robot.play()