Update

Control/Jetson/dronekit_server_client.py

@@ -160,16 +160,14 @@ class DroneKitComms:
         _ = self.make_request(self.nudge_fork_vert_endpoint, data)
 
     def zero_fork_horiz(self):
-        self.nudge_fork_horiz(go_left=True, amount=0.9)
         self.nudge_fork_horiz(go_left=True, amount=0.9)
         self.nudge_fork_horiz(go_left=True, amount=0.9)
         time.sleep(2)
-        self.nudge_fork_horiz(go_left=False, amount=0.9)
+        self.nudge_fork_horiz(go_left=False, amount=1.0)
-        self.nudge_fork_horiz(go_left=False, amount=0.4)  # TODO: tune this
 
 
     def approach_pallet(self):
-        _ = self.make_request(self.gps_target_radius_km)
+        _ = self.make_request(self.approach_pallet_endpoint)
         # print(rsp)
 
 

Control/Jetson/vision_cargo_approach.py

@@ -17,9 +17,13 @@ class Resolution:
     width: int
     height: int
 
-def remap(in_value, in_min, in_max, out_min, out_max):
+def remap(value, maxInput, minInput, maxOutput, minOutput):
-    remapped = (((in_value - in_min) * (out_max - out_min)) / (in_max - in_min)) + out_min
+	value = maxInput if value > maxInput else value
-    return remapped
+	value = minInput if value < minInput else value
+	inputSpan = maxInput - minInput
+	outputSpan = maxOutput - minOutput
+	scaledThrust = float(value - minInput) / float(inputSpan)
+	return minOutput + (scaledThrust * outputSpan)
 
 
 class VisionSystem:
