-
Notifications
You must be signed in to change notification settings - Fork 0
/
drive_commands.py
executable file
·96 lines (84 loc) · 3.65 KB
/
drive_commands.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
#!/usr/bin/env python
import rospy
from std_msgs.msg import String
class Drive:
def __init__(self, driver1):
self.frontClaw = driver1
self.direction = "stop"
self.speed = 0
self.current_exceeded = False
self.currents = [0, 0, 0, 0, 0, 0]
self.current_threshold = 1000
self.mode = "manual"
def fwd(self):
self.frontClaw.ForwardM1(0x80, self.speed)
self.frontClaw.BackwardM2(0x80, self.speed)
self.frontClaw.ForwardM1(0x81, self.speed)
self.frontClaw.BackwardM2(0x81, self.speed)
self.frontClaw.ForwardM1(0x82, self.speed)
self.frontClaw.BackwardM2(0x82, self.speed)
def bwd(self):
self.frontClaw.BackwardM1(0x80, self.speed)
self.frontClaw.ForwardM2(0x80, self.speed)
self.frontClaw.BackwardM1(0x81, self.speed)
self.frontClaw.ForwardM2(0x81, self.speed)
self.frontClaw.BackwardM1(0x82, self.speed)
self.frontClaw.ForwardM2(0x82, self.speed)
def stop(self):
self.frontClaw.ForwardM1(0x80, 0)
self.frontClaw.ForwardM2(0x80, 0)
self.frontClaw.ForwardM1(0x81, 0)
self.frontClaw.ForwardM2(0x81, 0)
self.frontClaw.ForwardM1(0x82, 0)
self.frontClaw.ForwardM2(0x82, 0)
def right(self):
self.frontClaw.ForwardM1(0x80, self.speed)
self.frontClaw.ForwardM2(0x80, self.speed)
self.frontClaw.ForwardM1(0x81, self.speed)
self.frontClaw.ForwardM2(0x81, self.speed)
self.frontClaw.ForwardM1(0x82, self.speed)
self.frontClaw.ForwardM2(0x82, self.speed)
def left(self):
self.frontClaw.BackwardM1(0x80, self.speed)
self.frontClaw.BackwardM2(0x80, self.speed)
self.frontClaw.BackwardM1(0x81, self.speed)
self.frontClaw.BackwardM2(0x81, self.speed)
self.frontClaw.BackwardM1(0x82, self.speed)
self.frontClaw.BackwardM2(0x82, self.speed)
def update_steer(self):
if self.direction == "stop":
self.stop()
elif self.direction == "forward":
self.fwd()
elif self.direction == "clockwise":
self.right()
elif self.direction == "backward":
self.bwd()
elif self.direction == "anticlockwise":
self.left()
def drive_callback(self, inp):
self.speed, self.direction, self.mode = int(inp.speed), inp.direction, inp.mode
if self.direction == "stop":
print("stop")
elif self.direction == "forward":
rospy.loginfo('Drive: Rover commanded to move Forward')
elif self.direction == "clockwise":
rospy.loginfo('Drive: Rover commanded to turn Clockwise')
elif self.direction == "backward":
rospy.loginfo('Drive: Rover commanded to move Backward')
elif self.direction == "anticlockwise":
rospy.loginfo('Drive: Rover commanded to turn Anti-Clockwise')
def current_limiter(self):
(i, self.currents[0], self.currents[1]) = self.frontClaw.ReadCurrents(0x80)
(i, self.currents[2], self.currents[3]) = self.frontClaw.ReadCurrents(0x80)
(i, self.currents[4], self.currents[5]) = self.frontClaw.ReadCurrents(0x80)
for i in range(6):
if self.currents[i] > self.current_threshold:
self.stop()
self.current_exceeded = True
return
self.current_exceeded = False
def update_current(self):
(i, self.currents[0], self.currents[1]) = self.frontClaw.ReadCurrents(0x80)
(i, self.currents[2], self.currents[3]) = self.frontClaw.ReadCurrents(0x80)
(i, self.currents[4], self.currents[5]) = self.frontClaw.ReadCurrents(0x80)