diff --git a/Code/.gitignore b/Code/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..b42097e865c14e2dde215fccdd2e303d6c103b36
--- /dev/null
+++ b/Code/.gitignore
@@ -0,0 +1 @@
+**/__pycache__/
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/ball_detection/__pycache__/esp32cam_live_feed.cpython-38.pyc b/Code/Control/Laptop_Code/ball_detection/__pycache__/esp32cam_live_feed.cpython-38.pyc
deleted file mode 100644
index 640418a2f7bf60aad201d640037e710058866105..0000000000000000000000000000000000000000
Binary files a/Code/Control/Laptop_Code/ball_detection/__pycache__/esp32cam_live_feed.cpython-38.pyc and /dev/null differ
diff --git a/Code/Control/Laptop_Code/ball_detection/ball_detection.py b/Code/Control/Laptop_Code/ball_detection/ball_detection.py
new file mode 100644
index 0000000000000000000000000000000000000000..c231c8a90efadcd02b88323cdef709e5b043c1da
--- /dev/null
+++ b/Code/Control/Laptop_Code/ball_detection/ball_detection.py
@@ -0,0 +1,157 @@
+## 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
+from IPython.display import Image, display, Javascript
+distanceDetect = __import__('ball_detection.distance-detection-torch.distance-detection-torch', fromlist = ['distanceDetect']).distanceDetect
+# print("torch.cuda.is_available() = ", torch.cuda.is_available())
+
+# 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'
+
+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'
+ return modelLoc + 'model_weights-' + str(type) + '-' + time + '-' + dev + '.pth'
+
+def returnModel(device, labelSet, modelLoc, modelFile):
+ model = core.Model(labelSet, modelname = modelName(int(modelFile[14])))
+ model._model.load_state_dict(torch.load(modelLoc + modelFile, map_location = torch.device(device)))
+ return model
+
+#### Live Detection
+def detectLive(model, minDetectionScore = 0.90, 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)
+
+ if showSight:
+ 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
+colorLow = 60
+colorHigh = 180
+cropFactor = 0.90
+distWtsFile = './ball_detection/distance-detection-torch/distance-detection-weights-3-2.0sd-20210808_212550.json'
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/ball_detection/esp32cam_live_feed.py b/Code/Control/Laptop_Code/ball_detection/ball_detection_full.py
similarity index 89%
rename from Code/Control/Laptop_Code/ball_detection/esp32cam_live_feed.py
rename to Code/Control/Laptop_Code/ball_detection/ball_detection_full.py
index 2d407591ff1ebc285fb26c5d13b40d5031823d1b..247ee0b07715263f5dd451ffce7bcefbe2c76dd7 100644
--- a/Code/Control/Laptop_Code/ball_detection/esp32cam_live_feed.py
+++ b/Code/Control/Laptop_Code/ball_detection/ball_detection_full.py
@@ -1,4 +1,4 @@
-## Codebase - Live Training-based Ball Detection (COMPLETE)
+## Codebase - Live Training-based Ball Detection
### Import Modules and Drive
import os
import cv2
@@ -22,14 +22,13 @@ from IPython.display import Image, display, Javascript
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
+# 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, 254]
@@ -38,9 +37,6 @@ def modifyCore():
# REPLACABLE LINE FOR DIFFERENT LOCAL COMPUTER DEVICES
coreFile = 'C:\\Users\\uclal\\.conda\\envs\\FORAY\\Lib\\site-packages\\detecto\\core.py'
-
- print('debug')
-
cModLineVals = [" def __init__(self, classes=None, device=None, pretrained=True, modelname=\'fasterrcnn_resnet50_fpn\'):\n",
" self._model = torchvision.models.detection.fasterrcnn_resnet50_fpn(pretrained=pretrained)\n"]
cAddLineVals = [" if modelname == \'fasterrcnn_resnet50_fpn\':\n",
@@ -110,9 +106,6 @@ def objectDetection(loc):
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))]
- # relativeDist, coordinates = depth_estimate((imHeight, imWidth), (x0, y0, w, h))
- # relativeDist = float(relativeDist)
- # coordinates = tuple([float(coord) for coord in coordinates])
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)
@@ -124,7 +117,7 @@ def objectDetection(loc):
# print('Relative Distance : ', relativeDist)
# print('Coordinates : ', coordinates)
print()
- if tScores[count] > minScore and avgClHue > colorLow and avgClHue < colorHigh:
+ 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])
@@ -132,18 +125,6 @@ def objectDetection(loc):
#### Live Detection
def detectLive(model, showSight = True):
- # cv2.namedWindow('Ball Detection')
- # try:
- # cap = cv2.VideoCapture(0)#, cv2.CAP_DSHOW)
- # except:
- # print('No webcam available.')
- # return
-
- # ret = True
- # while ret:
- # ret, frame = cap.read()
- #print(ret)
- #print(frame)
gbx = 0
gby = 0
dist = 0
@@ -155,7 +136,6 @@ def detectLive(model, showSight = True):
frame=cv2.imdecode(imgnp,-1)
labels, boxes, scores = model.predict(frame)
- # labels, boxes, scores = [np.array([]) for i in range(3)]
for i in range(boxes.shape[0]):
box = boxes[i]
x0, y0 = float(box[0]), float(box[1])
@@ -167,15 +147,12 @@ def detectLive(model, showSight = True):
avgClRGB = cv2.mean(frame[dCrop[0]:dCrop[1], dCrop[2]:dCrop[3]])
avgClHue = rgbToHue(avgClRGB)
- if scores[i] > minScore and avgClHue > colorLow and avgClHue < colorHigh:
+ 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]])
- # cv2.putText(frame, '{}: {}% at {} cm'.format(labels[i], round(scores[i].item()*100.0, 1), round(distance, 2)),
- # (int(box[0]), int(box[1]) - 10),
- # cv2.FONT_HERSHEY_SIMPLEX, 1, (255, 0, 0), 3)
gbx = int((x1 + x0) / 2)
gby = int((y1 + y0) / 2)
cv2.putText(frame, 'x:{}, y:{}'.format(gbx, gby),
@@ -185,11 +162,7 @@ def detectLive(model, showSight = True):
cv2.imshow('Ball Detection', frame)
key = cv2.waitKey(1) & 0xFF
- # if key == ord('q') or key == 27:
- # break
return gbx, gby,dist
- #cv2.destroyAllWindows()
- # cap.release()
### Supplementary Functions
#### Display of Image with Bounding Boxes
@@ -255,7 +228,6 @@ imageLoc = './ball_detection/images/train-test-4/' #
labelSet = ['ball']
modelLoc = './ball_detection/model_weights/' #
trainLoc = imageLoc + 'train/'
-minScore = 0.50
colorLow = 60
colorHigh = 180
modelFile = 'model_weights-2-20210818_002355-cpu.pth'
@@ -280,4 +252,4 @@ modelAction = 'load' # 'load' to load previous model, 'new' to train new model (
#
# ## Evaluation (using Trained Images)
# for file in sorted(glob.glob(imageLoc + '/train/*.jpg')):
-# objectDetection(file)
+# objectDetection(file)
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210728_215304-cuda.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210728_215304-cuda.pth
new file mode 100644
index 0000000000000000000000000000000000000000..95b2ffacb11e4a1333d0a62a64114527bfd88a1e
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210728_215304-cuda.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210808_132627-cuda.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210808_132627-cuda.pth
new file mode 100644
index 0000000000000000000000000000000000000000..6508c9f62979fc38e00d2616c3e3d732c62ddfcf
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210808_132627-cuda.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210809_162415-cuda.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210809_162415-cuda.pth
new file mode 100644
index 0000000000000000000000000000000000000000..7beaa71ccd7336654f454e025b970816dff5e5f4
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210809_162415-cuda.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210811_114017-cpu.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210811_114017-cpu.pth
new file mode 100644
index 0000000000000000000000000000000000000000..686dcbfb4aa89abb00f21aee4c0c7c3e5c82d5aa
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-1-20210811_114017-cpu.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210811_183605-cpu.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210811_183605-cpu.pth
new file mode 100644
index 0000000000000000000000000000000000000000..1da22a53df3248b417ec4bd8e2dc21f098e7cdff
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210811_183605-cpu.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210812_081758-cpu.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210812_081758-cpu.pth
new file mode 100644
index 0000000000000000000000000000000000000000..3ebebd192ec041f31fa1002e9f9988407698f75f
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210812_081758-cpu.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210818_002355-cpu.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210818_002355-cpu.pth
new file mode 100644
index 0000000000000000000000000000000000000000..904a595ce300523035519e413be12758b3372cf9
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210818_002355-cpu.pth differ
diff --git a/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210818_155713-cpu.pth b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210818_155713-cpu.pth
new file mode 100644
index 0000000000000000000000000000000000000000..b4d5d2750da828123aa179c1c5734e5b96474912
Binary files /dev/null and b/Code/Control/Laptop_Code/ball_detection/model_weights/model_weights-2-20210818_155713-cpu.pth differ
diff --git a/Code/Control/Laptop_Code/inputs.py b/Code/Control/Laptop_Code/inputs.py
new file mode 100644
index 0000000000000000000000000000000000000000..e5b63a918108bc46a2b71d7777f0653a77a28f9a
--- /dev/null
+++ b/Code/Control/Laptop_Code/inputs.py
@@ -0,0 +1,662 @@
+import numpy
+
+minDetectionScore = 0.90
+
+
+def formatInput(inp):
+ tx, ty, tz, rx, ry, rz, d_lidar, debug = inp
+ t = [tx, ty, tz]
+ r = [rx, ry, rz]
+ return [t, r, d_lidar, debug]
+
+#include <esp_now.h>
+#include <WiFi.h>
+#include <SparkFun_VL53L1X.h>
+#include <Wire.h>
+#include <Adafruit_MotorShield.h>
+#include "LedPanel.h"
+#include <Arduino.h>
+#include <string>
+#include <vector>
+#include "Camera.h"
+#include <PID_v1.h>
+#include "utilities.h"
+
+// ============================== All variables ============================================
+//-------------------- lidar part --------------------
+SFEVL53L1X distanceSensor;
+int budgetIndex = 4;
+int budgetValue[7] = {15, 20, 33, 50, 100, 200, 500};
+int LED = LED_BUILTIN;
+// ---------------------------------------------------
+// -----------------------const variables----------------------------
+//Identify all the global constants that will be used by the robot
+const int BAUD_RATE = 115200;
+const int MAX_SPEED = 255;
+const double RESOLUTION_W = 320.0;
+const double RESOLUTION_H = 240.0;
+const int ENDDEMO_TAG = 0;
+const uint32_t THISNODE_ID = 88789821;
+double Setpointx, Inputx, Outputx;
+double Setpointy, Inputy, Outputy;
+
+
+// -------------------------------------------------------------------
+// ----------------------- variables ----------------------------
+uint8_t broadcastAddress[] = {0x40, 0xF5, 0x20, 0x44, 0xB6, 0x4C}; // #4
+String success;
+
+String sentDebugM = "";
+int count = 0;
+int count_var = 0;
+int print_count = 0;
+int change_count =0;
+//----------------------------------------------------------
+String strData = "";
+double valData = 0.0;
+String queData = "";
+double Kpx=2, Kix=0.1, Kdx=0.25;
+double Kpy=1, Kiy=0.1, Kdy=0.25;
+int8_t g1 = 0,g2=1,g3=2;
+int8_t goal_id[3] = {g1, g2, g3};
+// int8_t goal[3] = {0,1,2};
+// ------------- color part -------------------------
+ // LAB: L[min] - L[max], A[min] - A[max], B[min] - B[max]
+ //green old = (30, 100, -68, -13, 30, 127)
+ // green_new = (30, 100, -49, -22, 31, 127)
+ //(30,100,-68,2,6,127) - detect the yellow wall as well
+ // red (30, 100, 127, 41, 127, 13)
+int8_t Lmin = 30,Lmax = 100, Amin = -49,Amax = -22,Bmin = 31,Bmax = 127;
+int8_t threshold[6] = {Lmin, Lmax, Amin, Amax, Bmin, Bmax};
+int base_speed = 70;
+int seeking_speed = 70;
+int LIDAR_Thres = 300; // mm
+int gbx = 0, gby = 0,gbd = 0;
+double gbc = 0;
+int debug_ball_cap = 0;
+// -------------------------------------------------------------------
+
+//Identify all the variables that will be used by the robot
+int findcolor = 1; //based on the same code from openMV. Determine the color you want to find
+char newPTCL = '1';
+int pau = 0;
+int displayTracking = 0;
+// int8_t threshold[6] = {0, 0, 0, 0, 0, 0};
+// int BASE_SPEED = 70; //125;
+
+//The debug LEDs that we will be using. Description at:
+
+const int DEBUG_STATE = 31;
+const int DEBUG_KP = 30;
+const int DEBUG_KI = 29;
+const int DEBUG_KD = 28;
+const int DEBUG_BASESPEED = 27;
+const int DEBUG_THRESHOLD_MIN = 26;
+const int DEBUG_THRESHOLD_MAX = 25;
+const int DEBUG_VERTICALSPEED = 17;
+const int DEBUG_RSPEED = 16;
+const int DEBUG_LSPEED = 24;
+
+// =============================== 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);
+LedPanel panel(NUMPIXELS,PIN_PIXELS);
+// ========================== Motor part ====================================
+// Create the motor shield object with the default I2C address
+Adafruit_MotorShield AFMS = Adafruit_MotorShield();
+// changed
+Adafruit_DCMotor *motorVertical_L = AFMS.getMotor(1);
+Adafruit_DCMotor *motorVertical_R = AFMS.getMotor(2);
+Adafruit_DCMotor *motorLeft = AFMS.getMotor(3);
+Adafruit_DCMotor *motorRight = AFMS.getMotor(4);
+// changed
+// ========================== PID part ====================================
+// Setup PID
+PID PID_x(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, DIRECT);
+PID PID_y(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, DIRECT);
+
+//-------------------------------------------------------------------------------------
+typedef struct struct_message {
+ String StrD;
+ double ValD;
+ String DebugM;
+ String QueM;
+ String VarM;
+} struct_message;
+
+struct_message receivedData;
+//-------------------------------------------------------------------------------------
+// Callback when data is sent
+void OnDataSent(const uint8_t *mac_addr, esp_now_send_status_t status) {
+ print("\r\nLast Packet Send Status:\t");
+ print(status == ESP_NOW_SEND_SUCCESS ? "Delivery Success" : "Delivery Fail");
+
+ if (status ==0){
+ success = "Delivery Success :)";
+ }
+ else{
+ success = "Delivery Fail :(";
+ }
+}
+
+
+// Callback when data is received
+void OnDataRecv(const uint8_t * mac, const uint8_t *incomingData, int len) {
+ memcpy(&receivedData, incomingData, sizeof(receivedData));
+ print("\ndata:");
+ print(receivedData.StrD);
+ print(receivedData.ValD);
+ print(receivedData.QueM);
+
+ if (receivedData.QueM != "?"){
+ valData = receivedData.ValD;
+ }
+
+ strData = receivedData.StrD;
+ queData = receivedData.QueM;
+
+ count = 0;
+ count_var = 0;
+ print_count=0;
+ change_count = 0;
+
+ print("queData:");
+ print(queData);
+}
+//======================================================================================
+
+void setup() {
+ Serial.begin(BAUD_RATE);
+ AFMS.begin(); // create with the default frequency 1.6KHz
+ interface.begin(); //communication between ESP and OpenMV
+ panel.beginPanel();
+ // -------------- PID part --------------------
+ //the values that the PID will try to reach
+ Setpointx = 400.0;
+ Setpointy = 300.0;
+ PID_y.SetOutputLimits(-255, 255); //up positive
+ PID_x.SetOutputLimits(-255, 255); //left positive
+ PID_x.SetMode(AUTOMATIC);
+ PID_y.SetMode(AUTOMATIC);
+ // -------------- lidar part --------------------
+ pinMode(LED, OUTPUT);
+ digitalWrite(LED, HIGH);
+ if (distanceSensor.begin() == 0)
+ print("Sensor online!");
+ distanceSensor.startRanging();
+ distanceSensor.setIntermeasurementPeriod(budgetValue[budgetIndex]);
+ digitalWrite(LED, LOW);
+
+ // ----------------esp now -----------------------
+ Wire.begin();
+ WiFi.mode(WIFI_STA);
+
+ // Init ESP-NOW
+ if (esp_now_init() != ESP_OK) {
+ print("Error initializing ESP-NOW");
+ return;
+ }
+
+ // Once ESPNow is successfully Init, we will register for Send CB to
+ // get the status of Trasnmitted packet
+ esp_now_register_send_cb(OnDataSent);
+
+ // Register peer
+ esp_now_peer_info_t peerInfo;
+ memcpy(peerInfo.peer_addr, broadcastAddress, 6);
+ peerInfo.channel = 0;
+ peerInfo.encrypt = False;
+
+ // Add peer
+ if (esp_now_add_peer(&peerInfo) != ESP_OK){
+ print("Failed to add peer");
+ return;
+ }
+
+ // Register for a callback function that will be called when data is received
+ esp_now_register_recv_cb(OnDataRecv);
+
+
+}
+
+// ============================== other functions ====================================
+def ML_detect_gb(x, y):
+ if x == 0 and y == 0:
+ return False
+ else:
+ return True
+
+def QueryVariables():
+ if (strData == "kpx"){
+ receivedData.VarM = String(Kpx);
+ }else if(strData == "kix"){
+ receivedData.VarM = String(Kix);
+ }else if(strData == "kdx"){
+ receivedData.VarM = String(Kdx);
+ }else if(strData == "kpy"){
+ receivedData.VarM = String(Kpy);
+ }else if(strData == "kiy"){
+ receivedData.VarM = String(Kiy);
+ }else if(strData == "kdy"){
+ receivedData.VarM = String(Kdy);
+ }else if(strData == "tha"){
+ receivedData.VarM = String(Lmin);
+ }else if(strData == "thb"){
+ receivedData.VarM = String(Lmax);
+ }else if(strData == "thc"){
+ receivedData.VarM = String(Amin);
+ }else if(strData == "thd"){
+ receivedData.VarM = String(Amax);
+ }else if(strData == "the"){
+ receivedData.VarM = String(Bmin);
+ }else if(strData == "thf"){
+ receivedData.VarM = String(Bmin);
+ }else if(strData == "bsp"){
+ receivedData.VarM = String(base_speed);
+ }else if(strData == "ssp"){
+ receivedData.VarM = String(seeking_speed);
+ }else if(strData == "lth"){
+ receivedData.VarM = String(LIDAR_Thres);
+ }
+
+ send_message_var_once();
+ // queData = "";
+ receivedData.VarM = "";
+}
+
+
+void send_message_var_once(){
+ if(count_var==0){
+ send_message();
+ count_var+=1;
+ }
+}
+
+
+void send_message_once(){
+ if(count==0){
+ send_message();
+ count+=1;
+ receivedData.DebugM = "";
+ }
+}
+
+
+void send_message(){
+ esp_err_t result = esp_now_send(broadcastAddress, (uint8_t *) &receivedData, sizeof(receivedData));
+ // esp_now_send(RxMACaddress, (uint8_t *) &sentData, sizeof(sentData));
+ //-------------------------------------------------------------------------------------
+ if (result == ESP_OK) {
+ print("Sent with success");
+ }
+ else {
+ print("Error sending the data");
+ }
+ //-------------------------------------------------------------------------------------
+ delay(100);
+}
+
+
+void ChangeVariables(){
+ // all the variables in this function can be remotely changed
+
+ //-------------------PID-----------------
+ Kpx = getDoubleVal(strData,"kpx",valData,Kpx);
+ Kix = getDoubleVal(strData,"kix",valData,Kix);
+ Kdx = getDoubleVal(strData,"kdx",valData,Kdx);
+ Kpy = getDoubleVal(strData,"kpy",valData,Kpy);
+ Kiy = getDoubleVal(strData,"kiy",valData,Kiy);
+ Kdy = getDoubleVal(strData,"kdy",valData,Kdy);
+
+ //-------------------Goal id-----------------
+ g1 = getIntVal(strData,"gda",valData,goal_id[0]);
+ g2 = getIntVal(strData,"gdb",valData,goal_id[1]);
+ g3 = getIntVal(strData,"gdc",valData,goal_id[2]);
+ goal_id[0] = g1;
+ goal_id[1] = g2;
+ goal_id[2] = g3;
+
+ //-------------------Color threshold-----------------
+ Lmin = getIntVal(strData,"tha",valData,threshold[0]);
+ Lmax = getIntVal(strData,"thb",valData,threshold[1]);
+ Amin = getIntVal(strData,"thc",valData,threshold[2]);
+ Amax = getIntVal(strData,"thd",valData,threshold[3]);
+ Bmin = getIntVal(strData,"the",valData,threshold[4]);
+ Bmax = getIntVal(strData,"thf",valData,threshold[5]);
+ threshold[0] = Lmin;
+ threshold[1] = Lmax;
+ threshold[2] = Amin;
+ threshold[3] = Amax;
+ threshold[4] = Bmin;
+ threshold[5] = Bmax;
+
+ //-------base_speed,seeking_speed,LIDAR_Thres-----------------
+ base_speed = abs(getDoubleVal(strData,"bsp",valData,base_speed));
+ seeking_speed = abs(getDoubleVal(strData,"ssp",valData,seeking_speed));
+ LIDAR_Thres = getDoubleVal(strData,"lth",valData,LIDAR_Thres);
+
+ //------ green ball x, y----
+ gbc = getDoubleVal(strData,"gbc",valData,gbc);
+ gbx = gbc /1000000;
+ gby = fmod(gbc,1000000)/1000;//(gbc.toInt() % 1000000)/1000;
+ gbd = fmod(gbc,1000); // gbc.toInt() % 1000;
+
+ //
+ Setpointx = getDoubleVal(strData,"spx",valData,Setpointx);
+ Setpointy = getDoubleVal(strData,"spy",valData,Setpointy);
+
+ debug_ball_cap = getDoubleVal(strData,"dbc",valData,debug_ball_cap);
+
+ // gbx = getDoubleVal(strData,"gbx",valData,gbx);
+ // gby = getDoubleVal(strData,"gby",valData,gby);
+
+}
+
+double getDoubleVal(String checkData,String Ans,double val,double ori_val){
+ if (checkData == Ans){
+ strData = "";
+ valData = 0.0;
+ return val;
+ }else {
+ return ori_val;
+ }
+}
+
+def getIntVal(checkData, Ans, val, ori_val):
+ if checkData == Ans:
+ strData = ""
+ valData = 0.0
+ return int(val)
+ else:
+ return ori_val
+
+def print_allvariables():
+ if print_count <= 1:
+ print("---------------")
+ print("base speed:")
+ print(base_speed)
+ print("|")
+ print("seeking speed:")
+ print(seeking_speed);
+ print("|");
+ print("lidar thres:");
+ print(LIDAR_Thres);
+
+ print("threshold:");
+ print(threshold[0]);
+ print("|");
+ print(threshold[1]);
+ print("|");
+ print(threshold[2]);
+ print("|");
+ print(threshold[3]);
+ print("|");
+ print(threshold[4]);
+ print("|");
+ print(threshold[5]);
+
+ print("gid:");
+ print(goal_id[0]);
+ print(goal_id[1]);
+ print(goal_id[2]);
+
+ print("Kdx:");
+ print(Kdx);
+ print("|");
+ print("Kix:");
+ print(Kix);
+ print("|");
+ print("Kpx:");
+ print(Kpx);
+ print("|");
+ print("Kdy:");
+ print(Kdy);
+ print("|");
+ print("Kiy:");
+ print(Kiy);
+ print("|");
+ print("Kpy:");
+ print(Kpy);
+ print("|");
+
+ print("gbc:");
+ print(gbc);
+ print("|");
+ print("gbx:");
+ print(gbx);
+ print("|");
+ print("gby:");
+ print(gby);
+ print("gbd:");
+ print(gbd);
+
+ print("---------------------------\n");
+ print_count +=1 ;
+ }
+}
+
+void seeking(){
+ moveVertical(0);
+ moveHorizontal(seeking_speed, 0);
+}
+
+# vel value should be between -255 to 255 with positive values moving the blimp upward.
+def moveVertical(vel):
+ if vel > 0: # up
+ panel.singleLED(DEBUG_VERTICALSPEED, abs(vel), 0, 0)
+ motorVertical_L->setSpeed(abs((int) vel))
+ motorVertical_R->setSpeed(abs((int) vel))
+ motorVertical_L->run(BACKWARD)
+ motorVertical_R->run(FORWARD)
+ elif vel < 0: # down
+ panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, abs(vel))
+ motorVertical_L->setSpeed(abs((int) vel))
+ motorVertical_R->setSpeed(abs((int) vel))
+ motorVertical_L->run(FORWARD)
+ motorVertical_R->run(BACKWARD)
+ else:
+ panel.singleLED(DEBUG_VERTICALSPEED, 0, 0, 0)
+ motorVertical_L->setSpeed(0)
+ motorVertical_R->setSpeed(0)
+
+def moveHorizontal(vel_rot, vel_trn):
+ lspeed = -1*vel_rot + vel_trn
+ rspeed = 1*vel_rot + vel_trn
+
+ if lspeed > 0:
+ motorLeft->run(BACKWARD) # make it move forward
+ else:
+ motorLeft->run(FORWARD)
+ if rspeed > 0:
+ motorRight-> run(FORWARD)
+ else:
+ motorRight-> run(BACKWARD) # make it move backward
+ displaySpeed(lspeed, rspeed)
+ motorLeft->setSpeed(min(MAX_SPEED, abs(lspeed )))
+ motorRight->setSpeed(min(MAX_SPEED, abs(rspeed)))
+
+def displaySpeed(lspeed, rspeed):
+ if lspeed < 0:
+ panel.singleLED(DEBUG_LSPEED, 0, 0, abs(lspeed))
+ else:
+ panel.singleLED(DEBUG_LSPEED, abs(lspeed), 0, 0)
+
+ if rspeed < 0:
+ panel.singleLED(DEBUG_RSPEED, 0, 0, abs(rspeed))
+ else:
+ panel.singleLED(DEBUG_RSPEED, abs(rspeed), 0, 0)
+
+//When using the camera to detect objects such as colored blobs or april tags. This function is
+//useful when only a single object is the target. The approximate position will be marked with an
+//led turning white in the 8*4 panel of the NeoPixel LED Panel. Therefore, this function can be
+//called assuming that you have created the LED panel object, which in this code is named "panel".
+//If the LED panel is called something else, just edit the code here
+void displayTrackedObject(int cx, int cy, int w_res, int h_res){
+ int lednum = 0;
+ int vertshift = 0;
+ panel.resetPanel();
+ lednum = cx/(w_res/8); //because lednum is an int, it will handle flooring the value
+ vertshift = cy/(h_res/4);
+ lednum = lednum + 8*vertshift;
+ panel.singleLED(lednum, 10, 10 , 10);
+}
+
+//This function translate a string of message into the constants for a PID controller. Ignoring the
+//first command character, user can input any one, two, or all three parameter to be changed by identifying
+//the new parameter with the capital letter of that specific parameter, each separated with a space.
+//Some of the valid msg examples are:
+// - "P0.02"
+// - "D1.23"
+// - "P0.14 D9.5"
+// - "P0.1 I0.1 D0.1"
+//
+//NOTE:
+// - the parameter doesn't have to be in order. You can do DI or IP in whichever order as long as it follows a valid double value
+// - while the function works if you passed a negative double, the PID controller will not and will produce error.
+// - suggested command character: 'T'[uning]
+def setPIDConstants(msg, &p_constant, &i_constant, &d_constant):
+ new_p = Kpx
+ new_i = Kix
+ new_d = Kdx
+
+ msgLen = msg.length()
+ startpoint = 0
+ endpoint = 0
+
+ for i in range(0, msgLen):
+ if msg[i] == 'P':
+ startpoint = i + 1
+ for j in range(i + 1, msgLen):
+ if msg[j] == ' ':
+ endpoint = j
+ break
+ else:
+ endpoint = msgLen
+ if endpoint > startpoint # check to see if it is a valid value
+ new_p = msg.substring(startpoint, endpoint).toDouble()
+
+ if msg[i] == 'I':
+ startpoint = i + 1
+ for j in range(i + 1, msgLen):
+ if msg[j] == ' ':
+ endpoint = j
+ break
+ else:
+ endpoint = msgLen
+ if endpoint > startpoint # check to see if it is a valid value
+ new_i = msg.substring(startpoint, endpoint).toDouble()
+
+ if msg[i] == 'D':
+ startpoint = i + 1
+ for j in range(i + 1, msgLen + 1):
+ if msg[j] == ' ' or msg[j] == '\0':
+ endpoint = j
+ break
+ else:
+ endpoint = msgLen
+ if endpoint > startpoint # check to see if it is a valid value
+ new_d = msg.substring(startpoint, endpoint).toDouble()
+
+ ## Debugging
+ unsigned long t = millis();
+
+ debugPIDConstants(DEBUG_KP, p_constant, new_p)
+ debugPIDConstants(DEBUG_KI, i_constant, new_i)
+ debugPIDConstants(DEBUG_KD, d_constant, new_d)
+
+ p_constant = new_p
+ i_constant = new_i
+ d_constant = new_d
+
+ while(millis() < t+2000): # debugging LED will light up for 2 seconds
+ pass
+
+ panel.singleLED(DEBUG_KP, 0, 0, 0)
+ panel.singleLED(DEBUG_KI, 0, 0, 0)
+ panel.singleLED(DEBUG_KD, 0, 0, 0)
+}
+
+def debugPIDConstants(lednum, oldval, newval):
+ if newval > oldval:
+ panel.singleLED(lednum, 0, 10, 0)
+ elif newval < oldval:
+ panel.singleLED(lednum, 10, 0, 0)
+ else:
+ panel.singleLED(lednum, 10, 10, 10)
+
+def loop():
+ if change_count == 0:
+ if queData == "?":
+ QueryVariables()
+ else:
+ ChangeVariables()
+ change_count += 1
+
+ panel.singleLED(DEBUG_BASESPEED, base_speed, base_speed, base_speed)
+
+ ## Standby Mode
+ panel.singleLED(DEBUG_STATE, 10, 0, 0)
+
+ ## LIDAR Detection
+ d_LIDAR = 0
+ ballCapture = 0
+ d_LIDAR = distanceSensor.getDistance()
+ print(d_LIDAR)
+
+ if (d_LIDAR < LIDAR_Thres) and (d_LIDAR > 0): # if we capture the ball
+ ballCapture = 1
+ print("found ball")
+ receivedData.DebugM = String(d_LIDAR)
+ else:
+ ballCapture = 2
+ print("no ball")
+ receivedData.DebugM = String(d_LIDAR)
+
+ ## Goal Finder
+ int id = -1;
+ int tx = 0; int ty = 0; int tz = 0;
+ int rx = 0; int ry = 0; int rz = 0;
+ int x = 0;
+ int y = 0;
+
+ # ballDetect = cam.exe_color_detection_biggestblob(threshold[0], threshold[1], threshold[2], threshold[3], threshold[4], threshold[5], x, y)
+ ballDetect = ML_detect_gb(gbx, gby)
+ goalDetect = cam.exe_goalfinder(goal_id[0],goal_id[1],goal_id[2], id, tx, ty, tz, rx, ry, rz)
+ ballCapture =
+
+ if ballCapture == 1 or debug_ball_cap == 1: # Ball captured
+ if goalDetect: # Goal detected
+ panel.singleLED(DEBUG_STATE, 0, 10, 0);
+ Inputx = tx/1.00;
+ Inputy = ty/1.00;
+ PID_x.Compute();
+ PID_y.Compute();
+ receivedData.DebugM = "det g mv2g";
+ send_message_once();
+ moveVertical(Outputy);
+ moveHorizontal(Outputx, base_speed);
+ else: # Goal not detected
+ seeking();
+ receivedData.DebugM = "seeking2";
+ send_message_once();
+ else: # Ball not captured
+ if ballDetect: # Ball detected
+ receivedData.DebugM = "catching b";
+ send_message_once();
+ Inputx = gbx/1.00
+ Inputy = gby/1.00
+ PID_x.Compute()
+ PID_y.Compute()
+ moveVertical(Outputy)
+ moveHorizontal(Outputx, base_speed)
+ else: # Ball not detected
+ seeking()
+ receivedData.DebugM = "seeking1"
+ send_message_once()
+ print_allvariables()
+
+if __name__ == '__main__':
+ while(True):
+ loop()
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main.py b/Code/Control/Laptop_Code/main.py
index 355087d0d7a510aadf85c3f6813dc4948d96ca5b..3805f1db98525b63fc8ff621e2b204e9cca96424 100644
--- a/Code/Control/Laptop_Code/main.py
+++ b/Code/Control/Laptop_Code/main.py
@@ -1,34 +1,31 @@
-import ball_detection.esp32cam_live_feed as live_feed
+import ball_detection.ball_detection as ball_detection
import serial
import time
-PORT = "COM6" # for Alianware
+PORT = "COM6" # for Alienware
# serial_port = serial.Serial(PORT,115200)
# serial_port.close()
# serial_port.open()
-gbx = 0
-gby = 0
+gbc = 0
+gbx = 0
+gby = 0
dist = 0
-gbc = 0
### Declare Varables and Constants
-imageLoc = './ball_detection/images/train-test-4/' #
-device = 'cuda'
-trainLoc = imageLoc + 'train/'
-labelSet = ['ball']
-modelLoc = './ball_detection/model_weights/' #
-modelFile = 'model_weights-2-20210818_002355-cpu.pth'
-modelAction = 'load'
+device = 'cuda'
+labelSet = ['ball']
+modelLoc = './ball_detection/model_weights/' #
+modelFile = 'model_weights-2-20210818_002355-cpu.pth'
+minDetectionScore = 0.90
-model = live_feed.returnModel(modelAction, device, trainLoc, labelSet, modelLoc, modelFile)
-
-waitTime = 0.1
+waitTime = 0.10
+model = ball_detection.returnModel(device, labelSet, modelLoc, modelFile)
while True:
## Testing (using Live Streaming)
- gbx, gby, dist = live_feed.detectLive(model, showSight = True)
+ gbx, gby, dist = ball_detection.detectLive(model, minDetectionScore, showSight = True)
intDist = int(dist) # cm
'''