@@ -96,8 +100,7 @@ class VisionSystem:
             cv2.putText(frame, str(botom_right), (int(corners[2][0]), int(corners[2][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA)
             cv2.putText(frame, str(bottom_left), (int(corners[3][0]), int(corners[3][1])), font, 0.7, (255, 0, 255), 2, cv2.LINE_AA)
             tag_area = self.get_apriltag_size(top_left, top_right, botom_right)
-            print(tag_area)
+
-            print('---')
             pos_x = 2 * (center[0] - (self.resolution.width/2)) / self.resolution.width
             pos_y = 2 * (center[1] - (self.resolution.height/2)) / self.resolution.height
             clean_pos_y = float("{0:.3f}".format(pos_y))
@@ -122,7 +125,7 @@ class VisionSystem:
 
 class CargoApproachingSystem:
     def __init__(self) -> None:
-        self.ky = 2.2
+        self.ky = 2
         self.kt = -1
 
     def calculate(self, ey, et):
@@ -130,62 +133,6 @@ class CargoApproachingSystem:
         return delta
 
 
-class Tester:
-    def __init__(self) -> None:
-        self.log = logging.getLogger("TESTER")
-        self.log.setLevel("DEBUG")
-        self.camera = VisionSystem()
-        self.steering_law = CargoApproachingSystem()
-        self.no_detects = 0
-        self.max_approach_area_thresh = 40000
-        self.centre_distance_thresh = (-0.04, 0.08)
-        self.centre_angle_thresh = (-0.2, 0.2)
-
-    def run_pc(self):
-        while True:
-            pallet_pose = self.camera.get_pallet_pose_pc()
-            if pallet_pose:
-                position, rotation, area = pallet_pose
-                error_y = position[0]
-                error_theta = rotation[1]
-                # print(error_y, error_theta)
-                delta = self.steering_law.calculate(error_y, error_theta)
-                # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000)
-                delta_PWM = remap(delta, -0.7, 0.7, 500, 2500)
-                print(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}")
-            cv2.waitKey(1)
-
-    def run_jetson(self):
-        self.log.info("Starting vision system")
-        self.is_running = True
-        while self.is_running:
-            time.sleep(0.05)
-            pallet_pose = self.camera.get_pallet_pose()  # getting the position, rotation and size of the tracker
-            if pallet_pose:  # If we found a tracker
-                self.no_detects = 0
-                position, rotation, area = pallet_pose
-                error_y = position[0]  # centre offset distance
-                error_theta = rotation[1]  # centre offset angle
-                # print(error_y, error_theta)
-                delta = self.steering_law.calculate(error_y, error_theta)  # getting the steering angle from the control law
-                delta_PWM = remap(delta, -0.7, 0.7, 500, 2500)  # Converting steering law output into PWM values
-                self.log.info(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}")
-                if area < self.max_approach_area_thresh:  # If we are far enough from the cargo, move towards it
-                    self.log.info(f"Setting speed: 1950, steering: {delta_PWM}")
-                else:
-                    self.log.info(f"We are close enough to cargo!, {area}")
-                    is_aligned = self.camera.check_cargo_alignment(self.centre_distance_thresh, self.centre_angle_thresh, (error_y, error_theta))
-                    if is_aligned:
-                        self.log.info(f'Cargo aligned: {error_y} in {self.centre_distance_thresh} and {error_theta} in {self.centre_angle_thresh}')
-                    else:
-                        self.log.info(f'Cargo NOT aligned: {error_y} not in {self.centre_distance_thresh} or {error_theta} inot n {self.centre_angle_thresh}')
-
-            else:
-                self.no_detects +=1
-                if self.no_detects > 3:  # if we do not detect anything 3 times in a row, stop the vehicle
-                    self.log.info("No cargo detected, setting speed to 1500")
-
-
 class VisionCargoApproacher:
     def __init__(self, forklift_comms) -> None:
         self.log = logging.getLogger("VISION CARGO APPROACHER")
@@ -201,6 +148,7 @@ class VisionCargoApproacher:
         self.centre_distance_thresh = (-0.2, 0.2)  # horizontal tolerance for pallet alignment pre fork alignment
         self.centre_angle_thresh = (-0.2, 0.2)  # angular tolerance for pallet alignment pre fork alignment
         self.horizontal_camera_offset = 0  # In case the camera isn't perfectly straight, we can compansate for that in code
+        self.steering_log_file = open("slf.file", 'w+')
 
     def navigate_to_cargo_vision(self):
         self.log.info("Starting vision system")
@@ -217,13 +165,12 @@ class VisionCargoApproacher:
                 error_x += self.horizontal_camera_offset  # adding offset
                 # print(error_x, error_theta)
                 delta = self.steering_law.calculate(error_x, error_theta)  # getting the steering angle from the control law
-                self.log.info("***")
-                self.log.info(f"Error y {delta} | error theta {error_x} | delta {error_theta}")
-                self.log.info("***")
                 # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000)
-                delta_PWM = remap(delta, -0.7, 0.7, 3000, 1)  # Converting steering law output into PWM values
+                delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100)  # Converting steering law output into PWM values
-                self.log.info(f"PWM: {delta_PWM}, ey: {error_x}, et: {error_theta}")
+                little_log = f"Error y: {delta} | error theta {error_x} | steering_raw {error_theta} | steering_conv: {delta_PWM}, tag_area: {area}"
-                self.log.warning(f"TAG AREA: {area}")
+                self.log.info(little_log)
+                self.steering_log_file.writelines([little_log])
+
                 if area < self.max_approach_area_thresh:  # If we are far enough from the cargo, move towards it
                     speed_pwm = remap(area, 40000, 1000, 1500, 2000)
                     self.forklift.set_speed_steering(speed_pwm, delta_PWM)
@@ -244,7 +191,7 @@ class VisionCargoApproacher:
                         time.sleep(0.2)
                         return True
                         delta = self.steering_law.calculate(error_x, error_theta)  # getting the steering angle from the control law
-                        delta_PWM = remap(delta, 0.7, -0.7, 2000, 1000)
+                        delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100)
                         self.forklift.set_speed_steering(1000, delta_PWM)
                         time.sleep(3)
                         self.forklift.set_speed_steering(1500, 1500)
@@ -255,6 +202,8 @@ class VisionCargoApproacher:
                     self.log.warning("DID NOT DETECT CODE!")
                     self.forklift.set_speed(1500)
 
+            self.steering_log_file.close()
+
     # def stop(self):
     #     self.is_running = False
     #     self.forklift.set_disarm()
@@ -281,5 +230,61 @@ class VisionCargoApproacher:
 # else:
 #     logging.critical(f"Platform {platform.release()} not tested!")
 
+class Tester:
+    def __init__(self) -> None:
+        self.log = logging.getLogger("TESTER")
+        self.log.setLevel("DEBUG")
+        self.camera = VisionSystem()
+        self.steering_law = CargoApproachingSystem()
+        self.no_detects = 0
+        self.max_approach_area_thresh = 40000
+        self.centre_distance_thresh = (-0.04, 0.08)
+        self.centre_angle_thresh = (-0.2, 0.2)
+
+    def run_pc(self):
+        while True:
+            pallet_pose = self.camera.get_pallet_pose_pc()
+            if pallet_pose:
+                position, rotation, area = pallet_pose
+                error_y = position[0]
+                error_theta = rotation[1]
+                # print(error_y, error_theta)
+                delta = self.steering_law.calculate(error_y, error_theta)
+                # delta_PWM = remap(delta, -0.8, 0.8, 1000, 2000)
+                delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100)
+                print(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}")
+            cv2.waitKey(1)
+
+    def run_jetson(self):
+        self.log.info("Starting vision system")
+        self.is_running = True
+        while self.is_running:
+            time.sleep(0.05)
+            pallet_pose = self.camera.get_pallet_pose()  # getting the position, rotation and size of the tracker
+            if pallet_pose:  # If we found a tracker
+                self.no_detects = 0
+                position, rotation, area = pallet_pose
+                error_y = position[0]  # centre offset distance
+                error_theta = rotation[1]  # centre offset angle
+                # print(error_y, error_theta)
+                delta = self.steering_law.calculate(error_y, error_theta)  # getting the steering angle from the control law
+                delta_PWM = remap(delta, 0.7, -0.7, 1900, 1100)  # Converting steering law output into PWM values
+                self.log.info(f"PWM: {delta_PWM}, ey: {error_y}, et: {error_theta}")
+                if area < self.max_approach_area_thresh:  # If we are far enough from the cargo, move towards it
+                    self.log.info(f"Setting speed: 1950, steering: {delta_PWM}")
+                else:
+                    self.log.info(f"We are close enough to cargo!, {area}")
+                    is_aligned = self.camera.check_cargo_alignment(self.centre_distance_thresh, self.centre_angle_thresh, (error_y, error_theta))
+                    if is_aligned:
+                        self.log.info(f'Cargo aligned: {error_y} in {self.centre_distance_thresh} and {error_theta} in {self.centre_angle_thresh}')
+                    else:
+                        self.log.info(f'Cargo NOT aligned: {error_y} not in {self.centre_distance_thresh} or {error_theta} inot n {self.centre_angle_thresh}')
+
+            else:
+                self.no_detects +=1
+                if self.no_detects > 3:  # if we do not detect anything 3 times in a row, stop the vehicle
+                    self.log.info("No cargo detected, setting speed to 1500")
+
+
 if __name__ == "__main__":
     print("ERROR: This script is no longer used in standalone mode")