-
Notifications
You must be signed in to change notification settings - Fork 0
/
RobotController.py
103 lines (82 loc) · 3.15 KB
/
RobotController.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
from pyPS4Controller.controller import Controller
from MotorController.L298nMotor import Motor
# DS4 stick values are in the range [-32767, 32767].
# This variable is necessary to calculate the turn factors.
DS4_VALUE_OFFSET = 32767
# GPIO Pins (BCM)
LEFT_MOTORS_FORWARD_PIN = 23
LEFT_MOTORS_BACKWARD_PIN = 24
LEFT_MOTORS_PWM_PIN = 12
RIGHT_MOTORS_FORWARD_PIN = 22
RIGHT_MOTORS_BACKWARD_PIN = 27
RIGHT_MOTORS_PWM_PIN = 13
class RobotController(Controller):
"""
Provides an interface to control a 4-wheeled skid steer robot with a Dualshock 4 controller.
Controls:
- L2: drive forward
- R2: drive backward
- L1: turn left in place
- R1: turn right in place
- L3: adjust turn factor
"""
def __init__(self, **kwargs):
Controller.__init__(self, **kwargs)
self.left_motors = Motor(
forward_pin=LEFT_MOTORS_FORWARD_PIN, backward_pin=LEFT_MOTORS_BACKWARD_PIN, pwm=LEFT_MOTORS_PWM_PIN)
self.right_motors = Motor(forward_pin=RIGHT_MOTORS_FORWARD_PIN,
backward_pin=RIGHT_MOTORS_BACKWARD_PIN, pwm=RIGHT_MOTORS_PWM_PIN)
self.speed_left = 1
self.speed_right = 1
self.turn_factor_left = 1
self.turn_factor_right = 1
def __stop(self):
self.left_motors.stop()
self.right_motors.stop()
def __reset_speed(self):
self.speed_left = 1
self.speed_right = 1
def __reset_turn_factors(self):
self.turn_factor_left = 1
self.turn_factor_right = 1
def on_R2_press(self, value):
self.speed_left = abs(value) / DS4_VALUE_OFFSET * self.turn_factor_left
self.speed_right = abs(value) / DS4_VALUE_OFFSET * \
self.turn_factor_right
self.left_motors.forward(self.speed_left)
self.right_motors.forward(self.speed_right)
def on_R2_release(self):
self.__stop()
def on_L2_press(self, value):
self.speed_left = abs(value) / DS4_VALUE_OFFSET * self.turn_factor_left
self.speed_right = abs(value) / DS4_VALUE_OFFSET * \
self.turn_factor_right
self.left_motors.backward(self.speed_left)
self.right_motors.backward(self.speed_right)
def on_L2_release(self):
self.__stop()
def on_L3_right(self, value):
self.turn_factor_left = 1
self.turn_factor_right = (
DS4_VALUE_OFFSET - abs(value)) / DS4_VALUE_OFFSET
def on_L3_left(self, value):
self.turn_factor_left = (
DS4_VALUE_OFFSET - abs(value)) / DS4_VALUE_OFFSET
self.turn_factor_right = 1
def on_L3_release(self):
self.__reset_turn_factors()
def on_R1_press(self):
self.__reset_speed()
self.left_motors.forward(self.speed_left)
self.right_motors.backward(self.speed_right)
def on_R1_release(self):
self.__stop()
def on_L1_press(self):
self.__reset_speed
self.left_motors.backward(self.speed_left)
self.right_motors.forward(self.speed_right)
def on_L1_release(self):
self.__stop()
controller = RobotController(
interface="/dev/input/js0", connecting_using_ds4drv=False)
controller.listen(timeout=60)