backup/backup.py at main · ash-yang123/backup · 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
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
120
121
122
123
124
import time
import numpy as np
from motors import directions
import serial
import vnpy

# def up(speed):
#   return f'{speed},6:{speed},7:{speed},4:{speed},5:'


def forward(speed):
    return f"{speed},{directions.get('HFL')}:{speed},{directions.get('HFR')}:{speed},{directions.get('HBL')}:{speed},{directions.get('HBR')}:"


def left(speed):
    return f"{speed},{directions.get('HFR')}:{-speed},{directions.get('HBL')}:"


def right(speed):
    return f"{speed},{directions.get('HFL')}:{-speed},{directions.get('HBR')}:"


class PID:
    def __init__(self, Kp, Ki, Kd, wanted_direction):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd

        self.wanted_direction = wanted_direction
        self.integral_error = 0
        self.previous_error = 0
        self.previous_time = time.time()

    def signal(self, current):
        time_diff = time.time() - self.previous_time
        error = self.wanted_direction - current
        self.integral_error += error
        derivative = (error - self.prev) / time_diff

        self.previous_error = error
        signal = (
            (self.Kp * error) + (self.Ki * self.integral_error) + (self.Kd * derivative)
        )

        return signal


class HeadingPID:
    def __init__(self, wanted_heading, Kp, Ki, Kd):
        self.pid = PID(Kp, Ki, Kd, 0)
        self.wanted = wanted_heading

    def update(self, sensors):
        q = sensors.imu.get_rotation()
        v_quat = q * np.quaternion(0, 0.6, 0.0, 0.1) * q.conjugate()

        v_prime = np.array([v_quat.x, v_quat.y, v_quat.z])

        yaw = np.degrees(np.arctan2(v_prime[1], v_prime[0]))

        diff = self.wanted - yaw

        if abs(diff) >= 180:
            sign = yaw / abs(yaw)
            abs_diff_yaw = 180 - abs(yaw)
            abs_diff_target = 180 - abs(self.wanted)
            diff = sign * (abs_diff_yaw + abs_diff_target)

        signal = self.pid.signal(-diff)
        return np.array([0.0, 0.0, 0.0, 0.0, 0.0, signal])


class VectorNavIMU:
    def __init__(self, port, baud):
        self.imu = vnpy.VnSensor()
        self.imu.connect(port, baud)

    def get_heading(self):
        return self.imu.read_yaw_pitch_roll().x

    def angle_to(self, target):
        yaw = self.get_heading()
        diff = target - yaw

        if abs(target - yaw) >= 180:
            sign = yaw / abs(yaw)
            abs_diff_yaw = 180 - abs(yaw)
            abs_diff_target = 180 - abs(target)
            diff = sign * (abs_diff_yaw + abs_diff_target)
        return diff

    def get_average_heading(self, sample_count, interval=0.1):
        samples = []
        for i in range(sample_count):
            samples.append(self.get_heading())
            time.sleep(interval)
        return sum(samples) / sample_count


wanted_speed = 0.5
heading_PID = HeadingPID(0.00, 0.05, 0.00, 0.03)


sensor = VectorNavIMU("/dev/ttyUSB1", 9600)
port = serial.Serial("/dev/ttyUSB1", 9600)

try:
    time0 = time.time()
    while time.time() - time0 < 15:
        time.sleep(0.1)
        port.write(forward(wanted_speed)).encode("utf-8")
        update = heading_PID.update(sensor)
        if abs(update) > 1:
            update = abs(update) / update
        if update > 0:
            port.write(right(update).encode("utf-8"))
            print(f"sent {right(update).encode('utf-8')}")
        if update < 0:
            port.write(left(update).encode("utf-8"))
            print(f"sent {left(update).encode('utf-8')}")
except:
    print("Stopping...")
finally:
    port.write((forward(0)).encode("utf-8"))