-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathtorpedoes.py
119 lines (91 loc) · 3.44 KB
/
torpedoes.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
118
119
import pigpio
import time
from logpy.LogPy import Logger
from ports import TORPEDO_FIRE_DRIVER_PORT, TORPEDO_READY_CLIENT_PORT
from rov_comm import Client
PWM_serwo = 23
reed_switch = 24
trigger = 25
en = 26
DELAY_AFTER_TRIGGER = 0.05 # delay between high and low state on trigger output in seconds
TIME_OF_INITIAL_ROTATION = 0.5 # time required to make one full rotation with max angle speed in seconds
FAST_ROTATION = 230 # you add/sub this to/from the neutral position
SLOW_ROTATION = 70 # same as above
NEUTRAL = 458
class TorpedoHandling:
def __init__(self):
self.pi = pigpio.pi()
self.logger = Logger(filename='torpedoes_handling')
if not self.pi.connected:
self.logger.log("pi not connected", logtype='error')
exit()
self.logger.log("setup")
self.setup()
def setup(self):
self.pi.set_mode(reed_switch, pigpio.INPUT)
self.pi.set_mode(trigger, pigpio.OUTPUT)
self.pi.set_mode(en, pigpio.OUTPUT)
self.pi.write(en, 1)
self.pi.set_PWM_frequency(PWM_serwo, 330)
self.pi.set_PWM_range(PWM_serwo, 1000) # 270 max_cw, 730 max_ccw, 500 neutral
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
def initial_rotation(self):
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL + FAST_ROTATION)
time.sleep(TIME_OF_INITIAL_ROTATION)
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
def position(self):
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL + FAST_ROTATION)
while self.pi.read(reed_switch) == 0:
pass
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL + SLOW_ROTATION)
while self.pi.read(reed_switch) == 1:
pass
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL - SLOW_ROTATION)
while self.pi.read(reed_switch) == 0:
pass
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
def fire(self):
self.pi.write(trigger, 1)
time.sleep(DELAY_AFTER_TRIGGER)
self.pi.write(trigger, 0)
def stop(self):
self.pi.set_PWM_dutycycle(PWM_serwo, NEUTRAL)
self.pi.write(en, 0)
def sequence(self):
#self.pi.set_PWM_dutycycle(PWM_serwo, 300)
self.logger.log("initial rotation")
self.initial_rotation() # TODO move to __init__
self.logger.log("position")
self.position()
self.logger.log("fire")
self.fire()
self.logger.log("stop")
self.stop()
self.logger.log("koniec")
self.pi.set_PWM_dutycycle(PWM_serwo, 0)
self.pi.stop()
class Torpedoes:
is_ready = True
def __init__(self):
self.client_fire = Client(TORPEDO_FIRE_DRIVER_PORT)
self.client_ready = Client(TORPEDO_READY_CLIENT_PORT)
self.torpedo_handling = TorpedoHandling()
def fire(self):
command = self.client_fire.get_data()
#print(command)
if command == "FIRE" and self.is_ready:
self.torpedo_handling.sequence()
self.client_ready.send_data('WAIT')
self.is_ready = False
self.reload_torpedo()
def reload_torpedo(self):
reload_time = 2 # czas przeładowywania, do ustawienia
"""
TUTAJ PRZEŁADOWANIE
"""
time.sleep(reload_time) # jak umiecie jakieś lepsze czekanie, to zmieńcie
self.client_ready.send_data('READY')
self.is_ready = True
if __name__ == "__main__":
HANDLING = TorpedoHandling()
HANDLING.sequence()