更新ros_arduino_python代码

src/ros_arduino_bridge/ros_arduino_msgs/CMakeLists.txt

@@ -16,7 +16,7 @@ add_service_files(FILES
                   AnalogWrite.srv
                   AnalogSensorWrite.srv
                   AnalogFloatSensorWrite.srv
-                  AnalogPinMode.srv           
+                  AnalogPinMode.srv
                   AnalogRead.srv
                   AnalogSensorRead.srv
                   AnalogFloatSensorRead.srv
@@ -30,8 +30,6 @@ add_service_files(FILES
                   Enable.srv
                   Relax.srv
                   AnalogSensorRead.srv
-                  ServoAttach.srv
-                  ServoDetach.srv
                   ServoRead.srv
                   ServoWrite.srv
                   SetSpeed.srv
@@ -42,7 +40,7 @@ add_service_files(FILES
                   DynamicPID.srv
                  )
 
-generate_messages(   
+generate_messages(
 	DEPENDENCIES  
 	std_msgs  
 )  

src/ros_arduino_bridge/ros_arduino_msgs/srv/ServoAttach.srv

@@ -1,3 +0,0 @@
-uint8 id
----
-

src/ros_arduino_bridge/ros_arduino_msgs/srv/ServoDetach.srv

@@ -1,3 +0,0 @@
-uint8 id
----
-

src/ros_arduino_bridge/ros_arduino_python/cfg/my_arduino_params.yaml

@@ -24,7 +24,7 @@ rate: 25
 
 cmd_topic: cmd_vel
 
-base_controller_rate: 10.0
+base_controller_rate: 15.0
 base_controller_timeout: 0.7
 
 # For a robot that uses base_footprint, change base_frame to base_footprint
@@ -36,7 +36,7 @@ wheel_diameter: 0.067
 wheel_track: 0.21     #L value
 encoder_resolution: 11 #12V DC motors
 gear_reduction: 30     #empty payload rpm 330 r/m,if rpm=530,gear_reduction=19
-linear_scale_correction: 1.028
+linear_scale_correction: 1.00
 angular_scale_correction: 1.00
 
 # === PID parameters
@@ -44,15 +44,15 @@ debugPID: False
 
 accel_limit: 0.05
 
-AWheel_Kp: 22
-AWheel_Kd: 30
+AWheel_Kp: 18
+AWheel_Kd: 18
 AWheel_Ki: 0
-AWheel_Ko: 50
+AWheel_Ko: 10
 
-BWheel_Kp: 22
-BWheel_Kd: 30
+BWheel_Kp: 18
+BWheel_Kd: 18
 BWheel_Ki: 0
-BWheel_Ko: 50
+BWheel_Ko: 10
 
 # Sensor definitions (case sensitive!):
 sensors: {

src/ros_arduino_bridge/ros_arduino_python/package.xml

@@ -8,9 +8,9 @@
   <maintainer email="patrick@pirobot.org">Patrick Goebel</maintainer>
   <license>BSD</license>
   <url>http://ros.org/wiki/ros_arduino_python</url>
-  
+
   <buildtool_depend>catkin</buildtool_depend>
-  
+
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>diagnostic_updater</build_depend>
   <build_depend>diagnostic_aggregator</build_depend>
@@ -28,5 +28,4 @@
   <run_depend>ros_arduino_msgs</run_depend>
   <run_depend>python-serial</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
-
 </package>

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_driver.py

@@ -23,8 +23,8 @@ class Arduino:
     N_DIGITAL_PORTS = 54
 
     def __init__(self, is_use_serial,serial_port="/dev/ttyACM0",baudrate=57600,i2c_smbus_num=1,i2c_slave_addr=8,timeout=0.5):
-        self.PID_RATE = 40   #Don't change this ! It is a fixed property of the Arduino PID controller.
-        self.PID_INTERVAL = 25
+        self.PID_RATE = 50   #Don't change this ! It is a fixed property of the Arduino PID controller.
+        self.PID_INTERVAL = 20
 
         self.is_use_serial = is_use_serial #与下位机arduino的通信方式是否使用串口还是IIC
         self.serial_port = serial_port
@@ -42,12 +42,6 @@ class Arduino:
         # Keep things thread safe
         self.mutex = thread.allocate_lock()
 
-        # An array to cache analog sensor readings
-        self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
-
-        # An array to cache digital sensor readings
-        self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
-
     #与下位机arduino建立连接
     def connect(self):
         if self.is_use_serial:
@@ -329,7 +323,6 @@ class Arduino:
 
     def i2c_get_encoder_counts(self):
         #print "IIC Get Encoder count !"
-
         try:
             self.i2c_bus.write_byte(self.i2c_slave_addr, int(ord('e')))
             self.i2c_bus.write_byte(self.i2c_slave_addr, ord('\r'))
@@ -353,8 +346,6 @@ class Arduino:
             traceback.print_exc(file=sys.stdout)
             return None
 
-
-
     def reset_encoders(self):
         ''' Reset the encoder counts to 0 by serial port
         '''

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/arduino_sensors.py

@@ -246,28 +246,6 @@ class IR2Y0A02(IRSensor):
 
         return dist
 
-class US025(MySonarSensor):
-    def __init__(self, *args, **kwargs):
-        super(US025, self).__init__(*args, **kwargs)
-
-        self.msg.field_of_view = 0.01
-        self.msg.min_range = 0.03
-        self.msg.max_range = 6
-
-    def read_value(self):
-        values = self.controller.detect_ultrasonic_distance()
-        distances = np.array([values[0], values[1], values[2]])
-        return distances/100.0
-
-class MotorTotalCurrent(AnalogFloatSensor):
-    def __init__(self, *args, **kwargs):
-        super(MotorTotalCurrent, self).__init__(*args, **kwargs)
-
-    def read_value(self):
-        midVal = mylist[15]
-        result = (midVal/1024.0*4523.00 - 4523.00/2)/100
-        return Decimal(result).quantize(Decimal('0.00'))
-
 class BatPercent(DigitalSensor):
     def __init__(self, *args, **kwargs):
         super(BatPercent, self).__init__(*args, **kwargs)
@@ -276,15 +254,7 @@ class BatPercent(DigitalSensor):
         percent = self.controller.getBatPercent()
         return int(percent)
 
-class CurrentValue(DigitalSensor):
-    def __init__(self, *args, **kwargs):
-        super(CurrentValue, self).__init__(*args, **kwargs)
-
-    def read_value(self):
-        currentvalue = self.controller.getCurrentValue()
-        #rospy.loginfo("read currentvalue: "+currentvalue)
-        return float(currentvalue)
-
 
 if __name__ == '__main__':
     rospy.loginf("arduino sensor main function ...")
+

src/ros_arduino_bridge/ros_arduino_python/src/ros_arduino_python/base_controller.py

@@ -37,19 +37,19 @@ class BaseController:
         self.debugPID   = rospy.get_param('/dynamic_tutorials/debugPID', False)
 
         pid_params = dict()
-        pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.058)
+        pid_params['wheel_diameter']     = rospy.get_param("~wheel_diameter", 0.067)
         pid_params['wheel_track']        = rospy.get_param("~wheel_track", 0.126)
         pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", 11)
-        pid_params['gear_reduction']     = rospy.get_param("~gear_reduction", 46)
-        pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 15)
-        pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 15)
+        pid_params['gear_reduction']     = rospy.get_param("~gear_reduction", 30)
+        pid_params['AWheel_Kp'] = rospy.get_param("~AWheel_Kp", 18)
+        pid_params['AWheel_Kd'] = rospy.get_param("~AWheel_Kd", 18)
         pid_params['AWheel_Ki'] = rospy.get_param("~AWheel_Ki", 0)
-        pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 50)
+        pid_params['AWheel_Ko'] = rospy.get_param("~AWheel_Ko", 10)
 
