ros中增加电流、电压检测功能

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

@@ -20,25 +20,26 @@
     http://www.gnu.org/licenses/gpl.html
 """
 
-import thread
+
 from math import pi as PI
 import sys, traceback
 from serial.serialutil import SerialException
 from serial import Serial
+import thread
 import rospy
 import time
 import os
 
 
 class Arduino:
-    ''' Configuration Arduino Mega2560 Board Parameters
+    ''' Configuration Arduino DUE Board Parameters
     '''
     N_ANALOG_PORTS  = 10
     N_DIGITAL_PORTS = 54
 
-    def __init__(self, port="/dev/ttyACM0", baudrate=57600, timeout=0.5):
+    def __init__(self, port="/dev/ttyAMA0", baudrate=57600, timeout=0.5):
         self.PID_RATE = 40 # Do not change this!  It is a fixed property of the Arduino PID controller.
-        self.PID_INTERVAL = 1000 / 30
+        self.PID_INTERVAL = 25
 
         self.port     = port
         self.baudrate = baudrate
@@ -321,6 +322,12 @@ class Arduino:
     def beep_ring(self, value):
         return self.execute_ack('p %d' %value)
 
+    def detect_voltage(self, value):
+        return self.execute_ack('g %d' %value)        
+    
+    def detect_current(self, value):
+        return self.execute_ack('f %d' %value)
+
     def light_show(self, value):
         return self.execute_ack('l %d' %value)
 

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

@@ -173,6 +173,9 @@ class BaseController:
 
         time_now = rospy.Time.now()
         if time_now > self.t_next:
+            vol = self.arduino.detect_voltage()
+            current = self.arduino.detect_current()
+            rospy.logwarn("Voltage: %f, Current: %f", vol, current)
             # Read the encoders
             try:
                 aWheel_enc, bWheel_enc, cWheel_enc = self.arduino.get_encoder_counts()