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 @@ + + + + + + + + + + + + + + + + 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..410586b --- 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(self.ml_table) + # 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..0ffa15c --- /dev/null +++ b/unnamed_toaster/unnamed_toaster_hw_interface/src/unnamed_toaster_hw_interface/aim.py @@ -0,0 +1,121 @@ +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 + + self.turnSpeed = 0.5 + + 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 + + def publish(self, table): + #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) + + + 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) + + +