From c2960f1c0b87e05ad0a0e1fdb5ccf6b2aa2958f4 Mon Sep 17 00:00:00 2001 From: Joe S <31870999+KenwoodFox@users.noreply.github.com> Date: Fri, 31 Jan 2020 13:43:21 -0500 Subject: [PATCH 1/5] Add ML camera source to urdf --- .../urdf/unnamed_toaster_description.urdf | 20 +++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/unnamed_toaster/unnamed_toaster/urdf/unnamed_toaster_description.urdf b/unnamed_toaster/unnamed_toaster/urdf/unnamed_toaster_description.urdf index 2ec9d4f..c7e1577 100644 --- a/unnamed_toaster/unnamed_toaster/urdf/unnamed_toaster_description.urdf +++ b/unnamed_toaster/unnamed_toaster/urdf/unnamed_toaster_description.urdf @@ -1,11 +1,11 @@ - + - + @@ -48,4 +48,20 @@ + + + + + + + + + + + + + + + + From cad2301c32aaf16bda42a8af2850fb034ec5aefd Mon Sep 17 00:00:00 2001 From: KhanSimeoni Date: Tue, 4 Feb 2020 17:56:55 -0500 Subject: [PATCH 2/5] files inserted d --- .../Make_TTB_Work.py | 16 ++++ .../test_network_receive.py | 96 +++++++++++++++++++ .../turn_to_ball.py | 50 ++++++++++ 3 files changed, 162 insertions(+) create mode 100644 unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py create mode 100644 unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py create mode 100644 unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/turn_to_ball.py diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py new file mode 100644 index 0000000..061f61f --- /dev/null +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py @@ -0,0 +1,16 @@ +#initialize and imports, do not edit +import sys +import time +from networktables import NetworkTables + +import logging +logging.basicConfig(level=logging.DEBUG) + +mlserver = NetworkTables +mlserver.initialize(server="10.17.21.2") +mltable = mlserver.getTable("ML") + +b1cx = 696969 + +def get_b1cx(): + return mltable.getNumber('ball1centerX', 0) diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py new file mode 100644 index 0000000..afbc65e --- /dev/null +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py @@ -0,0 +1,96 @@ +import sys +import time +from networktables import NetworkTables + +import logging # Required +logging.basicConfig(level=logging.DEBUG) + +if len(sys.argv) != 2: + print("Error: specify an IP to connect to!") + exit(0) + +ip = sys.argv[1] + +smoothLevel = 10000 +memRange = 10 + +NetworkTables.initialize(server=ip) +table = NetworkTables.getTable("ML") +i = 0 +p = 0 +ball1memX = 0 +ball2memX = 0 +tempmem1 = 0 +tempmem2 = 0 +while 1: + notEnoughBallage = False + object1posX = table.getNumberArray('boxes', [0,0,0,0]) + numBall = table.getNumber('nb_objects', 0) + try: + ball1centerX = ((object1posX[2]-object1posX[0])/2 + object1posX[0])/5.33 + except: + pass + + try: + ball2centerX = ((object1posX[6]-object1posX[4])/2 + object1posX[4])/5.33 + except: + pass + + if (numBall == 1): + try: + ball1memX = ball1centerX + i = 0 + except: + pass + + if (numBall == 2): + try: + ball2memX = ball2centerX + p = 0 + except: + pass + try: + ball1memX = ball1centerX + i = 0 + except: + pass + try: + if (tempmem2 >= (ball2memX - memRange) and tempmem2 <= (ball2memX + memRange)): + pass + if (tempmem1 >= (ball2memX - memRange ) and tempmem1 <= (ball2memX + memRange )): + ball1memX = tempmem2 + if (tempmem2 >= (ball1memX - memRange ) and tempmem2 <= (ball1memX + memRange )): + ball2memX = tempmem1 + if (tempmem1 >= (ball1memX - memRange ) and tempmem1 <= (ball1memX + memRange )): + pass + except: + pass + if (i >= smoothLevel): + ball1memX = 0 + else: + pass + + if (p >= smoothLevel): + ball2memX = 0 + else: + pass + + if (numBall >= 1): + print("The center of ball 1 is at " + str(ball1memX)) + print("The center of ball 2 is at " + str(ball2memX)) + elif (numBall > 2): + print("Too many balls") + elif (numBall <= 1): + if (i <= smoothLevel - 1): + print("The center of ball 1 is at " + str(ball1memX)) + if (p <= smoothLevel - 1): + print("The center of ball 2 is at " + str(ball2memX)) + if (p >= smoothLevel or i >= smoothLevel): + notEnoughBallage = True + if (notEnoughBallage == True): + print("Not enough ballage") + #print(ball1memX) + p = p + 1 + i = i + 1 + table.putNumber("ball1centerX", ball1memX) + table.putNumber("ball2centerX", ball2memX) \ No newline at end of file diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/turn_to_ball.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/turn_to_ball.py new file mode 100644 index 0000000..afea69d --- /dev/null +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/turn_to_ball.py @@ -0,0 +1,50 @@ +#initialize and imports, do not edit +import sys +import time +import Make_TTB_Work +from networktables import NetworkTables + +import logging +logging.basicConfig(level=logging.DEBUG) + + +rosserver = NetworkTables +rosserver.initialize(server="10.17.21.2") +NetworkTables.setServer([('10.17.21.2', 5800), ]) +rostable = rosserver.getTable("ROS") + + + + +#values for tweaking +turnSpeed = 0.5 +#math & code +while 1: + b1cx = Make_TTB_Work.get_b1cx() + + leftSpeed = 0 + rightSpeed = 0 + if (b1cx >=27 and b1cx <= 31): + leftSpeed = 0 + rightSpeed = 0 + elif(b1cx < 27 and b1cx != 0): + leftSpeed = turnSpeed + elif(b1cx > 31): + rightSpeed = turnSpeed + else: + leftSpeed = 0 + rightSpeed = 0 + + + rostable.putNumber("coprocessorPort", 1 ) + rostable.putNumber("coprocessorStarboard", 1 ) + + test = rostable.getNumber("coprocessorPort", 45) + + #print(rightSpeed) + #print(leftSpeed) + print(b1cx) + print(test) + + + From d8ad71226224ecaba91221c752f633d95d0b088e Mon Sep 17 00:00:00 2001 From: Joe S <31870999+KenwoodFox@users.noreply.github.com> Date: Tue, 4 Feb 2020 18:10:24 -0500 Subject: [PATCH 3/5] halfway done --- ...nterface.py => default_table_interface.py} | 25 +++-- .../src/unnamed_toaster_hw_interface/aim.py | 87 +++++++++++++++++ .../test_network_receive.py | 96 ------------------- 3 files changed, 105 insertions(+), 103 deletions(-) rename unnamed_toaster/unnamed_toaster_hw_interface/scripts/{limelight_table_interface.py => default_table_interface.py} (74%) mode change 100755 => 100644 create mode 100644 unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py delete mode 100644 unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/scripts/limelight_table_interface.py b/unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py old mode 100755 new mode 100644 similarity index 74% rename from unnamed_toaster/unnamed_toaster_hw_interface/scripts/limelight_table_interface.py rename to unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py index 8583069..5410d42 --- a/unnamed_toaster/unnamed_toaster_hw_interface/scripts/limelight_table_interface.py +++ b/unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py @@ -32,26 +32,37 @@ import rospy from unnamed_toaster_hw_interface.network_tables_interface import NetworkTablesInterface from unnamed_toaster_hw_interface.limelight import Limelight +from unnamed_toaster_hw_interface.aim import AIM -class LimelightTableInterface: +class DefaultTableInterface: def __init__(self): ip = rospy.get_param("~ip", "roboRIO-1721-FRC") # Get the parameter for the IP (fallback to DNS (DNS does not work on the field!)) self.rate = rospy.get_param("~rate", 50) # Get the paramater for the rate - self.table = NetworkTablesInterface("limelight", ip) # Get the table limelight from the server at "ip" - self.limelight = Limelight() + + # Get the table limelight from the server at "ip" + self.limelight_table = NetworkTablesInterface("limelight", ip) + self.limelight = Limelight() # Create a new instance of limelight + + # Get the table ml from the server at "ip" + self.ml_table = NetworkTablesInterface("ML", ip) + self.aim = AIM() # Create a new instance of AIM def run(self): rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): - self.limelight.update(self.table) + self.limelight.update(self.limelight_table) self.limelight.publish() + self.aim.update(self.ml_table) + self.aim.publish() + # Put other logic and data here to run other attached scripts + # Sleep rate.sleep() if __name__=="__main__": - rospy.init_node("limelight") - table = LimelightTableInterface() - table.run() + rospy.init_node("default_table_node") + table = DefaultTableInterface() + table.run() \ No newline at end of file diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py new file mode 100644 index 0000000..7399769 --- /dev/null +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py @@ -0,0 +1,87 @@ +import rospy + +class AIM(): + + def __init__(self): + smoothLevel = get_param("~smooth_level", 10000) # Parameter name smooth_level will have default value unless overriden + memRange = 10 + + i = 0 + p = 0 + ball1memX = 0 + ball2memX = 0 + tempmem1 = 0 + tempmem2 = 0 + + def update(self, table): + notEnoughBallage = False + object1posX = table.getNumberArray('boxes', [0,0,0,0]) + numBall = table.getNumber('nb_objects', 0) + try: + ball1centerX = ((object1posX[2]-object1posX[0])/2 + object1posX[0])/5.33 + except: + pass + + try: + ball2centerX = ((object1posX[6]-object1posX[4])/2 + object1posX[4])/5.33 + except: + pass + + if (numBall == 1): + try: + ball1memX = ball1centerX + i = 0 + except: + pass + + if (numBall == 2): + try: + ball2memX = ball2centerX + p = 0 + except: + pass + try: + ball1memX = ball1centerX + i = 0 + except: + pass + try: + if (tempmem2 >= (ball2memX - memRange) and tempmem2 <= (ball2memX + memRange)): + pass + if (tempmem1 >= (ball2memX - memRange ) and tempmem1 <= (ball2memX + memRange )): + ball1memX = tempmem2 + if (tempmem2 >= (ball1memX - memRange ) and tempmem2 <= (ball1memX + memRange )): + ball2memX = tempmem1 + if (tempmem1 >= (ball1memX - memRange ) and tempmem1 <= (ball1memX + memRange )): + pass + except: + pass + if (i >= smoothLevel): + ball1memX = 0 + else: + pass + + if (p >= smoothLevel): + ball2memX = 0 + else: + pass + + if (numBall >= 1): + print("The center of ball 1 is at " + str(ball1memX)) + print("The center of ball 2 is at " + str(ball2memX)) + elif (numBall > 2): + print("Too many balls") + elif (numBall <= 1): + if (i <= smoothLevel - 1): + print("The center of ball 1 is at " + str(ball1memX)) + if (p <= smoothLevel - 1): + print("The center of ball 2 is at " + str(ball2memX)) + if (p >= smoothLevel or i >= smoothLevel): + notEnoughBallage = True + if (notEnoughBallage == True): + print("Not enough ballage") + #print(ball1memX) + p = p + 1 + i = i + 1 + table.putNumber("ball1centerX", ball1memX) + table.putNumber("ball2centerX", ball2memX) \ No newline at end of file diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py deleted file mode 100644 index afbc65e..0000000 --- a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/test_network_receive.py +++ /dev/null @@ -1,96 +0,0 @@ -import sys -import time -from networktables import NetworkTables - -import logging # Required -logging.basicConfig(level=logging.DEBUG) - -if len(sys.argv) != 2: - print("Error: specify an IP to connect to!") - exit(0) - -ip = sys.argv[1] - -smoothLevel = 10000 -memRange = 10 - -NetworkTables.initialize(server=ip) -table = NetworkTables.getTable("ML") -i = 0 -p = 0 -ball1memX = 0 -ball2memX = 0 -tempmem1 = 0 -tempmem2 = 0 -while 1: - notEnoughBallage = False - object1posX = table.getNumberArray('boxes', [0,0,0,0]) - numBall = table.getNumber('nb_objects', 0) - try: - ball1centerX = ((object1posX[2]-object1posX[0])/2 + object1posX[0])/5.33 - except: - pass - - try: - ball2centerX = ((object1posX[6]-object1posX[4])/2 + object1posX[4])/5.33 - except: - pass - - if (numBall == 1): - try: - ball1memX = ball1centerX - i = 0 - except: - pass - - if (numBall == 2): - try: - ball2memX = ball2centerX - p = 0 - except: - pass - try: - ball1memX = ball1centerX - i = 0 - except: - pass - try: - if (tempmem2 >= (ball2memX - memRange) and tempmem2 <= (ball2memX + memRange)): - pass - if (tempmem1 >= (ball2memX - memRange ) and tempmem1 <= (ball2memX + memRange )): - ball1memX = tempmem2 - if (tempmem2 >= (ball1memX - memRange ) and tempmem2 <= (ball1memX + memRange )): - ball2memX = tempmem1 - if (tempmem1 >= (ball1memX - memRange ) and tempmem1 <= (ball1memX + memRange )): - pass - except: - pass - if (i >= smoothLevel): - ball1memX = 0 - else: - pass - - if (p >= smoothLevel): - ball2memX = 0 - else: - pass - - if (numBall >= 1): - print("The center of ball 1 is at " + str(ball1memX)) - print("The center of ball 2 is at " + str(ball2memX)) - elif (numBall > 2): - print("Too many balls") - elif (numBall <= 1): - if (i <= smoothLevel - 1): - print("The center of ball 1 is at " + str(ball1memX)) - if (p <= smoothLevel - 1): - print("The center of ball 2 is at " + str(ball2memX)) - if (p >= smoothLevel or i >= smoothLevel): - notEnoughBallage = True - if (notEnoughBallage == True): - print("Not enough ballage") - #print(ball1memX) - p = p + 1 - i = i + 1 - table.putNumber("ball1centerX", ball1memX) - table.putNumber("ball2centerX", ball2memX) \ No newline at end of file From 0384763c073b1b3df3817525c584bb06120f5e67 Mon Sep 17 00:00:00 2001 From: Joe S <31870999+KenwoodFox@users.noreply.github.com> Date: Tue, 4 Feb 2020 18:20:07 -0500 Subject: [PATCH 4/5] Publish the stuff here --- .../scripts/default_table_interface.py | 2 +- .../Make_TTB_Work.py | 16 ---------------- .../src/unnamed_toaster_hw_interface/aim.py | 5 +++-- 3 files changed, 4 insertions(+), 19 deletions(-) delete mode 100644 unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py b/unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py index 5410d42..410586b 100644 --- a/unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py +++ b/unnamed_toaster/unnamed_toaster_hw_interface/scripts/default_table_interface.py @@ -55,7 +55,7 @@ def run(self): self.limelight.publish() self.aim.update(self.ml_table) - self.aim.publish() + self.aim.publish(self.ml_table) # Put other logic and data here to run other attached scripts # Sleep diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py deleted file mode 100644 index 061f61f..0000000 --- a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/Make_TTB_Work.py +++ /dev/null @@ -1,16 +0,0 @@ -#initialize and imports, do not edit -import sys -import time -from networktables import NetworkTables - -import logging -logging.basicConfig(level=logging.DEBUG) - -mlserver = NetworkTables -mlserver.initialize(server="10.17.21.2") -mltable = mlserver.getTable("ML") - -b1cx = 696969 - -def get_b1cx(): - return mltable.getNumber('ball1centerX', 0) diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py index 7399769..a4cc13f 100644 --- a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py @@ -83,5 +83,6 @@ def update(self, table): #print(ball1memX) p = p + 1 i = i + 1 - table.putNumber("ball1centerX", ball1memX) - table.putNumber("ball2centerX", ball2memX) \ No newline at end of file + + def publish(self, table): + # Publish the stuff here \ No newline at end of file From c08fff1c8d0351846f5c0194912acafb6ce15b15 Mon Sep 17 00:00:00 2001 From: KhanSimeoni Date: Tue, 4 Feb 2020 18:26:57 -0500 Subject: [PATCH 5/5] gited --- .../src/unnamed_toaster_hw_interface/aim.py | 35 ++++++++++++++++++- 1 file changed, 34 insertions(+), 1 deletion(-) diff --git a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py index a4cc13f..0ffa15c 100644 --- a/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py @@ -12,6 +12,8 @@ def __init__(self): ball2memX = 0 tempmem1 = 0 tempmem2 = 0 + + self.turnSpeed = 0.5 def update(self, table): notEnoughBallage = False @@ -85,4 +87,35 @@ def update(self, table): i = i + 1 def publish(self, table): - # Publish the stuff here \ No newline at end of file + #values for tweaking + + #math & code + while 1: + b1cx = Make_TTB_Work.get_b1cx() + + leftSpeed = 0 + rightSpeed = 0 + if (b1cx >=27 and b1cx <= 31): + leftSpeed = 0 + rightSpeed = 0 + elif(b1cx < 27 and b1cx != 0): + leftSpeed = self.turnSpeed + elif(b1cx > 31): + rightSpeed = self.turnSpeed + else: + leftSpeed = 0 + rightSpeed = 0 + + + print("LS: " + str(leftSpeed)) + print("RS: " + str(rightSpeed)) + + #test = rostable.getNumber("coprocessorPort", 45) + + #print(rightSpeed) + #print(leftSpeed) + print("B1CX: " + str(b1cx)) + #print(test) + + +