diff --git a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
index f2704f63c156bbccaf2b60e498b34b2cc4270080..b11be5f9df94ba962c821fca7c15eabf8c86c55b 100644
--- a/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
+++ b/Code/Control/Laptop_Code/ESP32_slave/ESP32_slave.ino
@@ -16,7 +16,7 @@
#define Sensor2_newAddress 45
// MAC Address of the receiver (MASTER)
-int masterID = 14;
+int masterID = 13;
uint8_t broadcastAddresses[][6] = {{0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}, // #1
{0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}, // #2
{0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}, // #3
@@ -173,6 +173,8 @@ void setup() {
Wire.setClock(100000);
pinMode(XSHUT_pin1, INPUT);
+ pinMode(XSHUT_pin2, INPUT);
+
if (!Sensor1.init()){
Serial.println("Failed to detect and initialize sensor1!");
Lidar1_flag = 0;
@@ -186,7 +188,6 @@ void setup() {
}
- pinMode(XSHUT_pin2, INPUT);
if (!Sensor2.init())
{
Serial.println("Failed to detect and initialize sensor2!");
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 5bea05d4d7d09133efb72e175f6347d507ffa69a..4eb996d44138c38b08cc0a37b42b383b8e232229 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
@@ -4,7 +4,7 @@
// ==================================== broadcast address ============================================
// MAC Address of the receiver (SLAVE)
-int slaveID = 9;
+int slaveID = 7;
uint8_t broadcastAddresses[][6] = {{0xC4, 0xDD, 0x57, 0x9E, 0x91, 0x74}, // #1
{0x40, 0xF5, 0x20, 0x44, 0xBD, 0xF8}, // #2
{0xC4, 0xDD, 0x57, 0x9E, 0x8A, 0x98}, // #3
diff --git a/Code/Control/Laptop_Code/constants.py b/Code/Control/Laptop_Code/constants.py
index cf107ee169462aa071eba5fb4eb5f210e4060979..1169488e3bb52d9631582ea743e1f4421143df51 100644
--- a/Code/Control/Laptop_Code/constants.py
+++ b/Code/Control/Laptop_Code/constants.py
@@ -30,15 +30,14 @@ Bmin = 31
Bmax = 127
threshold = [Lmin, Lmax, Amin, Amax, Bmin, Bmax]
-base_speed = 120
+base_speed = 100
seeking_speed = 50
LIDAR_Thres = 300 # mm
-
# PID Control in move2ball
-kpx,kix,kdx = 1.2, 0.01, 0.5
+kpx,kix,kdx = 0.8, 0.01, 0.2
# kpx,kix,kdx = 0.0, 0.00, 0.0
-kpy,kiy,kdy = 1.2, 0.05, 0.9
+kpy,kiy,kdy = 0.9, 0.1, 0.2
# kpy,kiy,kdy = 0.0, 0.0, 0.0
# PID Control in move2goal
@@ -58,5 +57,10 @@ kph,kih,kdh = 1.2,0.01,0.5
# Break between AT detection during ball seeking
AT_detectBreak = 60
-url_AT = 'http://10.0.0.5/cam-hi.jpg' # 1
-url_gb = 'http://10.0.0.4/cam-hi.jpg' # 6
\ No newline at end of file
+url_AT = 'http://10.0.0.9/cam-hi.jpg' # 1
+url_gb = 'http://10.0.0.5/cam-hi.jpg' # 6
+
+ml = 1
+esp_cam_on = 1
+openmv_on = 1
+seekVertDir = 1
\ No newline at end of file
diff --git a/Code/Control/Laptop_Code/main_joystick.py b/Code/Control/Laptop_Code/main_joystick.py
index bd8635bdc14cf840d45cebfc25f4815e2fb2d3a4..5281a41012e8824d6a29b71b2f355e6f6c7a5cc0 100644
--- a/Code/Control/Laptop_Code/main_joystick.py
+++ b/Code/Control/Laptop_Code/main_joystick.py
@@ -4,7 +4,7 @@ import time
import simple_pid.PID as PID
from constants import *
from ESP32_AT.imageTread_AT import get_AT_6DOF_info
-from ESP32_AT.imageTread_AT_multiple import get_AT_6DOF_info_list
+# from ESP32_AT.imageTread_AT_multiple import get_AT_6DOF_info_list
global mc_print,ml_on,esp_cam_on,feather_data_on
@@ -56,7 +56,7 @@ def manual_horizontal():
return abs(pwm3),abs(pwm4),dir3,dir4
-def manual_control(serial_port):
+def manual_control():
pwm1,pwm2,dir1,dir2 = manual_vertical()
pwm3,pwm4,dir3,dir4 = manual_horizontal()
if mc_print == 1:
@@ -143,29 +143,29 @@ def serial_port_out(serial_port, Ctl_com):
def test_function():
- # tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url_AT)
- tids, txs, tys, tzs, rxs, rys, rzs = get_AT_6DOF_info_list(url_AT)
+ tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url_AT)
+ # tids, txs, tys, tzs, rxs, rys, rzs = get_AT_6DOF_info_list(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("tid:{}".format(tids))
- print("tx,ty,tz:{},{},{}".format(txs,tys,tzs))
- print("rx,ry,rz:{},{},{}".format(rxs,rys,rzs))
+ print("tid:{}".format(tid))
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+ print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+ # print("tid:{}".format(tids))
+ # print("tx,ty,tz:{},{},{}".format(txs,tys,tzs))
+ # print("rx,ry,rz:{},{},{}".format(rxs,rys,rzs))
print("gbx,gby,gb_dist:{},{},{}".format(gbx,gby,gb_dist))
-def auto_init(serial_port,auto_init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM):
+def auto_init(serial_port,auto_init_count):
print("This is auto_init")
auto_init_count += 1
bcap_man = -1 # default: use lidar to determine
- Ctl_com = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, bcap_man)
- serial_port_out(serial_port, Ctl_com)
+ # Ctl_com = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, bcap_man)
+ # serial_port_out(serial_port, Ctl_com)
return auto_init_count,bcap_man
-def auto_control(serial_port):
+def auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM):
# Ctl_com = [0, 0, 0, 0, "+", "+", "+", "+"]
if ml_on == 1:
@@ -182,6 +182,9 @@ def auto_control(serial_port):
if esp_cam_on == 1:
# ids,txs,tys,tzs,rxs,rys,rzs = get_AT_6DOF_info_list(url_AT)
tid, tx, ty, tz, rx, ry, rz = get_AT_6DOF_info(url_AT)
+ # LIDAR_dist1 = 0
+ # LIDAR_dist2 = 0
+ # debugM = "Using Esp32 cam"
# main auto control loop
Ctl_com = main_control(gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM, bcap_man)
@@ -189,6 +192,7 @@ def auto_control(serial_port):
return Ctl_com
def manual_in_auto(Ctl_com, serial_port, flag):
+ done = 1
if joystick.get_button(axes_win.get('button_LB')):
flag = 12
print("manual in auto")
@@ -487,7 +491,7 @@ if __name__ == '__main__':
# =========== SET UP ============
# Defining Variables for ESP 32 Serial I/O
- PORT = "COM9" # for Alienware
+ PORT = "COM19" # for Alienware
serial_port = serial.Serial(PORT, 115200)
serial_port.close()
serial_port.open()
@@ -543,6 +547,8 @@ if __name__ == '__main__':
flag = 0
print_count = 0
auto_init_count = 0
+ bcap_man = -1
+
while not done:
done = pygame_init(done)
@@ -550,7 +556,7 @@ if __name__ == '__main__':
joystick = pygame.joystick.Joystick(controller_id)
joystick.init()
- if joystick.get_button(axes_win.get('button_start')) and joystick.get_button(axes_win.get('button_back')):
+ if joystick.get_button(axes_win.get('button_LB')) and joystick.get_button(axes_win.get('button_RB')):
print("kill the program")
done = True
@@ -559,9 +565,9 @@ if __name__ == '__main__':
flag = 1
while (flag==1):
done = pygame_init(done)
- if auto_init_count == 0:
- auto_init_count,bcap_man = auto_init(serial_port,auto_init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
- Ctl_com = auto_control(serial_port)
+ # if auto_init_count == 0:
+ # auto_init_count,bcap_man = auto_init(serial_port,auto_init_count,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
+ Ctl_com = auto_control(serial_port,gbx, gby, gb_dist, tx, ty, tz, rx, ry, rz, LIDAR_dist1, LIDAR_dist2, debugM)
serial_port_out(serial_port, Ctl_com) # send the control data out
time.sleep(waitTime)