hovercraft/main.py at main · beaver-auv/hovercraft · GitHub

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 numpy as np
import quaternion

from ezauv.auv import AUV
from ezauv.hardware import MotorController, Motor, SensorInterface
from ezauv.utils.inertia import InertiaBuilder, Cuboid
from ezauv.mission.tasks.main import AccelerateVector
from ezauv.mission.tasks.subtasks import HeadingPID
from ezauv.mission import Path

from hardware_interface import HovercraftHardware
from hover_task import Hover


motor_locations = [
    np.array([-1., -1., 0.]),
    np.array([-1., 1., 0.]),
    np.array([1., 1., 0.]),
    np.array([1., -1., 0.])
    ]

motor_directions = [
    np.array([1., -1., 0.]),
    np.array([1., 1., 0.]),
    np.array([1., -1., 0.]),
    np.array([1., 1., 0.])
    ]


bounds = [[-0.2, 0.2]] * 4
deadzone = [[-0.11, 0.11]] * 4

hardware = HovercraftHardware(
    arduino_port='/dev/ttyUSB0',
    vectornav_port='/dev/ttyUSB1'
)

anchovy = AUV(
    motor_controller = MotorController(
        inertia = InertiaBuilder(
            Cuboid(
                mass=1,
                width=1,
                height=1,
                depth=0.1,
                center=np.array([0,0,0])
            )).moment_of_inertia(),
            motors = [
                Motor(
                    direction,
                    loc,
                    lambda magnitude, i=i: hardware.set_motor(i+2, magnitude),
                    lambda i=i: hardware.initialize_motor(i+2),
                    Motor.Range(bounds[i][0], bounds[i][1]),
                    Motor.Range(-deadzone[i][0], deadzone[i][1])
                    )
                for i, (loc, direction) in enumerate(zip(motor_locations, motor_directions))
                ]
        ),
        sensors = SensorInterface(imu=hardware.imu, depth=hardware.depth),
        pin_kill = hardware.kill
    )

anchovy.register_subtask(HeadingPID(0, 0.03, 0.0, 0.01))
anchovy.register_subtask(Hover(0.8))

mission = Path(
    AccelerateVector(np.array([0., 0., 0., 0., 0., 0.]), 20)
)

anchovy.travel_path(mission)