diff --git a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
index fd39118bde4a5c31f7d1d3f9c4117676820b4c0a..b2deee2c997ed34154930d592863395824f1a9c4 100644
--- a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
+++ b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
@@ -2,7 +2,7 @@ import cv2
from urllib.request import urlopen, Request
import numpy as np
import time
-
+import math
# import apriltag
from pupil_apriltags import Detector
def nothing(x):
@@ -11,16 +11,23 @@ def nothing(x):
# detector = apriltag.Detector()
-detector = Detector()
+
def get_AT_6DOF_info(url):
+ tid,tx,ty,tz= 0,0,0,0
+ rx = [0.0,0.0,0.0]
+ ry = [0.0, 0.0, 0.0]
+ rz = [0.0, 0.0, 0.0]
header = {
"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
req = Request(url, headers=header)
img_resp = urlopen(req, timeout=60)
imgnp = np.array(bytearray(img_resp.read()), dtype=np.uint8)
frame = cv2.imdecode(imgnp, -1)
+
+ # ret,frame = cap.read()
gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
h, w, _ = frame.shape
# put a dot in center of the frame
cv2.circle(frame, (w // 2, h // 2), 7, (255, 0, 0), -1)
@@ -29,7 +36,8 @@ def get_AT_6DOF_info(url):
fy = 600
cx = 400
cy = 300
- results = detector.detect(gray_image, estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=0.16)
+ AT_size = 0.16
+ results = detector.detect(gray_image, estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=AT_size)
debug_print = 0
for r in results:
# extract the bounding box (x, y)-coordinates for the AprilTag
@@ -41,6 +49,7 @@ def get_AT_6DOF_info(url):
ptA = (int(ptA[0]), int(ptA[1]))
tx, ty, tz = r.pose_t # in meters
rx,ry,rz = r.pose_R
+ tid = r.tag_id
# draw the bounding box of the AprilTag detection
cv2.line(frame, ptA, ptB, (0, 255, 0), 5)
@@ -55,7 +64,8 @@ def get_AT_6DOF_info(url):
tagFamily = r.tag_family.decode("utf-8")
- tid = r.tag_id
+
+
cv2.putText(frame, tagFamily, (ptA[0], ptA[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.putText(frame, "tx: {:.2f} ty: {:.2f} tz:{:.2f}".format(tx[0], ty[0], tz[0]), (ptA[0], ptA[1] + 30),
@@ -71,20 +81,35 @@ def get_AT_6DOF_info(url):
return tid,tx,ty,tz,rx,ry,rz
+
+def rotationMatrixToEulerAngles(rx,ry,rz) :
+
+ sy = math.sqrt(rx[0] * rx[0] + ry[0] * ry[0])
+ roll = math.atan2(rz[1] , rz[2])
+ pitch = math.atan2(-rz[0], sy)
+ yaw = math.atan2(ry[0], rx[0])
+
+ return np.array([roll, pitch, yaw])
if __name__ == "__main__":
#change the IP address below according to the
#IP shown in the Serial monitor of Arduino code
# url='http://192.168.4.1/cam-hi.jpg'
# url='http://192.168.1.107/cam-hi.jpg'
url='http://192.168.4.1/cam-hi.jpg'
- # url = 'http://192.168.1.118/cam-hi.jpg'
+ url = 'http://192.168.1.118/cam-hi.jpg'
# cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
# cv2.namedWindow("live transmission", cv2.WINDOW_NORMAL)
+ detector = Detector()
+ tid,tx,ty,tz,rx,ry,rz = 0,0,0,0,0,0,0
+ test_webcam = 0
+ if test_webcam == 1:
+ cap = cv2.VideoCapture(0)
while True:
+ """
header = {"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
req = Request(url, headers=header)
img_resp = urlopen(req, timeout=60)
@@ -97,7 +122,7 @@ if __name__ == "__main__":
# put a dot in center of the frame
cv2.circle(frame, (w//2, h//2), 7, (255, 0, 0), -1)
-
+ """
"""
If you also want to extract the tag pose, estimate_tag_pose should be set to True
and camera_params ([fx, fy, cx, cy])
@@ -117,6 +142,7 @@ if __name__ == "__main__":
fx, fy, cx, cy are given in Pixels in Computer Vision ( and openCV)
but e.g. in Photogrammetry you often use mm
"""
+ """
fx = 800
fy = 600
cx = 400
@@ -156,11 +182,24 @@ if __name__ == "__main__":
# show the output image after AprilTag detection
cv2.imshow("Image", frame)
+ """
+ tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url)
+ print("testing new function")
+ print("-----------------------")
+ print("tid:{}".format(tid))
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+ print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+ # R = np.array([rx,ry,rz])
+ roll,pitch, yaw = rotationMatrixToEulerAngles(rx,ry,rz)
+ roll = roll * 180 /math.pi
+ pitch = pitch * 180 / math.pi
+ yaw = yaw * 180 / math.pi
+ print("roll,pitch,yaw:{},{},{}".format(roll, pitch, yaw))
key=cv2.waitKey(5)
if key==ord('q'):
break
-
#cv2.destroyAllWindows()
diff --git a/Code/Control/Laptop_Code/Figs/fig1.png b/Code/Control/Laptop_Code/Figs/fig1.png
new file mode 100644
index 0000000000000000000000000000000000000000..238f4463386fc1ea8118aaa6a50bf04510c41bdc
Binary files /dev/null and b/Code/Control/Laptop_Code/Figs/fig1.png differ
diff --git a/Code/Control/Laptop_Code/README.md b/Code/Control/Laptop_Code/README.md
index a19c8a625086acc4cbcf3c094426f04b6ba1f620..16fa2cbb921756723a618b3451de1b1ebe719301 100644
--- a/Code/Control/Laptop_Code/README.md
+++ b/Code/Control/Laptop_Code/README.md
@@ -1,10 +1,43 @@
-[Progress] The overall logic is clear and tested!
+# Laptop Code
-What to do next in python main:
+## 1 Main structure
+
+## 2 Software Requirements
+
+### Step 1:
+
+Download Anaconda or Pycharm
+
+### Step 2:
+
+- If using Anaconda, then create a virtual environment first:
+
+```shell
+# crreate virtual environment
+conda create -n FORAYenv python=3.7
+# activate virtual environment
+conda activate FORAYenv
+# install environment installation tool pip (if it doesn't have one)
+conda install pip
+```
+
+- If using Pycharm, at the very beginning, create a virtual environment
+
+
+
+### Step 3:
+
+Install dependency
+
+- If using Anaconda, install it from the terminal. And always remember to activate the conda environment fist: `conda activate FORAYenv`
+
+```shell
+# install pyserial
+pip install pyserial
+# install pygame
+pip install pygame
+# install cv2
+pip install opencv-python
+# install
+```
-- Have keyboard interruption in the python function
- - To tweak PID value while running the program
- - To stop the balloon from being crazy
-- The meter version April tag detection is still problematic so we need to tune that
-- Use field test experiment to test PID
--
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/constants.py b/Code/Control/Laptop_Code/constants.py
index 7d4416bf3702150f02d81d81f1b34eb217c43408..336b1d4ad468da7c2d177d72374d8f8fb72d00cf 100644
--- a/Code/Control/Laptop_Code/constants.py
+++ b/Code/Control/Laptop_Code/constants.py
@@ -46,3 +46,7 @@ kpy_g,kiy_g,kdy_g = 1.2, 0.01, 0.5
# difference between center of AT and center of goal
AT_goal_Delta = 110 #cm
+
+AT_h1 = 50 # meters
+
+kph,kih,kdh = 1.2,0.01,0.5
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index 184a14f7cefcb2ca3ab35cf95d25c00ae608d75d..d82613177590319aeebe23d10a0037a787f11bde 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -1,13 +1,17 @@
import time
import serial
-import ball_detection.ball_detection as ball_detection
+
import simple_pid.PID as PID
import timeit
import pygame
from constants import *
+from ESP32_AT.imageTread_AT import get_AT_6DOF_info
global ml
-ml = 1
+ml = 0
+if ml == 1:
+ import ball_detection.ball_detection as ball_detection
+
# ========= Serial Port I/O ===========
def serial_port_in(serial_port):
@@ -118,6 +122,12 @@ def ball_capture(LIDAR_dist):
else:
return False
+def AT_detect(tx,ty):
+ if tx == 0 and ty == 0:
+ return False
+ else:
+ return True
+
def stop_all():
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
@@ -197,17 +207,65 @@ def seeking():
dir1, dir2, dir3, dir4
"""
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+ delta_h = 2
+ url = 'http://192.168.1.118/cam-hi.jpg'
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+
+ AT_detected = AT_detect(tx, ty)
+ count_h = 0
+
+ threshold_h = 0.3
+ if AT_detected:
+ current_h = get_altitude_from_AT(AT_h1,ty)
+ count_h += 1
+ if count_h == 1:
+ set_h = current_h + delta_h # in meter
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+ diff_h = abs(set_h - current_h)
+ if diff_h < threshold_h:
+ new_ssp = 200
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+ else:
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = vertical_control(current_h, set_h)
+ else:
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+def vertical_control(current_h,set_h):
+
+ pid_h = PID(kph, kih, kdh, setpoint = set_h)
+ pid_h.auto_mode = True
+ pid_h.set_auto_mode(True, last_output = 8.0)
+ pid_h.output_limits = (-255,255)
+
+ input_h = current_h
+ output_h = pid_h(input_h)
+
+ pwm1 = abs(output_h)
+ pwm2 = abs(output_h)
+ pwm3 = 0
+ pwm4 = 0
+ dir1 = '+'
+ dir2 = '+'
+ dir3 = '+'
+ dir4 = '+'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
def rotate_one_direction():
pwm1, pwm2, pwm3, pwm4 = 0, 0, seeking_speed, seeking_speed
dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+def rotate_one_direction_ssp(ssp):
+ pwm1, pwm2, pwm3, pwm4 = 0, 0, ssp, ssp
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
def move2ball(gbx, gby, gb_dist):
"""
Description:
@@ -303,13 +361,13 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist, debugM):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty)
else: # Goal not detected
# stop_all() # Debug
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
else: # Ball not captured
if ballDetect: # Ball detected
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2ball(gbx,gby,gb_dist)
else: # Ball not detected
# stop_all() # Debug
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = seeking()
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
@@ -480,6 +538,24 @@ def decode_ctl(Ctl_com):
dir3 = Ctl_com[6]
dir4 = Ctl_com[7]
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
+
+def get_altitude_from_AT(AT_h,ty):
+ # below AT center, ty > 0
+ # above AT center, ty < 0
+ altitude = AT_h - ty
+ return altitude
+
+
+def test_function():
+ url = 'http://192.168.1.118/cam-hi.jpg'
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+ print("testing new function")
+ print("-----------------------")
+ print("tid:{}".format(tid))
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+ print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
@@ -550,6 +626,12 @@ if __name__ == '__main__':
flag = 0
print_count = 1
+ elif get_key('t'):
+ flag = 4
+ while (flag == 4):
+ test_function()
+ flag, print_count = keyboard_stop(flag, print_count)
+
elif get_key('k'):
break