Initial commit

Author: PiscesXD

.DS_Store

@@ -0,0 +1,2 @@
+# Auto detect text files and perform LF normalization
+* text=auto

.gitattributes

@@ -0,0 +1,182 @@
+import serial
+import struct
+import time
+
+class ominibothv:
+    def __init__(self,
+                 port = '/dev/ominibot',
+                 baud = 115200,
+                 divisor_mode = 3,
+                 motor_direct = 2,
+                 encoder_direct = 0,
+                 motor_pwm_max = 5200,
+                 motor_pwm_min = 720,
+                 encoder_ppr = 390,
+                 wheel_space = 110,
+                 axle_space = 0,
+                 gear_ratio = 30,
+                 wheel_diameter = 60,
+                 pos_kp = 3000,
+                 pos_ki = 1050,
+                 pos_kd = 0,
+                 vel_kp = 3000,
+                 vel_ki = 1050):
+        
+        self.ser = serial.Serial(port, baud, timeout = 1)
+
+        self.robot_mode = divisor_mode
+        
+        self.forced_stop()
+        time.sleep(0.5)
+
+        #system setting(motor range: 3v-6v, encoder ppr: 660/4 = 165)
+        sys_set = bytearray(b'\x7b\x23')
+        sys_set += motor_direct.to_bytes(1, byteorder='big')
+        sys_set += encoder_direct.to_bytes(1, byteorder='big')
+        sys_set += motor_pwm_max.to_bytes(2, byteorder='big')
+        sys_set +=  motor_pwm_min.to_bytes(2, byteorder='big')
+        sys_set +=  encoder_ppr.to_bytes(2, byteorder='big')
+        sys_set += bytearray(b'\x00\x00')
+        bcc = self.calculate_bcc(sys_set).to_bytes(1, byteorder='big')
+        sys_set += bcc + bytearray(b'\x7d')
+        self.ser.write(sys_set)
+        time.sleep(0.1)
+
+        # robot size setting(motor dear 1:55)
+        bot_set = bytearray(b'\x7b\x24')
+        bot_set += wheel_space.to_bytes(2, byteorder='big')
+        bot_set += axle_space.to_bytes(2, byteorder='big')
+        bot_set += gear_ratio.to_bytes(2, byteorder='big')
+        bot_set += wheel_diameter.to_bytes(2, byteorder='big')
+        bot_set += bytearray(b'\x00\x00')
+        bcc = self.calculate_bcc(bot_set).to_bytes(1, byteorder='big')
+        bot_set += bcc + bytearray(b'\x7d')
+        self.ser.write(bot_set)
+        time.sleep(0.1)
+        
+        # robot pid setting
+        pid_set = bytearray(b'\x7b\x40')
+        pid_set += pos_kp.to_bytes(2, byteorder='big')
+        pid_set += pos_ki.to_bytes(2, byteorder='big')
+        pid_set += pos_kd.to_bytes(2, byteorder='big')
+        pid_set += vel_kp.to_bytes(2, byteorder='big')
+        pid_set += vel_ki.to_bytes(2, byteorder='big')
+        bcc = self.calculate_bcc(pid_set).to_bytes(1, byteorder='big')
+        pid_set += bcc + bytearray(b'\x7d')
+        self.ser.write(pid_set)
+        time.sleep(0.1)
+    
+    def check_cmd(self, name):
+        if name == 'system':
+            self.ser.write(b'\x7b\x33\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x48\x7d')
+        elif name == 'robot':
+            self.ser.write(b'\x7b\x34\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x4f\x7d')
+        else:
+            self.ser.write(b'\x7b\x50\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x2b\x7d')
+        
+        read_val = self.ser.read(14)
+        
+        return read_val
+        
+    def calculate_bcc(self, data):
+        bcc = 0
+        for byte in data:
+            bcc ^= byte
+        return bcc
+    
+    def clamp_number(self, num , a, b):
+        return max(min(num, max(a, b)), min(a, b))
+    
+    def forced_stop(self):
+        set_motor_go = bytearray(b'\x7b\x25\x00')
+        set_motor_go += self.robot_mode.to_bytes(1, byteorder='big')
+        set_motor_go += bytearray(b'\x00\x00\x00\x00\x00\x00\x00\x00')
+        bcc = self.calculate_bcc(set_motor_go).to_bytes(1, byteorder='big')
+        set_motor_go += bcc + bytearray(b'\x7d')
+        self.ser.write(set_motor_go)
+    
+    def motor_speed(self, m1, m2, m3, m4):
+        set_motor_go = bytearray(b'\x7b\x26\x02')
+        set_motor_go += self.robot_mode.to_bytes(1, byteorder='big')
+        set_motor_go += struct.pack('!i',int(m1*1000))[2:]
+        set_motor_go += struct.pack('!i',int(m2*1000))[2:]
+        set_motor_go += struct.pack('!i',int(m3*1000))[2:]
+        set_motor_go += struct.pack('!i',int(m4*1000))[2:]
+        
+        bcc = self.calculate_bcc(set_motor_go).to_bytes(1, byteorder='big')
+        
+        set_motor_go += bcc + bytearray(b'\x7d')
+        
+        self.ser.write(set_motor_go)
+    
+    def robot_speed(self, lx, ly, az):
+        set_motor_go = bytearray(b'\x7b\x25\x02')
+        set_motor_go += self.robot_mode.to_bytes(1, byteorder='big')
+        set_motor_go += struct.pack('!i',int(lx*1000))[2:]
+        set_motor_go += struct.pack('!i',int(ly*1000))[2:]
+        set_motor_go += struct.pack('!i',int(az*1000))[2:]
+        set_motor_go += bytearray(b'\x00\x00')
+        
+        bcc = self.calculate_bcc(set_motor_go).to_bytes(1, byteorder='big')
+        
+        set_motor_go += bcc + bytearray(b'\x7d')
+        self.ser.write(set_motor_go)
+        
+    
+    def serial_close(self):
+        self.ser.close()
+
+    def serial_write(self, cmd):
+        self.ser.write(cmd)
+    
+    def serial_read(self, choose=None):
+        self.ser.read(choose)
+        
+    def read_robot_data(self):
+        while True:
+            if self.ser.read().hex() == '7b':
+                robot_vel = self.ser.read(7)[1:]
+                imu_val = self.ser.read(20)
+                bat_val = self.ser.read(3)
+                if self.ser.read(1).hex() == '7d':
+                    check_code = bytearray(b'\x7b\x00') + robot_vel + imu_val + bat_val[:2]
+                    bcc = self.calculate_bcc(check_code)
+                    if hex(bcc) == hex(bat_val[2]):
+                        check = 1
+                    else:
+                        check = 0
+                    break
+            
+        return check, robot_vel, imu_val
+    
+import numpy as np
+
+def quaternion_to_euler(q):
+    # 歸一化四元數
+    q = q / np.linalg.norm(q)
+    
+    # 計算歐拉角
+    roll = np.arctan2(2*(q[0]*q[1] + q[2]*q[3]), 1 - 2*(q[1]**2 + q[2]**2))
+    pitch = np.arcsin(2*(q[0]*q[2] - q[3]*q[1]))
+    yaw = np.arctan2(2*(q[0]*q[3] + q[1]*q[2]), 1 - 2*(q[2]**2 + q[3]**2))
+    
+    # 將歐拉角從弧度轉換為角度
+    roll = np.degrees(roll)
+    pitch = np.degrees(pitch)
+    yaw = np.degrees(yaw)
+    
+    return roll, pitch, yaw 
+
+if __name__ == '__main__':
+    import time
+    pi = ominibothv(
+        port = '/dev/ominibot',
+        baud = 115200,
+        divisor_mode = 3)
+    
+    pi.robot_speed(0.2, 0.0, 0.0)
+    time.sleep(10)
+    pi.robot_speed(0.0, 0.0, 0.0)
+
+    
+

