speed to 25, new poi test, image longer for kalman

catkin_ws/src/asclinic_pkg/src/FSM.py

@@ -29,7 +29,7 @@ class FSM:
         #self.phi = 0
         self.phi = math.pi/2##########################
         #self.poi = [[1,-1], [4, -2], [6,-3], [7,1], [6,3], [3,3], [-1,1], [-1, -2]]
-        self.poi = [[-1,1],[1,1],[1, -1]] ############
+        self.poi = [[-1,1],[2,-1],[-1, -1]] ############
         self.poi_index = 0
         self.outer_control = rospy.Publisher("/outer_control_signal", pathpoint, queue_size=1, latch=False)
         self.camera_control = rospy.Publisher("/camera_control_signal", UInt32, queue_size=1, latch=False)

catkin_ws/src/asclinic_pkg/src/fcontroller1.cpp

@@ -58,7 +58,7 @@ float w_now = 0;
 float v_now = 0;
 int state_value = 2; // 0 is forward, 1 is rotate , 2 is pending
 
-float initial_pwm = 26.3;
+float initial_pwm = 26;
 //initial 26.5
 void straightline(const asclinic_pkg::vw& vw_msg) {
 

catkin_ws/src/asclinic_pkg/src/image.py

@@ -63,7 +63,7 @@ def answer_callback(msg, markers, publisher):
                 ry = fiducial_marker.rvec[1]
                 rz = fiducial_marker.rvec[2]
                 now_d = math.sqrt(tx**2 + tz**2)
-                if now_d > 2.5:
+                if now_d > 4:
                     continue
                 valid_number+=1
                 # Compute rotation matrix