From 4638779af77eef20efcc4fb89a70721a00fea448 Mon Sep 17 00:00:00 2001
From: philip <philip@randomsmiths.com>
Date: Sat, 23 Mar 2024 03:03:22 -0400
Subject: [PATCH] nitin is crazy

---
 .../urdf/robots/bloodstone.urdf.xacro         |  6 +-
 src/grr_guis/grr_guis/main_gui.py             |  3 +-
 .../grr_hardware_python/robot.py              | 16 +++--
 .../grr_python_controllers/state_machine.py   | 14 +++++
 .../state_actions/drive_to_gap.py             | 63 ++++++++++++-------
 5 files changed, 69 insertions(+), 33 deletions(-)

diff --git a/src/grr_description/urdf/robots/bloodstone.urdf.xacro b/src/grr_description/urdf/robots/bloodstone.urdf.xacro
index 97318c5..2214424 100755
--- a/src/grr_description/urdf/robots/bloodstone.urdf.xacro
+++ b/src/grr_description/urdf/robots/bloodstone.urdf.xacro
@@ -12,7 +12,7 @@
   <xacro:include filename="$(find grr_description)/urdf/parts/small_package_sweeper.xacro"/>
   <xacro:include filename="$(find grr_description)/urdf/parts/bridge.xacro"/>
   <xacro:include filename="$(find grr_description)/urdf/parts/lidar.xacro"/>
-  <xacro:include filename="$(find grr_description)/urdf/parts/imu.xacro"/>
+  <!-- <xacro:include filename="$(find grr_description)/urdf/parts/imu.xacro"/> -->
   <xacro:include filename="$(find grr_description)/urdf/parts/zed_cam.xacro"/>
   <xacro:include filename="$(find grr_description)/urdf/parts/usb_camera.xacro"/>
   <xacro:include filename="$(find grr_description)/urdf/control/drive.xacro"/>
@@ -112,12 +112,12 @@
     parent="chassis_link"
     origin="0.0 -0.08 0.145"
   />
-  <xacro:imu 
+  <!-- <xacro:imu 
     parent="camera_stand_link"
     origin="0 0 0.1"
     radius="0.01"
     length="0.02"
-  />
+  /> -->
   <!-- <xacro:lidar
     radius="${lidar_radius}"
     length="${lidar_length}"
diff --git a/src/grr_guis/grr_guis/main_gui.py b/src/grr_guis/grr_guis/main_gui.py
index be37d33..54c73e6 100644
--- a/src/grr_guis/grr_guis/main_gui.py
+++ b/src/grr_guis/grr_guis/main_gui.py
@@ -130,6 +130,7 @@ def kill_motor(self, checked):
         self.drive_train_enable = not checked
         self.effort_pub.publish(Float64MultiArray(data=[0.0, 0.0]))
         self.raw_cmd.publish(Twist())
+        self.drivetrain_enable.publish(Bool(data=not checked))
         
     def send_start(self, checked):
         self.window.start_button.setText(f"Ready to Start? {'Yes' if checked else 'No '}")
@@ -166,7 +167,7 @@ def gui_loop(self):
         nodes = [((y + "/") if not y  == "/" else "/") + x for x,y in self.get_node_names_and_namespaces()]
         if not nodes == self.previous_nodes:
             self.display_nodes(nodes)
-        self.drivetrain_enable.publish(Bool(data=self.drive_train_enable))
+
             
 
 def main():
diff --git a/src/grr_hardware_python/grr_hardware_python/robot.py b/src/grr_hardware_python/grr_hardware_python/robot.py
index a264292..1e5d036 100644
--- a/src/grr_hardware_python/grr_hardware_python/robot.py
+++ b/src/grr_hardware_python/grr_hardware_python/robot.py
@@ -73,7 +73,7 @@ def __init__(self) -> None:
         self.relay = digitalio.DigitalInOut(board.D6)
         self.relay.direction = digitalio.Direction.OUTPUT
         
-        self.sensor_loop_timer = self.create_timer(.1, self.sensor_loop)
+        self.sensor_loop_timer = self.create_timer(1/40., self.sensor_loop)
         
         self.back_sensor = TCS34725(self.tca[1])
         self.start_light_publisher = self.create_publisher(Bool, "/grr/start_light", 10) 
@@ -177,13 +177,13 @@ def line_array(self):
         
     def sensor_loop(self):
         
+        gui_str = ""
+        
         try:
             threshold = self.get_parameter('LED_Threshold').get_parameter_value().integer_value
             color_rgb = self.back_sensor.color_rgb_bytes
             msg = Bool()
             msg.data = color_rgb[1] >= threshold
-            str_msg = String(data=str(color_rgb))
-            self.debug_pub.publish(str_msg) 
             self.start_light_publisher.publish(msg)
         except OSError as e:
             self.get_logger().warning(f"BACK COLOR FAILED {e}")
@@ -193,6 +193,7 @@ def sensor_loop(self):
             tof_msg = LaserScan(range_min=.005,range_max=.1)
             tof_msg.ranges = [self.down_tof.range / 1000]
             self.down_tof_pub.publish(tof_msg)
+            gui_str += str(tof_msg.ranges) 
         except OSError as e:
             self.get_logger().warning(f"DOWN TOF FAILED {e}")
         
@@ -204,20 +205,23 @@ def sensor_loop(self):
             head.frame_id = "imu_link"
             mf_msg.header = head
             imu_msg.header = head