OminiBot_HV.py

@@ -0,0 +1,21 @@
+import cv2
+import uuid
+cap = cv2.VideoCapture(0)
+cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M','J','P','G'))
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
+
+while True:
+  ret, frame = cap.read()
+  cv2.imshow('frame', frame)
+  
+  
+  keyin = cv2.waitKey(1) & 0xFF
+
+  if keyin == ord('q'):
+    break
+  elif keyin == ord('s'):
+    cv2.imwrite('{}.jpg'.format(uuid.uuid1()), frame)
+
+cap.release()
+cv2.destroyAllWindows()

image_save.py

@@ -0,0 +1,148 @@
+import cv2
+import numpy as np
+import time
+from OminiBot_HV import ominibothv
+
+def region_of_interest(img, vertices):
+    mask = np.zeros_like(img)
+    cv2.fillPoly(mask, vertices, (255, 255, 255))
+    return cv2.bitwise_and(img, mask)
+
+# hsv color
+low_yellow = np.array([26, 77, 100])
+high_yellow = np.array([34, 255, 255])
+low_white = np.array([0, 0, 221])
+high_white = np.array([180, 15, 255])
+
+cap = cv2.VideoCapture(0)
+cap.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
+cap.set(cv2.CAP_PROP_FRAME_WIDTH, 320)
+cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 240)
+
+robot_control = ominibothv('/dev/ominibot', 115200)
+time.sleep(3)
+
+CENTER_X = 160
+BASE_SPEED = 0.15
+TURN_GAIN = BASE_SPEED / 80.0
+SMOOTH = 0.5
+HALF_LANE = 70            # px offset when following a single wall
+MARGIN = 20               # dead zone around center for left/right classing
+Y0, Y1 = 145, 205         # lookahead scan band
+CORR_MAX = 0.16
+MAX_STEP = 35             # max target move per frame (anti-runaway / anti-jump)
+LOST_STOP = 40
+MIN_AREA = 25             # ignore tiny noise blobs
+
+interest_vertices = [np.array([
+    [0, 240], [0, 200], [80, 120], [240, 120], [320, 200], [320, 240]
+])]
+
+def blob_centroids(band):
+    num, _, stats, cents = cv2.connectedComponentsWithStats(band, connectivity=8)
+    return [cents[i][0] for i in range(1, num) if stats[i, cv2.CC_STAT_AREA] >= MIN_AREA]
+
+show_ok = True
+prev_correction = 0.0
+prev_target = CENTER_X
+lost = 0
+
+try:
+    while True:
+        ret, frame = cap.read()
+        if not ret or frame is None:
+            robot_control.motor_speed(0.0, 0.0, 0.0, 0.0)
+            continue
+
+        cropped = region_of_interest(frame.copy(), interest_vertices)
+        hsv = cv2.cvtColor(cropped, cv2.COLOR_BGR2HSV)
+        yellow_mask = cv2.inRange(hsv, low_yellow, high_yellow)
+        white_mask = cv2.inRange(hsv, low_white, high_white)
+
+        # ---- yellow walls: classify blobs by side, take the inner one each side
+        yblobs = blob_centroids(yellow_mask[Y0:Y1, :])
+        left_side = [x for x in yblobs if x < CENTER_X - MARGIN]
+        right_side = [x for x in yblobs if x > CENTER_X + MARGIN]
+        left_wall = max(left_side) if left_side else None       # rightmost left blob
+        right_wall = min(right_side) if right_side else None     # leftmost right blob
+
+        # ---- white center line: the white blob nearest to image center
+        wblobs = blob_centroids(white_mask[Y0:Y1, :])
+        white_x = min(wblobs, key=lambda x: abs(x - CENTER_X)) if wblobs else None
+
+        # ---- choose target ----
+        if left_wall is not None and right_wall is not None:
+            target_x = int((left_wall + right_wall) / 2)
+            src = "walls"
+        elif white_x is not None:
+            target_x = int(white_x)
+            src = "white"
+        elif left_wall is not None:
+            target_x = int(left_wall + HALF_LANE)
+            src = "Lwall"
+        elif right_wall is not None:
+            target_x = int(right_wall - HALF_LANE)
+            src = "Rwall"
+        else:
+            target_x = prev_target
+            src = "hold"
+
+        have_signal = (left_wall is not None) or (right_wall is not None) or (white_x is not None)
+        lost = 0 if have_signal else lost + 1
+
+        # rate limit: target can only move so far per frame
+        target_x = max(prev_target - MAX_STEP, min(prev_target + MAX_STEP, target_x))
+        target_x = max(0, min(319, target_x))
+        prev_target = target_x
+        target_y = (Y0 + Y1) // 2
+
+        # ---- steering ----
+        error = target_x - CENTER_X
+        correction = error * TURN_GAIN
+        correction = SMOOTH * correction + (1 - SMOOTH) * prev_correction
+        correction = max(min(correction, CORR_MAX), -CORR_MAX)
+        prev_correction = correction
+
+        forward = max(0.07, BASE_SPEED - 0.6 * abs(correction))
+        if lost > LOST_STOP:
+            forward = 0.0
+            correction = 0.0
+            src = "STOP"
+
+        robot_speed_l = max(min(forward + correction, 0.30), -0.12)
+        robot_speed_r = max(min(forward - correction, 0.30), -0.12)
+
+        print("{:>5} L:{} R:{} w:{} tgt:{:>3} corr:{:+.3f} l:{:.2f} r:{:.2f}".format(
+            src, None if left_wall is None else int(left_wall),
+            None if right_wall is None else int(right_wall),
+            white_x if white_x is None else int(white_x),
+            target_x, correction, robot_speed_l, robot_speed_r))
+
+        robot_control.motor_speed(robot_speed_l * -1, robot_speed_r, 0.0, 0.0)
+
+        # ---- visualization ----
+        cv2.rectangle(frame, (0, Y0), (319, Y1), (80, 80, 80), 1)
+        if left_wall is not None:
+            cv2.circle(frame, (int(left_wall), target_y), 6, (255, 0, 0), -1)
+        if right_wall is not None:
+            cv2.circle(frame, (int(right_wall), target_y), 6, (255, 0, 0), -1)
+        if white_x is not None:
+            cv2.circle(frame, (int(white_x), target_y), 6, (0, 255, 0), -1)
+        cv2.line(frame, (CENTER_X, 0), (CENTER_X, 240), (0, 0, 255), 1)
+        cv2.arrowedLine(frame, (CENTER_X, 240), (target_x, target_y), (0, 255, 255), 3, tipLength=0.3)
+        cv2.circle(frame, (target_x, target_y), 6, (255, 0, 255), -1)
+        cv2.imwrite('debug_live.jpg', frame)
+        if show_ok:
+            try:
+                cv2.imshow('frame', frame)
+                if (cv2.waitKey(1) & 0xFF) == ord('q'):
+                    break
+            except cv2.error:
+                show_ok = False
+
+except KeyboardInterrupt:
+    pass
+finally:
+    robot_control.motor_speed(0.0, 0.0, 0.0, 0.0)
+    cap.release()
+    cv2.destroyAllWindows()