diff --git a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
index f17cf50b98ea65da0647eafb4182d8a98671f471..0c8ef6bdcc136bbe18da1ef89a356d6402391cfe 100644
--- a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
+++ b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
@@ -3,7 +3,9 @@ from urllib.request import urlopen, Request
import numpy as np
import time
import math
+from math import sin,atan2,cos,pi,atan,sqrt
# import apriltag
+import sys
from pupil_apriltags import Detector
def nothing(x):
pass
@@ -18,6 +20,7 @@ def get_AT_6DOF_info(url):
rx = [0.0,0.0,0.0]
ry = [0.0, 0.0, 0.0]
rz = [0.0, 0.0, 0.0]
+ pose_r = np.array([[0,0,0],[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)
@@ -50,6 +53,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
+ pose_r = r.pose_R
tid = r.tag_id
# draw the bounding box of the AprilTag detection
@@ -82,15 +86,159 @@ def get_AT_6DOF_info(url):
return tid,tx,ty,tz,rx,ry,rz
+def get_AT_6DOF_info_posR(url):
+ tid,tx,ty,tz= 0,100000,100000,0
+ rx = [0.0,0.0,0.0]
+ ry = [0.0, 0.0, 0.0]
+ rz = [0.0, 0.0, 0.0]
+ pose_r = np.array([[0,0,0],[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)
-def rotationMatrixToEulerAngles(rx,ry,rz) :
+ # ret,frame = cap.read()
+ gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- 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])
+ h, w, _ = frame.shape
+ # put a dot in center of the frame
+ cv2.circle(frame, (w // 2, h // 2), 7, (255, 0, 0), -1)
+ # set camera parameters
+ fx = 800
+ fy = 600
+ cx = 400
+ cy = 300
+ #AT_size = 0.16
+ AT_size = 0.74
+ 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
+ # and convert each of the (x, y)-coordinate pairs to integers
+ (ptA, ptB, ptC, ptD) = r.corners
+ ptB = (int(ptB[0]), int(ptB[1]))
+ ptC = (int(ptC[0]), int(ptC[1]))
+ ptD = (int(ptD[0]), int(ptD[1]))
+ ptA = (int(ptA[0]), int(ptA[1]))
+ tx, ty, tz = r.pose_t # in meters
+ rx,ry,rz = r.pose_R
+ pose_r = r.pose_R
+ tid = r.tag_id
+
+ # draw the bounding box of the AprilTag detection
+ cv2.line(frame, ptA, ptB, (0, 255, 0), 5)
+ cv2.line(frame, ptB, ptC, (0, 255, 0), 5)
+ cv2.line(frame, ptC, ptD, (0, 255, 0), 5)
+ cv2.line(frame, ptD, ptA, (0, 255, 0), 5)
+ # draw the center (x, y)-coordinates of the AprilTag
+ (cX, cY) = (int(r.center[0]), int(r.center[1]))
+ cv2.circle(frame, (cX, cY), 5, (0, 0, 255), -1)
+
+ # draw the tag family on the image
+
+
+ tagFamily = r.tag_family.decode("utf-8")
+
+
+ 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),
+ cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+
+ if debug_print == 1:
+ print("tx,ty,tz:{},{},{}".format(tx, ty, tz))
+ print("cX,cY:{},{}".format(cX, cY))
+ print("[INFO] tag id: {}".format(tid))
+
+ # show the output image after AprilTag detection
+ cv2.imshow("Image", frame)
+
+ return tid,tx,ty,tz,rx,ry,rz,pose_r
+
+
+def rotationM2Euler(rx,ry,rz) :
+ R_00 = rx[0]
+ R_01 = rx[1]
+ R_02 = rx[2]
+ R_10 = ry[0]
+ R_11 = ry[1]
+ R_12 = ry[2]
+ R_20 = rz[0]
+ R_21 = rz[1]
+ R_22 = rz[2]
+
+ sy = math.sqrt(R_00 * R_00 + R_10 * R_10)
+ singular = sy < 1e-6
+ if not singular:
+ x = math.atan2(R_21, R_22)
+ y = math.atan2(-R_20, sy)
+ z = math.atan2(R_10, R_00)
+ else:
+ x = math.atan2(-R_12, R_11)
+ y = math.atan2(-R_20, sy)
+ z = 0
+
+ print("phi =", np.rad2deg(x))
+ print("theta =", np.rad2deg(y))
+ print("psi =", np.rad2deg(z))
+
+ """
+ tol = sys.float_info.epsilon * 10
+
+ if abs(R_00) < tol and abs(R_10) < tol:
+ eul1 = 0
+ eul2 = atan2(-R_20, R_00)
+ eul3 = atan2(-R_12, R_11)
+ else:
+ eul1 = atan2(R_10, R_00)
+ sp = sin(eul1)
+ cp = cos(eul1)
+ eul2 = atan2(-R_20, cp * R_00 + sp * R_10)
+ eul3 = atan2(sp * R_02 - cp * R_12, cp * R_11 - sp * R_01)
+
+ print("phi =", np.rad2deg(eul1))
+ print("theta =", np.rad2deg(eul2))
+ print("psi =", np.rad2deg(eul3))
+ """
+
+
+def rotationMatrixToEulerAngles(R) :
+ assert(isRotationMatrix(R))
+ sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
+ singular = sy < 1e-6
+ if not singular :
+ x = math.atan2(R[2,1] , R[2,2])
+ y = math.atan2(-R[2,0], sy)
+ z = math.atan2(R[1,0], R[0,0])
+ else :
+ x = math.atan2(-R[1,2], R[1,1])
+ y = math.atan2(-R[2,0], sy)
+ z = 0
+ print("R_phi =", np.rad2deg(x))
+ print("R_theta =", np.rad2deg(y))
+ print("R_psi =", np.rad2deg(z))
+
+def R2EA(pose_r):
+ r11 = pose_r[0][0]
+ r12 = pose_r[0][1]
+ r13 = pose_r[0][2]
+ r21 = pose_r[1][0]
+ r22 = pose_r[1][1]
+ r23 = pose_r[1][2]
+ r31 = pose_r[2][0]
+ r32 = pose_r[2][1]
+ r33 = pose_r[2][2]
+
+ AprilTagPitch = round(np.degrees(atan(-r31 / sqrt((r32 * r32) + (r33 * r33)))), 3)
+ AprilTagRoll = round(np.degrees(atan(-r32 / r33)), 3)
+ ApriTagYaw = round(np.degrees(atan(r21 / r11)), 3)
+
+ print("pitch =", AprilTagPitch)
+ print("roll =", AprilTagRoll)
+ print("yaw =", ApriTagYaw )
- 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
@@ -101,8 +249,7 @@ if __name__ == "__main__":
url = 'http://10.0.0.3/cam-hi.jpg' #03
url = 'http://10.0.0.5/cam-hi.jpg' #01
url = 'http://10.0.0.6/cam-hi.jpg' #02
- url = 'http://10.0.0.7/cam-hi.jpg' #06
-
+ url = 'http://10.0.0.9/cam-hi.jpg' # 6
# cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
@@ -115,94 +262,30 @@ if __name__ == "__main__":
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)
- imgnp=np.array(bytearray(img_resp.read()),dtype=np.uint8)
- frame=cv2.imdecode(imgnp,-1)
-
- gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- #print(len(gray_image.shape))
- h,w,_ = frame.shape
-
- # 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])
- and tag_size (in meters) should be supplied.
- The detect method returns a list of Detection objects each having
- the following attributes
- (note that the ones with an asterisks are computed only if estimate_tag_pose=True):
- """
-
- """
- So fx and fy are the focal lengths expressed in pixels.
- Cx and Cy describe the coordinates of the so called principal
- point that should be in the center of the image.
- It is e.g. not in the center of the image if you cropped the image,
- what you should never do when calibrating.
-
- 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
- cy = 300
- results = detector.detect(gray_image,estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=0.16)
-
- # loop over the AprilTag detection results
- for r in results:
- # extract the bounding box (x, y)-coordinates for the AprilTag
- # and convert each of the (x, y)-coordinate pairs to integers
- (ptA, ptB, ptC, ptD) = r.corners
- ptB = (int(ptB[0]), int(ptB[1]))
- ptC = (int(ptC[0]), int(ptC[1]))
- ptD = (int(ptD[0]), int(ptD[1]))
- ptA = (int(ptA[0]), int(ptA[1]))
- tx,ty,tz = r.pose_t
- print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
- # draw the bounding box of the AprilTag detection
- cv2.line(frame, ptA, ptB, (0, 255, 0), 5)
- cv2.line(frame, ptB, ptC, (0, 255, 0), 5)
- cv2.line(frame, ptC, ptD, (0, 255, 0), 5)
- cv2.line(frame, ptD, ptA, (0, 255, 0), 5)
- # draw the center (x, y)-coordinates of the AprilTag
- (cX, cY) = (int(r.center[0]), int(r.center[1]))
- cv2.circle(frame, (cX, cY), 5, (0, 0, 255), -1)
- # draw the tag family on the image
- print("cX,cY:{},{}".format(cX,cY))
-
-
- 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), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
- print("[INFO] tag id: {}".format(tid))
-
- # show the output image after AprilTag detection
- cv2.imshow("Image", frame)
- """
- tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url)
+ tid,tx,ty,tz,rx,ry,rz,R = 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])
+
+ print("rotation matrix:{}".format(R))
+ R2EA(R)
+
+ # print("R00,R10,R20:{},{},{}".format(rx[0], ry[0], rz[0]))
+ # rotationM2Euler(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))
+ """
+ # rotationMatrixToEulerAngles(R)
key=cv2.waitKey(5)
if key==ord('q'):
diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index f79c97c83061e6cf1f75ad7859c7af98c83b4f03..ccf46dc1f3fe91a849c426a2c652c46a6bc63d23 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -1,12 +1,19 @@
#include <esp_now.h>
#include <WiFi.h>
#include <Wire.h>
-#include <SparkFun_VL53L1X.h>
+#include <VL53L1X.h>
#include <Arduino.h>
#include "Camera.h"
#include "utilities.h"
#include <Adafruit_MotorShield.h>
+
+// For LIDAR 1 and 2
+#define XSHUT_pin1 A7
+#define XSHUT_pin2 A6
+
+#define Sensor1_newAddress 43
+#define Sensor2_newAddress 45
// REPLACE WITH THE MAC Address of your receiver (MASTER)
// Slave: 40:F5:20:44:B6:4C
@@ -35,9 +42,10 @@ String Rec_dir4;
int count = 0;
int count_var = 0;
int print_count = 0;
-int change_count =0;
-int Lidar_flag = 0;
-
+int change_count =0;
+int Lidar1_flag = 0;
+int Lidar2_flag = 0;
+
// Define variables to be sent;
int Sent_tx = 0;
int Sent_ty = 0;
@@ -46,12 +54,17 @@ int Sent_rx = 0;
int Sent_ry = 0;
int Sent_rz = 0;
int Sent_dist = 0;
+int Sent_dist1 = 0;
+int Sent_dist2 = 0;
int8_t g1 = 0,g2=1,g3=2;
int8_t goal_id[3] = {g1, g2, g3};
// Define Lidar variables
-SFEVL53L1X distanceSensor;
+// Define Lidar variables
+VL53L1X Sensor1;
+VL53L1X Sensor2;
+
int budgetIndex = 4;
int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
@@ -68,7 +81,8 @@ typedef struct struct_message {
int Rrx;
int Rry;
int Rrz;
- int Rdist;
+ int Rdist1;
+ int Rdist2;
String DebugM;
int Spwm1;
int Spwm2;
@@ -119,8 +133,8 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
// =============================== All the setup ===========================================
//Create the interface that will be used by the camera
-openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
-openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
+// openmv::rpc_scratch_buffer<256> scratch_buffer; // All RPC objects share this buffer.
+// openmv::rpc_i2c_master interface(0x12, 100000); //to make this more robust, consider making address and rate as constructor argument
//Camera cam(&interface);
// ========================== Motor part ====================================
// Create the motor shield object with the default I2C address
@@ -135,23 +149,46 @@ void setup() {
// Init Serial Monitor
Serial.begin(115200);
Wire.begin();
- interface.begin(); //communication between ESP and OpenMV
+ // interface.begin(); //communication between ESP and OpenMV
AFMS.begin(); // create with the default frequency 1.6KHz
// -------------- LED part --------------------
pinMode(LED, OUTPUT);
digitalWrite(LED, HIGH);
digitalWrite(LED, LOW);
-
- // -------------- lidar part --------------------
- if (distanceSensor.begin() == 0){
- Serial.println("Sensor online!");
- Lidar_flag = 1;
- }else {
- Lidar_flag = 0;
- }
-
- distanceSensor.startRanging();
- distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+
+ // -------------- lidar part --------------------
+ pinMode(XSHUT_pin1, OUTPUT);
+ pinMode(XSHUT_pin2, OUTPUT);
+
+ Wire.setClock(100000);
+
+ pinMode(XSHUT_pin1, INPUT);
+ if (!Sensor1.init()){
+ Serial.println("Failed to detect and initialize sensor1!");
+ Lidar1_flag = 0;
+ } else {
+ Lidar1_flag = 1;
+ delay(10);
+ Sensor1.setAddress(Sensor1_newAddress);
+ Sensor1.setTimeout(500);
+ Sensor1.setDistanceMode(VL53L1X::Medium);
+ Sensor1.setMeasurementTimingBudget(33000);
+ }
+
+
+ pinMode(XSHUT_pin2, INPUT);
+ if (!Sensor2.init())
+ {
+ Serial.println("Failed to detect and initialize sensor2!");
+ Lidar2_flag = 0;
+ } else {
+ Lidar2_flag = 1;
+ delay(10);
+ Sensor2.setAddress(Sensor2_newAddress);
+ Sensor2.setTimeout(500);
+ Sensor2.setDistanceMode(VL53L1X::Medium);
+ Sensor2.setMeasurementTimingBudget(33000);
+ }
// --------------------------- esp now ---------------------------
// Set device as a Wi-Fi Station
@@ -211,11 +248,22 @@ void loop()
Sent_ry = 0;
Sent_rz = 0;
}
- if (Lidar_flag == 1){
- Sent_dist = distanceSensor.getDistance(); // Lidar distance (use the real Lidar data)
+ if (Lidar1_flag == 1){
+ Sensor1.startContinuous(50);
+ Sensor1.read();
+ Sensor1.stopContinuous();
+ Sent_dist1 = Sensor1.ranging_data.range_mm; // Lidar distance (use the real Lidar data)
}else {
- Sent_dist = 0;
- }
+ Sent_dist1 = 0;
+ }
+ if (Lidar2_flag == 1){
+ Sensor2.startContinuous(50);
+ Sensor2.read();
+ Sensor2.stopContinuous();
+ Sent_dist2 = Sensor2.ranging_data.range_mm; // Lidar distance (use the real Lidar data)
+ }else {
+ Sent_dist2 = 0;
+ }
// ========== lidar state info =========
if (Sent_dist < 300 && Sent_dist > 0){
receivedData.DebugM = "found b";
@@ -229,7 +277,8 @@ void loop()
receivedData.Rrx = Sent_rx;
receivedData.Rry = Sent_ry;
receivedData.Rrz = Sent_rz;
- receivedData.Rdist = Sent_dist;
+ receivedData.Rdist1 = Sent_dist1;
+ receivedData.Rdist2 = Sent_dist2;
send_var_once();
print_received_Data();
diff --git a/Code/Control/Laptop_Code/ball_detection/ball_detection.py b/Code/Control/Laptop_Code/ball_detection/ball_detection.py
index 8e8f45f471757e00a417702f16a0127860554b28..ee6c9873287cb955a844ee1f5fb9dfba5d47c564 100644
--- a/Code/Control/Laptop_Code/ball_detection/ball_detection.py
+++ b/Code/Control/Laptop_Code/ball_detection/ball_detection.py
@@ -33,10 +33,10 @@ def modifyCore():
cAddLineNums = [254, 256, 257, 258, 259]
# REPLACABLE LINE FOR DIFFERENT LOCAL COMPUTER DEVICES
- coreFile = 'C:\\Users\\uclal\\.conda\\envs\\foray2\\Lib\\site-packages\\detecto\\core.py'
+ coreFile = 'C:\\Users\\aaronjs\\.conda\\envs\\FORAYenv\\Lib\\site-packages\\detecto\\core.py'
cModLineVals = [" def __init__(self, classes=None, device=None, pretrained=True, modelname=\'fasterrcnn_resnet50_fpn\'):\n",
- " # Load a model pre-trained on COCO - User-Modified",
+ " # Load a model pre-trained on COCO - User-Modified\n",
" self._model = torchvision.models.detection.fasterrcnn_resnet50_fpn(pretrained=pretrained)\n"]
cAddLineVals = [" if modelname == \'fasterrcnn_resnet50_fpn\':\n",
" elif modelname == \'fasterrcnn_mobilenet_v3_large_fpn\':\n",
diff --git a/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino b/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
index 446b2506f8497ee6d91e1e8a121d0b4f0ffc28cb..19c4fbe747a1eb1079353f5fe7550c168944e001 100644
--- a/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
+++ b/Code/Control/Laptop_Code/basic_comm_test/ESP32_master/ESP32_master.ino
@@ -9,8 +9,8 @@
//uint8_t broadcastAddress[] = {0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}; // #3
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xC0, 0xD8}; // #5
-//uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
-uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
+uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xDE, 0xE0}; // #6
+//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x4A, 0xD3, 0x3C}; // #7
//uint8_t broadcastAddress[] = {0x3C, 0x61, 0x05, 0x9B, 0x04, 0x98}; // #8
// ==================================== global data =================================================
String success;
@@ -29,7 +29,8 @@ typedef struct struct_message {
int Rrx;
int Rry;
int Rrz;
- int Rdist;
+ int Rdist1;
+ int Rdist2;
String DebugM;
int Spwm1;
int Spwm2;
@@ -70,7 +71,8 @@ void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
Serial.println(sentData.Rrx);
Serial.println(sentData.Rry);
Serial.println(sentData.Rrz);
- Serial.println(sentData.Rdist);
+ Serial.println(sentData.Rdist1);
+ Serial.println(sentData.Rdist2);
Serial.println(sentData.DebugM);
}
// =================================== setup ==================================================
diff --git a/Code/Control/Laptop_Code/constants.py b/Code/Control/Laptop_Code/constants.py
index add5d84528a380888d352f57cffcb3030d33dd1a..4bf80a33dc3d73df3ba1b13f4423b82830ae9aae 100644
--- a/Code/Control/Laptop_Code/constants.py
+++ b/Code/Control/Laptop_Code/constants.py
@@ -59,5 +59,4 @@ kph,kih,kdh = 1.2,0.01,0.5
AT_detectBreak = 60
-
-url_gb = 'http://10.0.0.5/cam-hi.jpg' #01
+url_gb = 'http://10.0.0.4/cam-hi.jpg' # 6
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/imageThread_AT_gb_1_6.py b/Code/Control/Laptop_Code/imageThread_AT_gb_1_6.py
index c38b498ce6ce7282a558f7cdc51a4b1f1c9933fa..69853142d45e29164e24affea9288054418c6eda 100644
--- a/Code/Control/Laptop_Code/imageThread_AT_gb_1_6.py
+++ b/Code/Control/Laptop_Code/imageThread_AT_gb_1_6.py
@@ -11,8 +11,8 @@ import cv2
if __name__ == "__main__":
- url_AT = 'http://10.0.0.9/cam-hi.jpg' #6
- url_gb = 'http://10.0.0.5/cam-hi.jpg' #1
+ url_AT = 'http://10.0.0.5/cam-hi.jpg' #1
+ url_gb = 'http://10.0.0.9/cam-hi.jpg' #6
tid,tx,ty,tz,rx,ry,rz = 0,0,0,0,0,0,0
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index d1436398643b6c480532b27dc32979af53441a32..2911a2a52615c3b36e2b570d1fa27cfb1fc6b341 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -8,7 +8,7 @@ from constants import *
from ESP32_AT.imageTread_AT import get_AT_6DOF_info
global ml,esp_cam_on,openmv_on
-ml = 0
+ml = 1
esp_cam_on = 1
openmv_on = 0
seekVertDir = 1
@@ -408,7 +408,7 @@ def move2ball(gbx, gby, gb_dist):
# =========== Main Control ===========
-def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h):
+def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h,bcap_man):
'''
Description:
Given green ball information and AT information, the main control logic
@@ -429,14 +429,20 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
dir1, dir2, dir3, dir4 = '+', '-', '+', '-'
ballDetect = ball_detect(gbx, gby)
- ballCapture = ball_capture(LIDAR_dist1)
+ # ballCapture = ball_capture(LIDAR_dist1)
goalDetect = goal_detect(tx, ty)
ballCapture = 0
# debug
- ballCapture = 1
- # goalDetect = 0
+ if bcap_man == 1:
+ ballCapture = 1 # Manually determine Ball captured
+ elif bcap_man == 0:
+ ballCapture = 0 # Ball not captured
+ elif bcap_man == -1:
+ ballCapture = ball_capture(LIDAR_dist1)
+
+ # goalDetect = 0
# ballDetect = 0
if ballCapture: # Ball captured
print('ballCapture TRUE')
@@ -458,7 +464,7 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
return pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4
-def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h):
+def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h,bcap_man):
# ===== STEP 1: TAKE ALL INPUT =====
# print('in auto_control')
if ml ==1:
@@ -473,7 +479,7 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
if esp_cam_on == 1:
- url = 'http://192.168.0.204/cam-hi.jpg'
+ url = 'http://10.0.0.5/cam-hi.jpg' # 1
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
LIDAR_dist1 = 0
LIDAR_dist2 = 0
@@ -481,8 +487,7 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
# ===== STEP 2: MAIN CONTROL LOOP =====
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2,
- debugM, count_h)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h,bcap_man)
# ===== STEP 3: FEED ALL OUTPUT =====
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -491,12 +496,13 @@ def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_di
def auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM):
count_h = 0
+ bcap_man = -1
# gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, count_h,bcap_man)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
init_count += 1
- return init_count,count_h
+ return init_count,count_h,bcap_man
def init():
pygame.init()
@@ -648,32 +654,56 @@ def get_altitude_from_AT(AT_h,ty):
def test_function():
- url = 'http://192.168.0.230/cam-hi.jpg'
- tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url)
+ url_AT = 'http://10.0.0.5/cam-hi.jpg' # 1
+ url_gb = 'http://10.0.0.4/cam-hi.jpg' # 6
+
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url_AT)
+ gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight=True)
+
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))
-
-
+ print("gbx,gby,gb_dist:{},{},{}".format(gbx,gby,gb_dist))
def test_function1():
url = 'http://192.168.0.230/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))
-
+def manual_in_auto(Ctl_com, serial_port, flag):
+ if get_key('m'):
+ flag = 12
+ while (flag == 12):
+ manual_control(Ctl_com, serial_port)
+ flag = manual_return2auto('r',flag)
+ return flag
+
+def manual_return2auto(key_press,flag_r):
+ if get_key(key_press):
+ flag_r = 1
+ return flag_r
+
+def manual_ballcapture(bcap_man):
+ if get_key('b'): # the green ball is captured
+ bcap_man = 1
+ elif get_key('n'): # no ball is captured
+ bcap_man = 0
+ elif get_key('v'): # visual
+ bcap_man = -1
+ return bcap_man
# ===== Main Function =====
if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM9" # for Alienware
+ PORT = "COM11" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -702,6 +732,7 @@ if __name__ == '__main__':
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0 # Not moving
dir1, dir2, dir3, dir4 = '+', '+', '+', '+'
Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
+
# Trigger the ESP32_SLAVE to talk first
"""
gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
@@ -721,10 +752,13 @@ if __name__ == '__main__':
flag = 1
while (flag == 1):
if init_count == 0:
- init_count,count_h = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
+ init_count,count_h,bcap_man = auto_init(init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
print('auto_control')
- auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h)
+ auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h,bcap_man)
flag, print_count = keyboard_stop(flag,print_count)
+ flag = manual_in_auto(Ctl_com, serial_port, flag)
+ bcap_man = manual_ballcapture(bcap_man)
+
elif get_key('s'):
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = stop_all()
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)