-            mf_msg.magnetic_field = Vector3(x=x/(10^6), y=y/(10^6), z=z/(10^6))
+            mf_msg.magnetic_field = Vector3(x=(x - 46.30225080385853)/(10^6), y=(y+48.72113417129494)/(10^6), z=(z-213.32943583747442)/(10^6))
             
             ax, ay, az = self.gyro.acceleration #m/s^2
             gx, gy, gz = self.gyro.gyro # rad / s
             
-            imu_msg.angular_velocity = Vector3(x=gx, y=gy, z=gz)
+            imu_msg.angular_velocity = Vector3(x=gx-0.002748893571891069, y=gy+0.08697193828844244, z=gz-0.18639025580516944)
             imu_msg.linear_acceleration = Vector3(x=ax, y=ay, z=az)
             
             self.magnometer_pub.publish(mf_msg)
-            self.imu_pub.publish(imu_msg)
+            self.imu_pub.publish(imu_msg)            
+        
             
             
         except OSError as e:
             self.get_logger().warning(f"MAGNOMETER FAILED {e}")
+            
+        self.debug_pub.publish(String(data=gui_str))
         
         
         # try:
diff --git a/src/grr_python_controllers/grr_python_controllers/state_machine.py b/src/grr_python_controllers/grr_python_controllers/state_machine.py
index 32dad40..4e075a4 100644
--- a/src/grr_python_controllers/grr_python_controllers/state_machine.py
+++ b/src/grr_python_controllers/grr_python_controllers/state_machine.py
@@ -137,6 +137,20 @@ def set_true(): self.promo_display=True
             Action()
         )
         
+        self.action_tree.setNext(
+            CornerReset(Pose(position=Point(x=1.40, y=0.55)))
+        ).setNext(
+            DriveToGap()
+        ).setNext(
+            DriveToPose(Pose(position=Point(x=1.24, y=0.55)), name="back up for bridge") #CHANGE Y
+        ).setNext(
+            ServoAction(JointState(name=['bridge_latch_joint'], position=[0.0]), 1, "Drop Bridge")
+        ).setNext(
+            DriveToPose(Pose(position=Point(x=-0.1,y=0.55)), name="across the bridge into the wall")
+        ).setNext(
+            Action()
+        )
+        
         # self.action_tree.setNext(
         #     ServoAction(JointState(name=['small_package_sweeper_joint', 'bridge_latch_joint', 'mechanism_package_joint', 'mechanism_thruster_joint', 'mechanism_lift_joint'], position=[0.0, 100.0, 35.0, 0.0, 0.0]), 2, name="Go down for thrusters")
         # ).setNext(
diff --git a/src/grr_python_controllers/state_actions/drive_to_gap.py b/src/grr_python_controllers/state_actions/drive_to_gap.py
index 09a1328..104ca5b 100644
--- a/src/grr_python_controllers/state_actions/drive_to_gap.py
+++ b/src/grr_python_controllers/state_actions/drive_to_gap.py
@@ -7,6 +7,7 @@
 from std_msgs.msg import Bool
 
 from state_actions.action import Action
+from state_actions.corner_reset import CornerReset
 
 import numpy as np
 class DriveToGap(Action):
@@ -18,34 +19,50 @@ def __init__(self, name:str=None):
             
         self.state = 0
         
-        self.normal_readings = []        
+        # state 0 == Flat
+        # State 1 == hill going up
+        # state 2 == Flat on hill
+        # state 3 == over gap
+        
+        self.normal_readings = []     
+        self.timesThroughState = 0
+        
+    def reset_on_edge(self, node): # >= 0.06
+        node.raw_cmd.publish(Twist())
+        newPose = Pose(position=Point(x=1.18, y=node.position.position.y))
+        self.setNext(CornerReset(newPose))
 
     def run(self, node):
-        reading = node.magnent_data.z
+        reading = node.tof_data["down"]
         node.drivetrain_enable.publish(Bool(data=False))
         
         node.get_logger().info(f"State: {self.state}, reading: {reading}")
         
-        if node.tof_data["down"] >= 0.06:
-            node.raw_cmd.publish(Twist())
-            newPose = Pose(position=Point(x=1.14, y=node.position.position.y))
-            node.new_pose.publish(newPose)
-            node.goal_pub.publish(newPose)
-            node.drivetrain_enable.publish(Bool(data=True))
-            return self.nextAction
-        
-        if self.state >= 2:
-            node.raw_cmd.publish(Twist(linear=Vector3(x=-0.05)))
-        else:
-            node.raw_cmd.publish(Twist(linear=Vector3(x=-0.42)))
-            
-        if self.state == 0:
-            if reading >= 92.0:
-                self.state = 1
-        if self.state == 1:
-            print("HELLO")
-            if reading <= 91.0:
-                print("this shouldnt print")
-                self.state = 2
+        speed = -0.05
+        
+        match(self.state):
+            case 0: #flat pre hill
+                speed = -0.45
+                if reading <= 0.008:
+                    self.state = 1
+            case 1:
+                speed = -0.45
+                if reading >= 0.018:
+                    print("OVER THE EDGE")
+                    self.state = 2
+            case 2: # flat top of hill
+                speed = -0.1 - ((30 - self.timesThroughState)) * 0.01
+                self.timesThroughState += 1
+                if self.timesThroughState >= 30:
+                    self.state = 3
+            case 3:
+                if reading >= 0.015:
+                    speed = 0.0
+                    self.reset_on_edge(node)
+                    return self.nextAction
+        
+        
+        
+        node.raw_cmd.publish(Twist(linear=Vector3(x=speed)))
     
         return self