diff --git a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
index 4fdade85df85c73c0641473b3dea9062fe91e1a8..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)
@@ -36,7 +39,8 @@ def get_AT_6DOF_info(url):
fy = 600
cx = 400
cy = 300
- AT_size = 0.16
+ #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:
@@ -49,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
@@ -81,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)
+
+ # 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)
+ # 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")
-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])
+ 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
@@ -97,6 +246,10 @@ if __name__ == "__main__":
# 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://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.9/cam-hi.jpg' # 6
# cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
@@ -109,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 c32e3b41b30ed8d5dc0e58491546bf190020a22c..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,9 +133,9 @@ 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
-Camera cam(&interface);
+// 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
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
@@ -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
@@ -195,7 +232,7 @@ void loop()
int y = 0;
bool goalfind_flag = 0;
- goalfind_flag = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz);
+// goalfind_flag = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz);
if (goalfind_flag){
Sent_tx = tx;
Sent_ty = ty;
@@ -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 f35e2f847384cd7b5e0a62e0d75132a9a210d627..ee6c9873287cb955a844ee1f5fb9dfba5d47c564 100644
--- a/Code/Control/Laptop_Code/ball_detection/ball_detection.py
+++ b/Code/Control/Laptop_Code/ball_detection/ball_detection.py
@@ -23,9 +23,9 @@ distanceDetect = __import__('ball_detection.distance-detection-torch.distance-de
# change the IP address below according to the IP shown in the Serial monitor of Arduino code
# url='http://192.168.1.107/cam-lo.jpg'
-url='http://192.168.4.1/cam-hi.jpg'
+# url='http://192.168.4.1/cam-hi.jpg'
# url='http://192.168.1.107/cam-mid.jpg'
-url='http://192.168.0.204/cam-hi.jpg' # url2
+# url='http://192.168.0.204/cam-hi.jpg' # url2
#### Modify Detecto Core to include possibility of other base models
def modifyCore():
@@ -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",
@@ -81,7 +81,7 @@ def returnModel(device, labelSet, modelLoc, modelFile):
return model
#### Live Detection
-def detectLive(model, minDetectionScore = 0.90, showSight = True):
+def detectLive(url , model, minDetectionScore = 0.90, showSight = True):
'''
Descrption:
ball detection ML Functions
diff --git a/Code/Control/Laptop_Code/ball_detection/ball_detection_full.py b/Code/Control/Laptop_Code/ball_detection/ball_detection_full.py
deleted file mode 100644
index ee9dceaa208cff7dc279518acd52ca0286831657..0000000000000000000000000000000000000000
--- a/Code/Control/Laptop_Code/ball_detection/ball_detection_full.py
+++ /dev/null
@@ -1,255 +0,0 @@
-## Codebase - Live Training-based Ball Detection
-### Import Modules and Drive
-import os
-import cv2
-import glob
-import torch
-import numpy as np
-import torch.nn as nn
-import torch.quantization
-import matplotlib.pyplot as plt
-import matplotlib.patches as patches
-
-from urllib.request import urlopen, Request #
-
-from base64 import b64decode
-from datetime import datetime
-from matplotlib import patches
-from detecto import core, utils
-from torchvision import transforms
-from matplotlib import pyplot as plt
-distanceDetect = __import__('ball_detection.distance-detection-torch.distance-detection-torch', fromlist = ['distanceDetect']).distanceDetect
-print("torch.cuda.is_available() = ", torch.cuda.is_available())
-
-minDetectionScore = 0.50
-
-# change the IP address below according to the IP shown in the Serial monitor of Arduino code
-# url='http://192.168.1.107/cam-lo.jpg'
-url='http://192.168.4.1/cam-hi.jpg'
-# url='http://192.168.1.107/cam-mid.jpg'
-
-#### Modify Detecto Core to include possibility of other base models
-def modifyCore():
- cModLineNums = [221, 253, 254]
- 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'
-
- cModLineVals = [" def __init__(self, classes=None, device=None, pretrained=True, modelname=\'fasterrcnn_resnet50_fpn\'):\n",
- " # Load a model pre-trained on COCO - User-Modified",
- " 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",
- " self._model = torchvision.models.detection.fasterrcnn_mobilenet_v3_large_fpn(pretrained=pretrained)\n",
- " elif modelname == \'fasterrcnn_mobilenet_v3_large_320_fpn\':\n",
- " self._model = torchvision.models.detection.fasterrcnn_mobilenet_v3_large_320_fpn(pretrained=pretrained)\n",
- " else:",
- " return ValueError('Unknown Pretrained Model')"]
- coreRead = open(coreFile, "r")
- coreLines = coreRead.readlines()
- if coreLines[253][-14:-1] != 'User-Modified':
- for count, cModLineNum in enumerate(cModLineNums):
- coreLines[cModLineNum] = cModLineVals[count]
- for count, cAddLineNum in enumerate(cAddLineNums):
- coreLines.insert(cAddLineNum, cAddLineVals[count])
- coreRead.close()
- coreWrite = open(coreFile, "w")
- coreWrite.writelines(coreLines)
- coreWrite.close()
-
-### Train the Model
-def modelName(type = 1):
- if type == 1:
- return 'fasterrcnn_resnet50_fpn'
- elif type == 2:
- return 'fasterrcnn_mobilenet_v3_large_fpn'
- elif type == 3:
- return 'fasterrcnn_mobilenet_v3_large_320_fpn'
- else:
- return ValueError('Unknown Pretrained Model')
-
-def modelTitle(modelLoc, device, type):
- time = datetime.now().strftime("%Y%m%d_%H%M%S")
- if device == 'cpu':
- dev = 'cpu'
- elif device[:4] == 'cuda':
- dev = 'cuda'
- print("using cuda")
- return modelLoc + 'model_weights-' + str(type) + '-' + time + '-' + dev + '.pth'
-
-def returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile):
- if modelAction == 'new':
- dataset = core.Dataset(trainLoc)
- model = core.Model(labelSet, device = device, modelname = modelName(modelType))
- losses = model.fit(dataset, epochs = 10, verbose = True)
- model.save(modelTitle(modelLoc, device, modelType))
- elif modelAction == 'load':
- model = core.Model(labelSet, modelname = modelname)
- #model = core.Model(classes = labelSet, device = device, pretrained = True, model_name = modelName(modelType))
- model._model.load_state_dict(torch.load(modelLoc + modelFile, map_location = torch.device(device)))
- # model = torch.quantization.quantize_dynamic(model.get_internal_model(), dtype=torch.qint8)
- return model
-
-### Object Detection
-def objectDetection(loc):
- image = utils.read_image(loc)
- predictions = model.predict(image)
- tLabels, tBoxes, tScores = predictions
- labels, boxes, scores = [[] for i in range(3)]
- imHeight, imWidth, _ = image.shape
- for count, box in enumerate(tBoxes):
- x0, y0 = float(box[0]), float(box[1])
- x1, y1 = float(box[2]), float(box[3])
- w = x1 - x0
- h = y1 - y0
- center = (x0 + (w/2), y0 + (h/2))
- dCrop = [int(y0*cropFactor + y1*(1-cropFactor)), int(y1*cropFactor + y0*(1-cropFactor)),
- int(x0*cropFactor + x1*(1-cropFactor)), int(x1*cropFactor + x0*(1-cropFactor))]
- distance = distanceDetect('load-testImg', distWtsFile, [[x0, x1, y0, y1], [imWidth, imHeight]])
- avgClRGB = cv2.mean(image[dCrop[0]:dCrop[1], dCrop[2]:dCrop[3]])
- avgClHue = rgbToHue(avgClRGB)
- print('Detection Score : ', round(float(tScores[count])*100, 2), '%')
- print('Average Color (RGB) : ', avgClRGB)
- print('Average Color (Hue) : ', avgClHue)
- print('Bounding Box Center : ', center)
- print('Distance to Ball : ', distance)
- # print('Relative Distance : ', relativeDist)
- # print('Coordinates : ', coordinates)
- print()
- if tScores[count] > minDetectionScore and avgClHue > colorLow and avgClHue < colorHigh:
- scores.append(float(tScores[count]))
- labels.append(tLabels[count]+' '+str(round(float(tScores[count])*100, 2))+'%')
- boxes.append ([x0, y0, x1, y1])
- displayBoxedImage(image, boxes, labels)
-
-#### Live Detection
-def detectLive(model, showSight = True):
- gbx = 0
- gby = 0
- dist = 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)
-
- labels, boxes, scores = model.predict(frame)
- for i in range(boxes.shape[0]):
- box = boxes[i]
- x0, y0 = float(box[0]), float(box[1])
- x1, y1 = float(box[2]), float(box[3])
- w = x1 - x0
- h = y1 - y0
- dCrop = [int(y0*cropFactor + y1*(1-cropFactor)), int(y1*cropFactor + y0*(1-cropFactor)),
- int(x0*cropFactor + x1*(1-cropFactor)), int(x1*cropFactor + x0*(1-cropFactor))]
- avgClRGB = cv2.mean(frame[dCrop[0]:dCrop[1], dCrop[2]:dCrop[3]])
- avgClHue = rgbToHue(avgClRGB)
-
- if scores[i] > minDetectionScore and avgClHue > colorLow and avgClHue < colorHigh:
- cv2.rectangle(frame, (int(x0), int(y0)),
- (int(x1), int(y1)),
- (0, 255, 0), 2)
- if labels:
- dist = distanceDetect('load-testImg', distWtsFile, [[x0, x1, y0, y1], frame.shape[:2]])
- gbx = int((x1 + x0) / 2)
- gby = int((y1 + y0) / 2)
- cv2.putText(frame, 'x:{}, y:{}'.format(gbx, gby),
- (int(box[0]), int(box[1]) - 10),
- cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 3)
-
-
- cv2.imshow('Ball Detection', frame)
- key = cv2.waitKey(1) & 0xFF
- return gbx, gby,dist
-
-### Supplementary Functions
-#### Display of Image with Bounding Boxes
-def displayBoxedImage(image, boxes = [], labels = None):
- fig, ax = plt.subplots(1)
- if isinstance(image, torch.Tensor):
- image = utils.reverse_normalize(image)
- image = transforms.ToPILImage()(image)
- ax.imshow(image)
- if labels is not None and not utils._is_iterable(labels):
- labels = [labels]
- for i in range(len(boxes)):
- box = boxes[i]
- width, height = box[2] - box[0], box[3] - box[1]
- initial_pos = (box[0], box[1])
- rect = patches.Rectangle(initial_pos, width, height, linewidth = 1, edgecolor = 'r', facecolor = 'none')
- if labels:
- ax.text(box[0] + 5, box[1] - 5, '{}'.format(labels[i]), color='red')
- ax.add_patch(rect)
- plt.show()
-
-#### Calculation of Hue from RGB (adopted from [code](https://www.geeksforgeeks.org/program-change-rgb-color-model-hsv-color-model/))
-def rgbToHue(RGB):
- RGB = [val/255.0 for val in RGB]
- cmax = max(RGB)
- cmin = min(RGB)
- diff = cmax - cmin
- r, g, b = RGB[0:3]
- if cmax == cmin:
- return 0
- if cmax == r:
- return (60 * ((g - b) / diff) + 360) % 360
- if cmax == g:
- return (60 * ((b - r) / diff) + 120) % 360
- if cmax == b:
- return (60 * ((r - g) / diff) + 240) % 360
-
-#### Formula-based Depth Estimation (adopted from [mono_depth.py](https://github.com/agrawalparth10/FORAY-Perception/blob/master/mono_depth/mono_depth.py))
-def depth_estimate(figDims, boundingBox):
- def x_hat(realLen, ip, i0, imageLen):
- return (realLen * (ip-i0)) / imageLen
- def y_hat(realLen, jp, j0, imageLen):
- return (realLen * (jp-j0)) / imageLen
- def z_hat(dist, pixelLen, imageLen):
- return dist*(pixelLen/imageLen)
- def cal_coordinates(dist, realLen, ip, jp, i0, j0, imageLen, pixelLen):
- return (x_hat(realLen, ip, i0, imageLen), y_hat(realLen, jp, j0, imageLen), z_hat(dist, pixelLen, imageLen))
- def measure(dist, realLen, ip, jp, i0, j0, imageLen, pixelLen):
- x_cor = x_hat(realLen, ip, i0, imageLen)
- z_cor = z_hat(dist, pixelLen, imageLen)
- dist = (x_cor ** 2 + z_cor ** 2) ** 0.5
- return dist
- imHeight, imWidth = figDims
- center = (imWidth // 2, imHeight // 2)
- x, y, w, h = boundingBox[0:4]
- relativeDist = measure (dist = 0.51, realLen = 0.228, ip = x, jp = y, i0 = center[0], j0 = center[1], imageLen = w, pixelLen = 500)
- coordinates = cal_coordinates(dist = 0.51, realLen = 0.228, ip = x, jp = y, i0 = center[0], j0 = center[1], imageLen = w, pixelLen = 500)
- return relativeDist, coordinates
-
-### Declare Varables and Constants
-device = 'cuda'
-imageLoc = './ball_detection/images/train-test-4/' #
-labelSet = ['ball']
-modelLoc = './ball_detection/model_weights/' #
-trainLoc = imageLoc + 'train/'
-colorLow = 60
-colorHigh = 180
-modelFile = 'model_weights-2-20210818_002355-cpu.pth'
-modelname = modelName(int(modelFile[14]))
-modelType = 2
-cropFactor = 0.90
-distWtsFile = './ball_detection/distance-detection-torch/distance-detection-weights-3-2.0sd-20210808_212550.json'
-modelAction = 'load' # 'load' to load previous model, 'new' to train new model (only possible on Colab)
-
-#FOR THE NEXT LINE : UNCOMMENT the first time you run, COMMENT OUT after the first time
-modifyCore()
-#########
-
-# model = returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
-# # # ## Testing (using Live Streaming)
-# while True:
-# detectLive(model)
-
-# ## Testing (using Untrained Images)
-# for file in sorted(glob.glob(imageLoc + '/test/*.jpg')):
-# objectDetection(file)
-#
-# ## Evaluation (using Trained Images)
-# for file in sorted(glob.glob(imageLoc + '/train/*.jpg')):
-# objectDetection(file)
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 9c7c47bd4b54c1f04a243ba48dd0bfd7d372fcfe..4bf80a33dc3d73df3ba1b13f4423b82830ae9aae 100644
--- a/Code/Control/Laptop_Code/constants.py
+++ b/Code/Control/Laptop_Code/constants.py
@@ -31,11 +31,10 @@ Bmax = 127
threshold = [Lmin, Lmax, Amin, Amax, Bmin, Bmax]
base_speed = 120
-seeking_speed = 70
+seeking_speed = 50
LIDAR_Thres = 300 # mm
-
# PID Control in move2ball
kpx,kix,kdx = 1.2, 0.01, 0.5
# kpx,kix,kdx = 0.0, 0.00, 0.0
@@ -44,14 +43,20 @@ kpy,kiy,kdy = 1.2, 0.05, 0.9
# PID Control in move2goal
# kpx_g,kix_g,kdx_g = 1.5, 0.01, 0.5
-kpx_g,kix_g,kdx_g = 2, 0.01, 0.7
+kpx_g,kix_g,kdx_g = 1.6, 0.01, 0.7
# kpx_g,kix_g,kdx_g = 0.0, 0.0, 0.0
-kpy_g,kiy_g,kdy_g = 1.7, 0.01, 0.7
+kpy_g,kiy_g,kdy_g = 2, 0.01, 0.7
# kpy_g,kiy_g,kdy_g = 0.0, 0.0, 0.0
# difference between center of AT and center of goal
AT_goal_Delta = 0 #cm
-AT_h1 = 50 # meters
+AT_h1 = 140 # openmv: | esp32_cam:meters
+
+kph,kih,kdh = 1.2,0.01,0.5
+
+# Break between AT detection during ball seeking
+AT_detectBreak = 60
+
-kph,kih,kdh = 1.2,0.01,0.5
\ No newline at end of file
+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
new file mode 100644
index 0000000000000000000000000000000000000000..69853142d45e29164e24affea9288054418c6eda
--- /dev/null
+++ b/Code/Control/Laptop_Code/imageThread_AT_gb_1_6.py
@@ -0,0 +1,38 @@
+# import sys
+# sys.path.append("../")
+
+from ESP32_AT.imageTread_AT import get_AT_6DOF_info
+
+from constants import *
+import ball_detection.ball_detection as ball_detection
+import cv2
+
+# detector = apriltag.Detector()
+
+
+if __name__ == "__main__":
+ 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
+
+ # GB
+ ball_detection.modifyCore()
+ # modelLoc = './ball_detection/model_weights/'
+ # modelFile = 'model_weights-2-20210818_002355-cpu.pth'
+ model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
+
+ gbx, gby = -1, -1 # by default (-1 means no found green ball)
+ gb_dist = -1 # by default (-1 means no found green ball)
+
+
+ while True:
+ 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)
+
+ key=cv2.waitKey(5)
+ if key==ord('q'):
+ break
+
+ cv2.destroyAllWindows()
diff --git a/Code/Control/Laptop_Code/imageThread_AT_gb.py b/Code/Control/Laptop_Code/imageThread_AT_gb_2_4.py
similarity index 75%
rename from Code/Control/Laptop_Code/imageThread_AT_gb.py
rename to Code/Control/Laptop_Code/imageThread_AT_gb_2_4.py
index fde64ac8e5d881978b58d3bd82a4cb24d13a791c..62ffb17295f372259451b2dc2030c6303d368906 100644
--- a/Code/Control/Laptop_Code/imageThread_AT_gb.py
+++ b/Code/Control/Laptop_Code/imageThread_AT_gb_2_4.py
@@ -11,8 +11,8 @@ import cv2
if __name__ == "__main__":
- url1 = 'http://192.168.0.230/cam-hi.jpg'
- url2 = 'http://192.168.0.204/cam-hi.jpg'
+ url_AT = 'http://10.0.0.3/cam-hi.jpg' #2
+ url_gb = 'http://10.0.0.6/cam-hi.jpg' #4
tid,tx,ty,tz,rx,ry,rz = 0,0,0,0,0,0,0
@@ -28,9 +28,9 @@ if __name__ == "__main__":
while True:
- tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url1)
+ tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url_AT)
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ # gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
key=cv2.waitKey(5)
if key==ord('q'):
diff --git a/Code/Control/Laptop_Code/main_keyboard.py b/Code/Control/Laptop_Code/main_keyboard.py
index e709ad57f84b34abe946a024c72fcf7b28066f61..2911a2a52615c3b36e2b570d1fa27cfb1fc6b341 100644
--- a/Code/Control/Laptop_Code/main_keyboard.py
+++ b/Code/Control/Laptop_Code/main_keyboard.py
@@ -9,8 +9,10 @@ from ESP32_AT.imageTread_AT import get_AT_6DOF_info
global ml,esp_cam_on,openmv_on
ml = 1
-esp_cam_on = 0
-openmv_on = 1
+esp_cam_on = 1
+openmv_on = 0
+seekVertDir = 1
+AT_detected_time = time.time()
if ml == 1:
import ball_detection.ball_detection as ball_detection
@@ -31,7 +33,7 @@ def serial_port_in_v1(serial_port):
# DEBUG Verbose
print("initiating one round of serial in ...")
- for i in range(8):
+ for i in range(7):
line = serial_port.readline()
val = int(line.decode())
@@ -49,9 +51,10 @@ def serial_port_in_v1(serial_port):
rz = val
elif i == 6:
LIDAR_dist1 = val
- elif i == 7:
- LIDAR_dist2 = val
+ # elif i == 7:
+ # LIDAR_dist2 = val
+ LIDAR_dist2 = 0
line = serial_port.readline()
debugM = line.decode()
@@ -190,16 +193,17 @@ def move2goal(tx, ty,tz):
setpoint_x1 = 0.0
setpoint_y1 = 0.0
- if tz <200:
- kpy_g,kiy_g,kdy_g = 0.0, 0.0, 0.0
- base_speed = 150
- AT_goal_Delta = 150
+ if tz < 2.0:
+ kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
+ base_speed = 200
+ AT_goal_Delta = -150
else:
- kpy_g,kiy_g,kdy_g = 2, 0.01, 0.5
- base_speed = 120
+ kpy_g,kiy_g,kdy_g = 2, 0.1, 0.5
+ # kpy_g,kiy_g,kdy_g = 0.0, 0.00, 0.0
+ base_speed = 140
AT_goal_Delta = 0
- inputx = tx / 1.00
- inputy = (ty + AT_goal_Delta) / 1.00 #
+ inputx = tx * 100/ 1.00
+ inputy = (ty * 100 + AT_goal_Delta) / 1.00 #
pid_x = PID(kpx_g, kix_g, kdx_g, setpoint = setpoint_x1)
pid_y = PID(kpy_g, kiy_g, kdy_g, setpoint = setpoint_y1)
@@ -211,17 +215,17 @@ def move2goal(tx, ty,tz):
outputx = pid_x(inputx)
outputy = pid_y(inputy)
-
+ print("outputy:{}".format(outputy))
# Vertical
pwm1 = abs(outputy)
pwm2 = abs(outputy)
if(outputy > 0):
- dir1 = '-'
- dir2 = '-'
- else:
dir1 = '+'
dir2 = '+'
+ else:
+ dir1 = '-'
+ dir2 = '-'
# Horizontal
lspeed = -1 * outputx + base_speed
@@ -253,9 +257,9 @@ def ball_seeking(count_h,tx,ty):
pwm1, pwm2, pwm3, pwm4
dir1, dir2, dir3, dir4
"""
- global set_h
+ global set_h, seekVertDir
if openmv_on == 1:
- delta_h = 100
+ delta_h = 100 # cm
threshold_h = 20
elif esp_cam_on == 1:
delta_h = 2 # meter
@@ -282,6 +286,23 @@ def ball_seeking(count_h,tx,ty):
else:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+
+ # if time.time() > AT_detected_time + AT_detectBreak:
+ # AT_detected = AT_detect(tx, ty)
+ # AT_detected_time = time.time()
+ #
+ # while facingWall():
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction_ssp(new_ssp)
+ #
+ # if AT_detected:
+ # current_h = get_altitude_from_AT(AT_h1,ty)
+ #
+ # if LIDAR_dist2 < LIDAR2_Thres:
+ # seekVertDir = 0
+ # if baroHeight < baroThres:
+ # seekVertDir = 1
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move_in_spiral(new_ssp, seekVertDir)
+
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
def vertical_control(current_h,set_h):
@@ -317,6 +338,14 @@ def rotate_one_direction_ssp(ssp):
return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+def move_in_spiral(ssp, dir):
+ pwm1, pwm2, pwm3, pwm4 = ssp, ssp, ssp, ssp/2
+ dir1, dir2, dir3, dir4 = '+', '+', '+', '-'
+ if dir == 0:
+ dir1, dir2 = '-', '-'
+
+ return int(pwm1), int(pwm2), int(pwm3), int(pwm4), dir1, dir2, dir3, dir4
+
def move2ball(gbx, gby, gb_dist):
"""
Description:
@@ -379,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
@@ -394,20 +423,29 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
Output:
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 : Blimp motor manuver parameters
'''
+ print('in main_control')
# placeholder
pwm1, pwm2, pwm3, pwm4 = 0 , 0 , 0 , 0
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 = 1
+ 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')
if goalDetect: # Goal detected
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = move2goal(tx, ty,tz)
@@ -415,21 +453,24 @@ def main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_d
# stop_all() # Debug
pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
else: # Ball not captured
+ print('ballCapture FALSE')
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 = rotate_one_direction()
- pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = ball_seeking(count_h,tx,ty)
+ pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = rotate_one_direction()
+
+ # pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4 = ball_seeking(count_h,tx,ty)
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:
- gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
line = serial_port.readline()
-
+ # print('auto')
if openmv_on == 1:
if line == b'SERIAL_IN_START\r\n':
tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM = serial_port_in_v1(serial_port)
@@ -438,12 +479,15 @@ 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.1.118/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
+ debugM = "using two esp32 cam"
+
# ===== 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)
@@ -452,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
- # gbx, gby, gb_dist = ball_detection.detectLive(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)
+ 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,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()
@@ -572,7 +617,7 @@ def manual_control(Ctl_com,serial_port):
waitTime = 0.05
# changed
- # gbx, gby, gb_dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
+ # gbx, gby, gb_dist = ball_detection.detectLive(url_gb, model, minDetectionScore, showSight = True)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
@@ -606,21 +651,59 @@ def get_altitude_from_AT(AT_h,ty):
altitude = AT_h - ty
return altitude
+
+
def test_function():
- url = 'http://192.168.1.118/cam-hi.jpg'
+ 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()
@@ -649,9 +732,10 @@ 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(model, minDetectionScore, showSight = True)
+ 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)
serial_port_out(serial_port, pwm1, pwm2, pwm3, pwm4, dir1, dir2, dir3, dir4)
"""
@@ -668,9 +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)
- auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM,count_h)
+ 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,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)
diff --git a/Code/ESP32_Cam/WifiCam/WifiCam.ino b/Code/ESP32_Cam/WifiCam/WifiCam.ino
index 6515ca9d1834f969bc466cf54edd0ac5300dd85b..ef9e520b37486e33033bac5aa344f183057c7d83 100644
--- a/Code/ESP32_Cam/WifiCam/WifiCam.ino
+++ b/Code/ESP32_Cam/WifiCam/WifiCam.ino
@@ -3,8 +3,14 @@
#include <esp32cam.h>
-const char* WIFI_SSID = "lemur";
-const char* WIFI_PASS = "lemur9473";
+// const char* WIFI_SSID = "lemur";
+// const char* WIFI_PASS = "lemur9473";
+
+// const char* WIFI_SSID = "uclahci";
+// const char* WIFI_PASS = "94739473";
+
+const char* WIFI_SSID = "NETGEAR33";
+const char* WIFI_PASS = "freestreet163";
// const char* WIFI_SSID = "LEMUR_ESP32_EYE_AP_1";
// const char* WIFI_PASS = "123456789";