Add ML code to LAPIS

unnamed_toaster/unnamed_toaster/urdf/unnamed_toaster_description.urdf

@@ -1,11 +1,11 @@
 <robot name="unnamed">
 
   <material name="green">
-	<color rgba="0 1 0 1"/>
+    <color rgba="0 1 0 1"/>
   </material>
 
   <material name="grey">
-	<color rgba="0.1 0.1 0.1 1"/>
+    <color rgba="0.1 0.1 0.1 1"/>
   </material>
 
   <link name="base_link">
@@ -48,4 +48,20 @@
 	  <material name="green"/>
     </visual>
   </link>
+
+  <joint name="base_link_to_ml_link" type="fixed">
+    <parent link="base_link"/>
+    <child link="ml_link"/>
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+  </joint>
+
+  <link name="ml_link">
+    <visual>
+      <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.0254 0.09779 0.05715 "/>
+      </geometry>
+	  <material name="grey"/>
+    </visual>
+  </link>
 </robot>

unnamed_toaster/unnamed_toaster_hw_interface/scripts/limelight_table_interface.py → 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()

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)
+
+
+

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)
+
+
+