-
Notifications
You must be signed in to change notification settings - Fork 0
/
submarine.py
72 lines (51 loc) · 1.86 KB
/
submarine.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
import seamoth
def main():
camera = seamoth.Camera()
conn = seamoth.DataConnection()
controllerValues = seamoth.ControllerValues()
LU = seamoth.Motor()
LF = seamoth.Motor()
RU = seamoth.Motor()
RF = seamoth.Motor()
Claw = seamoth.Servo()
LU.setMotor("LU")
LF.setMotor("LF")
RU.setMotor("RU")
RF.setMotor("RF")
Claw.setServo("Claw")
conn.clientStart("0.0.0.0", 1951)
forward_speed = .75
qual = 70
basic_movement = False
while True:
conn.send(seamoth.Camera.encode(camera.readCameraData()), qual)
if conn.output[0] == 12:
controllerValues = seamoth.ControllerValues.fromString(conn.output[1].decode('utf-8'))
if conn.output[0] == 13:
qual = int(conn.output[1].decode('utf-8'))
if controllerValues.A > 0.5:
basic_movement = False
if basic_movement:
left = controllerValues.LeftJoystickY
right = controllerValues.LeftJoystickY
if controllerValues.LeftJoystickX > 0.5:
right = -right
if controllerValues.LeftJoystickX < -0.5:
left = -left
LF.setSpeed(left * forward_speed)
RF.setSpeed(right * forward_speed)
else:
X = -controllerValues.LeftJoystickX * 100
Y = controllerValues.LeftJoystickY * 100
V = (100 - abs(X)) * (Y / 100) + Y
W = (100 - abs(Y)) * (X / 100) + X
R = (V + W) / 2
L = (V - W) / 2
LF.setSpeed(L / 100 * forward_speed)
RF.setSpeed(R / 100 * forward_speed)
LU.setSpeed(controllerValues.RightJoystickY)
RU.setSpeed(controllerValues.RightJoystickY)
Claw.setPosition(controllerValues.LeftTrigger)
conn.sendTelemetry("claw", controllerValues.LeftTrigger)
if __name__ == "__main__":
main()