-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathExcavator.py
108 lines (88 loc) · 3.6 KB
/
Excavator.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
from AMSpi import AMSpi
import time
class Excavator:
LEFT_CHAIN_MOTOR = AMSpi.DC_Motor_1
RIGHT_CHAIN_MOTOR = AMSpi.DC_Motor_2
BODY_MOTOR = AMSpi.DC_Motor_3
SHOVEL_MOTOR = AMSpi.DC_Motor_4
def __init__(self):
self.motors = AMSpi()
# Set PINs for controlling shift register (GPIO numbering)
self.motors.set_74HC595_pins(21, 20, 16)
# Set PINs for controlling all 4 motors (GPIO numbering)
self.motors.set_L293D_pins(5, 6, 13, 19)
self.motors_memo = []
self.body_angle = 0
self.shovel_angle = 0
def __enter__(self):
return self
def __exit__(self, exc_type, exc_val, exc_tb):
self.motors.clean_up()
def execute(self, run_time=1):
time.sleep(run_time)
for motor in self.motors_memo:
self.motors.stop_dc_motor(motor)
self.motors_memo = []
time.sleep(1)
def forward_left_chain(self, speed=100):
self.motors_memo.append(self.LEFT_CHAIN_MOTOR)
self.motors.run_dc_motor(self.LEFT_CHAIN_MOTOR,
clockwise=True,
speed=speed)
def backward_left_chain(self, speed=100):
self.motors_memo.append(self.LEFT_CHAIN_MOTOR)
self.motors.run_dc_motor(self.LEFT_CHAIN_MOTOR,
clockwise=False,
speed=speed)
def forward_right_chain(self, speed=100):
self.motors_memo.append(self.RIGHT_CHAIN_MOTOR)
self.motors.run_dc_motor(self.RIGHT_CHAIN_MOTOR,
clockwise=True,
speed=speed)
def backward_right_chain(self, speed=100):
self.motors_memo.append(self.RIGHT_CHAIN_MOTOR)
self.motors.run_dc_motor(self.RIGHT_CHAIN_MOTOR,
clockwise=False,
speed=speed)
def turn_left_body(self, speed=100):
self.motors_memo.append(self.BODY_MOTOR)
self.motors.run_dc_motor(self.BODY_MOTOR, clockwise=True, speed=speed)
def turn_right_body(self, speed=100):
self.motors_memo.append(self.BODY_MOTOR)
self.motors.run_dc_motor(self.BODY_MOTOR, clockwise=False, speed=speed)
def move_up_shovel(self, speed=100):
self.motors_memo.append(self.SHOVEL_MOTOR)
self.motors.run_dc_motor(self.SHOVEL_MOTOR,
clockwise=True,
speed=speed)
def move_down_shovel(self, speed=100):
self.motors_memo.append(self.SHOVEL_MOTOR)
self.motors.run_dc_motor(self.SHOVEL_MOTOR,
clockwise=False,
speed=speed)
def move_forward(self, speed=100):
self.backward_left_chain() # TODO: fix hardware issue
self.forward_right_chain(speed)
def move_backward(self, speed=100):
self.forward_left_chain() # TODO: fix hardware issue
self.backward_right_chain(speed)
def stop_all_motors(self):
self.motors.stop_dc_motors(
[self.LEFT_CHAIN_MOTOR, self.RIGHT_CHAIN_MOTOR, self.BODY_MOTOR, self.SHOVEL_MOTOR])
self.motors_memo = []
time.sleep(1)
def test_move(self):
self.forward_left_chain()
self.forward_right_chain()
self.execute(3)
self.backward_left_chain()
self.backward_right_chain()
self.execute(3)
self.turn_left_body()
self.execute(3)
self.turn_right_body()
self.execute(3)
self.move_down_shovel()
self.execute(5)
self.move_up_shovel()
self.execute(5)