-
Notifications
You must be signed in to change notification settings - Fork 1
/
example.py
61 lines (49 loc) · 1.58 KB
/
example.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
from __future__ import print_function
import time
from dual_tb9051ftg_rpi import motors, MAX_SPEED
# Define a custom exception to raise if a fault is detected.
class DriverFault(Exception):
def __init__(self, driver_num):
self.driver_num = driver_num
def raiseIfFault():
if motors.motor1.getFault():
raise DriverFault(1)
if motors.motor2.getFault():
raise DriverFault(2)
# Set up sequences of motor speeds.
test_forward_speeds = list(range(0, MAX_SPEED, 1)) + \
[MAX_SPEED] * 200 + list(range(MAX_SPEED, 0, -1)) + [0]
test_reverse_speeds = list(range(0, -MAX_SPEED, -1)) + \
[-MAX_SPEED] * 200 + list(range(-MAX_SPEED, 0, 1)) + [0]
try:
motors.setSpeeds(0, 0)
print("Motor 1 forward")
for s in test_forward_speeds:
motors.motor1.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
print("Motor 1 reverse")
for s in test_reverse_speeds:
motors.motor1.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
# Disable the drivers for half a second.
motors.disable()
time.sleep(0.5)
motors.enable()
print("Motor 2 forward")
for s in test_forward_speeds:
motors.motor2.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
print("Motor 2 reverse")
for s in test_reverse_speeds:
motors.motor2.setSpeed(s)
raiseIfFault()
time.sleep(0.002)
except DriverFault as e:
print("Driver %s fault!" % e.driver_num)
finally:
# Stop the motors, even if there is an exception
# or the user presses Ctrl+C to kill the process.
motors.forceStop()