-        pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 15)
-        pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 15)
+        pid_params['BWheel_Kp'] = rospy.get_param("~BWheel_Kp", 18)
+        pid_params['BWheel_Kd'] = rospy.get_param("~BWheel_Kd", 18)
         pid_params['BWheel_Ki'] = rospy.get_param("~BWheel_Ki", 0)
-        pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 50)
+        pid_params['BWheel_Ko'] = rospy.get_param("~BWheel_Ko", 10)
 
         self.accel_limit  = rospy.get_param('~accel_limit', 0.05)
         self.linear_scale_correction  = rospy.get_param("~linear_scale_correction",  1.0)
@@ -303,7 +303,6 @@ class BaseController:
             self.AVelPub.publish(self.v_A)
             self.BVelPub.publish(self.v_B)
 
-
     # stop move mobile base
     def stop(self):
         self.v_A = 0
@@ -326,7 +325,7 @@ class BaseController:
 
         #Inverse Kinematic
         vA = -(x + 0.5*self.wheel_track*th)
-        vB = (x - 0.5*self.wheel_track*th)
+        vB =  (x - 0.5*self.wheel_track*th)
 
         self.v_des_AWheel = int(vA * self.ticks_per_meter / self.arduino.PID_RATE)
         self.v_des_BWheel = int(vB * self.ticks_per_meter / self.arduino.PID_RATE)

src/teleop_twist_keyboard/teleop_twist_keyboard.py

@@ -98,8 +98,8 @@ if __name__=="__main__":
     pub = rospy.Publisher('cmd_vel', Twist, queue_size = 2)
     rospy.init_node('teleop_twist_keyboard')
 
-    speed = rospy.get_param("~speed", 0.3)
-    turn = rospy.get_param("~turn", 2.5)
+    speed = rospy.get_param("~speed", 0.12)
+    turn = rospy.get_param("~turn", 1.0)
 
     x = 0
     y = 0