├── ros_arduino_python ├── src │ └── ros_arduino_python │ │ ├── __init__.py │ │ ├── arduino_sensors.py │ │ ├── base_controller.py │ │ └── arduino_driver.py ├── launch │ └── arduino.launch ├── setup.py ├── CMakeLists.txt ├── package.xml ├── config │ ├── my_arduino_params.yaml │ └── arduino_params.yaml └── nodes │ └── arduino_node.py ├── ros_arduino_msgs ├── srv │ ├── DigitalRead.srv │ ├── ServoRead.srv │ ├── AnalogRead.srv │ ├── AnalogWrite.srv │ ├── DigitalWrite.srv │ ├── ServoWrite.srv │ └── DigitalSetDirection.srv ├── msg │ ├── SensorState.msg │ ├── Digital.msg │ ├── Analog.msg │ ├── AnalogFloat.msg │ └── ArduinoConstants.msg ├── package.xml └── CMakeLists.txt ├── ros_arduino_bridge ├── CMakeLists.txt └── package.xml ├── ros_arduino_firmware ├── CMakeLists.txt ├── src │ └── libraries │ │ ├── MegaRobogaiaPololu │ │ ├── servos.h │ │ ├── commands.h │ │ ├── sensors.h │ │ ├── diff_controller.h │ │ └── MegaRobogaiaPololu.ino │ │ └── ROSArduinoBridge │ │ ├── motor_driver.h │ │ ├── commands.h │ │ ├── encoder_driver.h │ │ ├── sensors.h │ │ ├── servos.h │ │ ├── servos.ino │ │ ├── motor_driver.ino │ │ ├── diff_controller.h │ │ ├── encoder_driver.ino │ │ └── ROSArduinoBridge.ino └── package.xml ├── .gitignore └── README.md /ros_arduino_python/src/ros_arduino_python/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/DigitalRead.srv: -------------------------------------------------------------------------------- 1 | uint8 pin 2 | --- 3 | bool value 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/ServoRead.srv: -------------------------------------------------------------------------------- 1 | uint8 id 2 | --- 3 | float32 value 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/AnalogRead.srv: -------------------------------------------------------------------------------- 1 | uint8 pin 2 | --- 3 | uint16 value 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/AnalogWrite.srv: -------------------------------------------------------------------------------- 1 | uint8 pin 2 | uint16 value 3 | --- 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/DigitalWrite.srv: -------------------------------------------------------------------------------- 1 | uint8 pin 2 | bool value 3 | --- 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/ServoWrite.srv: -------------------------------------------------------------------------------- 1 | uint8 id 2 | float32 value 3 | --- 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/srv/DigitalSetDirection.srv: -------------------------------------------------------------------------------- 1 | uint8 pin 2 | bool direction 3 | --- 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/msg/SensorState.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | string[] name 4 | float32[] value 5 | -------------------------------------------------------------------------------- /ros_arduino_msgs/msg/Digital.msg: -------------------------------------------------------------------------------- 1 | # Reading on a digital pin 2 | Header header 3 | uint8 value 4 | 5 | -------------------------------------------------------------------------------- /ros_arduino_msgs/msg/Analog.msg: -------------------------------------------------------------------------------- 1 | # Reading from a single analog IO pin. 2 | Header header 3 | uint16 value 4 | -------------------------------------------------------------------------------- /ros_arduino_msgs/msg/AnalogFloat.msg: -------------------------------------------------------------------------------- 1 | # Float message from a single analog IO pin. 2 | Header header 3 | float64 value -------------------------------------------------------------------------------- /ros_arduino_msgs/msg/ArduinoConstants.msg: -------------------------------------------------------------------------------- 1 | # Arduino-style constants 2 | uint8 LOW=0 3 | uint8 HIGH=1 4 | uint8 INPUT=0 5 | uint8 OUTPUT=1 6 | -------------------------------------------------------------------------------- /ros_arduino_bridge/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_arduino_bridge) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ros_arduino_python/launch/arduino.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /ros_arduino_firmware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_arduino_firmware) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package(DEPENDS) 6 | 7 | install(DIRECTORY src 8 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | ) 10 | -------------------------------------------------------------------------------- /ros_arduino_python/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | packages=['ros_arduino_python'], 8 | package_dir={'': 'src'}, 9 | ) 10 | 11 | setup(**d) 12 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/servos.h: -------------------------------------------------------------------------------- 1 | /* Define the attachment of any servos here. 2 | The example shows two servos attached on pins 3 and 5. 3 | */ 4 | 5 | #define N_SERVOS 2 6 | 7 | Servo servos [N_SERVOS]; 8 | byte servoPins [N_SERVOS] = {3, 5}; 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /ros_arduino_firmware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_arduino_firmware 3 | 0.2.0 4 | 5 | ROS Arduino Firmware. 6 | 7 | Patrick Goebel 8 | Patrick Goebel 9 | BSD 10 | http://ros.org/wiki/ros_arduino_firmware 11 | 12 | catkin 13 | 14 | -------------------------------------------------------------------------------- /ros_arduino_python/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_arduino_python) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package(DEPENDS) 6 | catkin_python_setup() 7 | 8 | install(DIRECTORY config 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | 12 | install(DIRECTORY launch 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 14 | ) 15 | 16 | install(DIRECTORY nodes 17 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 18 | ) 19 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/commands.h: -------------------------------------------------------------------------------- 1 | /* Define single-letter commands that will be sent by the PC over the 2 | serial link. 3 | */ 4 | 5 | #define ANALOG_READ 'a' 6 | #define GET_BAUDRATE 'b' 7 | #define PIN_MODE 'c' 8 | #define DIGITAL_READ 'd' 9 | #define READ_ENCODERS 'e' 10 | #define MOTOR_SPEEDS 'm' 11 | #define PING 'p' 12 | #define RESET_ENCODERS 'r' 13 | #define SERVO_WRITE 's' 14 | #define SERVO_READ 't' 15 | #define UPDATE_PID 'u' 16 | #define DIGITAL_WRITE 'w' 17 | #define ANALOG_WRITE 'x' 18 | #define LEFT 0 19 | #define RIGHT 1 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /ros_arduino_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_arduino_msgs 3 | 0.2.0 4 | 5 | ROS Arduino Messages. 6 | 7 | Patrick Goebel 8 | Patrick Goebel 9 | BSD 10 | http://ros.org/wiki/ros_arduino_msgs 11 | 12 | catkin 13 | 14 | message_generation 15 | std_msgs 16 | 17 | message_runtime 18 | std_msgs 19 | 20 | 21 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h: -------------------------------------------------------------------------------- 1 | /*********************************************************************** 2 | Motor driver function definitions - by James Nugen and Chaoyang Liu 3 | ***********************************************************************/ 4 | 5 | void initMotorController(); 6 | void setMotorSpeed(int i, int spd); 7 | void setMotorSpeeds(int leftSpeed, int rightSpeed); 8 | #ifdef L298N_DUAL_HBRIDGE 9 | // motor one 10 | #define ENA 5 11 | #define IN1 7 12 | #define IN2 8 13 | // motor two 14 | #define ENB 6 15 | #define IN3 9 16 | #define IN4 10 17 | #endif 18 | -------------------------------------------------------------------------------- /ros_arduino_bridge/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_arduino_bridge 3 | 0.2.0 4 | 5 | Metapackage for ros_arduino_bridge. 6 | 7 | Patrick Goebel 8 | Patrick Goebel 9 | BSD 10 | http://ros.org/wiki/ros_arduino_bridge 11 | 12 | catkin 13 | 14 | ros_arduino_firmware 15 | ros_arduino_msgs 16 | ros_arduino_python 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /ros_arduino_python/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ros_arduino_python 3 | 0.2.0 4 | 5 | ROS Arduino Python. 6 | 7 | Patrick Goebel 8 | Patrick Goebel 9 | BSD 10 | http://ros.org/wiki/ros_arduino_python 11 | 12 | catkin 13 | 14 | rospy 15 | std_msgs 16 | sensor_msgs 17 | geometry_msgs 18 | nav_msgs 19 | tf 20 | ros_arduino_msgs 21 | python-serial 22 | 23 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/commands.h: -------------------------------------------------------------------------------- 1 | /* Define single-letter commands that will be sent by the PC over the 2 | serial link. 3 | */ 4 | 5 | #ifndef COMMANDS_H 6 | #define COMMANDS_H 7 | 8 | #define ANALOG_READ 'a' 9 | #define GET_BAUDRATE 'b' 10 | #define PIN_MODE 'c' 11 | #define DIGITAL_READ 'd' 12 | #define READ_ENCODERS 'e' 13 | #define MOTOR_SPEEDS 'm' 14 | #define PING 'p' 15 | #define RESET_ENCODERS 'r' 16 | #define SERVO_WRITE 's' 17 | #define SERVO_READ 't' 18 | #define UPDATE_PID 'u' 19 | #define DIGITAL_WRITE 'w' 20 | #define ANALOG_WRITE 'x' 21 | #define LEFT 0 22 | #define RIGHT 1 23 | #define FORWARDS true 24 | #define BACKWARDS false 25 | 26 | #endif 27 | -------------------------------------------------------------------------------- /ros_arduino_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_arduino_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS std_msgs message_generation) 5 | 6 | add_message_files(FILES 7 | AnalogFloat.msg 8 | Analog.msg 9 | ArduinoConstants.msg 10 | Digital.msg 11 | SensorState.msg 12 | ) 13 | 14 | add_service_files(FILES 15 | DigitalSetDirection.srv 16 | DigitalWrite.srv 17 | DigitalRead.srv 18 | ServoRead.srv 19 | ServoWrite.srv 20 | AnalogWrite.srv 21 | AnalogRead.srv 22 | ) 23 | 24 | generate_messages( 25 | DEPENDENCIES 26 | std_msgs 27 | ) 28 | 29 | catkin_package(CATKIN_DEPENDS message_runtime std_msgs) 30 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.h: -------------------------------------------------------------------------------- 1 | /* ************************************************************* 2 | Encoder driver function definitions - by James Nugen 3 | ************************************************************ */ 4 | 5 | 6 | #ifdef ARDUINO_ENC_COUNTER 7 | //below can be changed, but should be PORTD pins; 8 | //otherwise additional changes in the code are required 9 | #define LEFT_ENC_PIN_A PD2 //pin 2 10 | #define LEFT_ENC_PIN_B PD3 //pin 3 11 | 12 | //below can be changed, but should be PORTC pins 13 | #define RIGHT_ENC_PIN_A PC4 //pin A4 14 | #define RIGHT_ENC_PIN_B PC5 //pin A5 15 | #endif 16 | 17 | #ifdef ARDUINO_MY_COUNTER 18 | #define LEFT_ENC_A 2 19 | #define LEFT_ENC_B 22 20 | #define RIGHT_ENC_A 21 21 | #define RIGHT_ENC_B 24 22 | void initEncoders(); 23 | void leftEncoderEvent(); 24 | void rightEncoderEvent(); 25 | #endif 26 | 27 | long readEncoder(int i); 28 | void resetEncoder(int i); 29 | void resetEncoders(); 30 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/sensors.h: -------------------------------------------------------------------------------- 1 | /* Functions for various sensor types */ 2 | 3 | float microsecondsToCm(long microseconds) 4 | { 5 | // The speed of sound is 340 m/s or 29 microseconds per cm. 6 | // The ping travels out and back, so to find the distance of the 7 | // object we take half of the distance travelled. 8 | return microseconds / 29 / 2; 9 | } 10 | 11 | long Ping(int pin) { 12 | long duration, range; 13 | 14 | // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. 15 | // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: 16 | pinMode(pin, OUTPUT); 17 | digitalWrite(pin, LOW); 18 | delayMicroseconds(2); 19 | digitalWrite(pin, HIGH); 20 | delayMicroseconds(5); 21 | digitalWrite(pin, LOW); 22 | 23 | // The same pin is used to read the signal from the PING))): a HIGH 24 | // pulse whose duration is the time (in microseconds) from the sending 25 | // of the ping to the reception of its echo off of an object. 26 | pinMode(pin, INPUT); 27 | duration = pulseIn(pin, HIGH); 28 | 29 | // convert the time into meters 30 | range = microsecondsToCm(duration); 31 | 32 | return(range); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/sensors.h: -------------------------------------------------------------------------------- 1 | /* Functions for various sensor types */ 2 | 3 | float microsecondsToCm(long microseconds) 4 | { 5 | // The speed of sound is 340 m/s or 29 microseconds per cm. 6 | // The ping travels out and back, so to find the distance of the 7 | // object we take half of the distance travelled. 8 | return microseconds / 29 / 2; 9 | } 10 | 11 | long Ping(int pin) { 12 | long duration, range; 13 | 14 | // The PING))) is triggered by a HIGH pulse of 2 or more microseconds. 15 | // Give a short LOW pulse beforehand to ensure a clean HIGH pulse: 16 | pinMode(pin, OUTPUT); 17 | digitalWrite(pin, LOW); 18 | delayMicroseconds(2); 19 | digitalWrite(pin, HIGH); 20 | delayMicroseconds(5); 21 | digitalWrite(pin, LOW); 22 | 23 | // The same pin is used to read the signal from the PING))): a HIGH 24 | // pulse whose duration is the time (in microseconds) from the sending 25 | // of the ping to the reception of its echo off of an object. 26 | pinMode(pin, INPUT); 27 | duration = pulseIn(pin, HIGH); 28 | 29 | // convert the time into meters 30 | range = microsecondsToCm(duration); 31 | 32 | return(range); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.h: -------------------------------------------------------------------------------- 1 | #ifndef SERVOS_H 2 | #define SERVOS_H 3 | 4 | 5 | #define N_SERVOS 2 6 | 7 | // This delay in milliseconds determines the pause 8 | // between each one degree step the servo travels. Increasing 9 | // this number will make the servo sweep more slowly. 10 | // Decreasing this number will make the servo sweep more quickly. 11 | // Zero is the default number and will make the servos spin at 12 | // full speed. 150 ms makes them spin very slowly. 13 | int stepDelay [N_SERVOS] = { 0, 0 }; // ms 14 | 15 | // Pins 16 | byte servoPins [N_SERVOS] = { 3, 4 }; 17 | 18 | // Initial Position 19 | byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees 20 | 21 | 22 | class SweepServo 23 | { 24 | public: 25 | SweepServo(); 26 | void initServo( 27 | int servoPin, 28 | int stepDelayMs, 29 | int initPosition); 30 | void doSweep(); 31 | void setTargetPosition(int position); 32 | Servo getServo(); 33 | 34 | private: 35 | Servo servo; 36 | int stepDelayMs; 37 | int currentPositionDegrees; 38 | int targetPositionDegrees; 39 | long lastSweepCommand; 40 | }; 41 | 42 | SweepServo servos [N_SERVOS]; 43 | 44 | #endif 45 | -------------------------------------------------------------------------------- /ros_arduino_python/config/my_arduino_params.yaml: -------------------------------------------------------------------------------- 1 | # For a direct USB cable connection, the port name is typically 2 | # /dev/ttyACM# where is # is a number such as 0, 1, 2, etc 3 | # For a wireless connection like XBee, the port is typically 4 | # /dev/ttyUSB# where # is a number such as 0, 1, 2, etc. 5 | 6 | port: /dev/ttyACM0 7 | baud: 57600 8 | timeout: 0.1 9 | 10 | rate: 50 11 | sensorstate_rate: 10 12 | 13 | use_base_controller: True 14 | base_controller_rate: 10 15 | 16 | # For a robot that uses base_footprint, change base_frame to base_footprint 17 | base_frame: base_footprint 18 | 19 | # === Robot drivetrain parameters 20 | wheel_diameter: 0.065 21 | wheel_track: 0.145 22 | encoder_resolution: 11700 23 | gear_reduction: 0.033 24 | motors_reversed: true 25 | 26 | # === PID parameters 27 | Kp: 10 28 | Kd: 12 29 | Ki: 0 30 | Ko: 50 31 | accel_limit: 1.0 32 | 33 | # === Sensor definitions. Examples only - edit for your robot. 34 | # Sensor type can be one of the follow (case sensitive!): 35 | # * Ping 36 | # * GP2D12 37 | # * Analog 38 | # * Digital 39 | # * PololuMotorCurrent 40 | # * PhidgetsVoltage 41 | # * PhidgetsCurrent (20 Amp, DC) 42 | 43 | 44 | 45 | sensors: { 46 | motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5}, 47 | motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5}, 48 | #ir_front_center: {pin: 2, type: GP2D12, rate: 10}, 49 | #sonar_front_center: {pin: 5, type: Ping, rate: 10}, 50 | arduino_led: {pin: 13, type: Digital, rate: 5, direction: output} 51 | } 52 | -------------------------------------------------------------------------------- /ros_arduino_python/config/arduino_params.yaml: -------------------------------------------------------------------------------- 1 | # For a direct USB cable connection, the port name is typically 2 | # /dev/ttyACM# where is # is a number such as 0, 1, 2, etc 3 | # For a wireless connection like XBee, the port is typically 4 | # /dev/ttyUSB# where # is a number such as 0, 1, 2, etc. 5 | 6 | port: /dev/ttyACM0 7 | baud: 57600 8 | timeout: 0.1 9 | 10 | rate: 50 11 | sensorstate_rate: 10 12 | 13 | use_base_controller: False 14 | base_controller_rate: 10 15 | 16 | # For a robot that uses base_footprint, change base_frame to base_footprint 17 | base_frame: base_link 18 | 19 | # === Robot drivetrain parameters 20 | wheel_diameter: 0.146 21 | wheel_track: 0.2969 22 | encoder_resolution: 8384 # from Pololu for 131:1 motors 23 | gear_reduction: 1.0 24 | motors_reversed: True 25 | 26 | # === PID parameters 27 | Kp: 10 28 | Kd: 12 29 | Ki: 0 30 | Ko: 50 31 | accel_limit: 1.0 32 | 33 | # === Sensor definitions. Examples only - edit for your robot. 34 | # Sensor type can be one of the follow (case sensitive!): 35 | # * Ping 36 | # * GP2D12 37 | # * Analog 38 | # * Digital 39 | # * PololuMotorCurrent 40 | # * PhidgetsVoltage 41 | # * PhidgetsCurrent (20 Amp, DC) 42 | 43 | 44 | 45 | sensors: { 46 | motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5}, 47 | motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5}, 48 | #ir_front_center: {pin: 2, type: GP2D12, rate: 10}, 49 | #sonar_front_center: {pin: 5, type: Ping, rate: 10}, 50 | arduino_led: {pin: 13, type: Digital, rate: 5, direction: output} 51 | } 52 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Created by https://www.gitignore.io/api/python 3 | 4 | ### Python ### 5 | # temp file 6 | *~ 7 | 8 | # Byte-compiled / optimized / DLL files 9 | __pycache__/ 10 | *.py[cod] 11 | *$py.class 12 | 13 | # C extensions 14 | *.so 15 | 16 | # Distribution / packaging 17 | .Python 18 | env/ 19 | build/ 20 | develop-eggs/ 21 | dist/ 22 | downloads/ 23 | eggs/ 24 | .eggs/ 25 | lib/ 26 | lib64/ 27 | parts/ 28 | sdist/ 29 | var/ 30 | *.egg-info/ 31 | .installed.cfg 32 | *.egg 33 | 34 | # PyInstaller 35 | # Usually these files are written by a python script from a template 36 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 37 | *.manifest 38 | *.spec 39 | 40 | # Installer logs 41 | pip-log.txt 42 | pip-delete-this-directory.txt 43 | 44 | # Unit test / coverage reports 45 | htmlcov/ 46 | .tox/ 47 | .coverage 48 | .coverage.* 49 | .cache 50 | nosetests.xml 51 | coverage.xml 52 | *,cover 53 | .hypothesis/ 54 | 55 | # Translations 56 | *.mo 57 | *.pot 58 | 59 | # Django stuff: 60 | *.log 61 | local_settings.py 62 | 63 | # Flask stuff: 64 | instance/ 65 | .webassets-cache 66 | 67 | # Scrapy stuff: 68 | .scrapy 69 | 70 | # Sphinx documentation 71 | docs/_build/ 72 | 73 | # PyBuilder 74 | target/ 75 | 76 | # IPython Notebook 77 | .ipynb_checkpoints 78 | 79 | # pyenv 80 | .python-version 81 | 82 | # celery beat schedule file 83 | celerybeat-schedule 84 | 85 | # dotenv 86 | .env 87 | 88 | # virtualenv 89 | .venv/ 90 | venv/ 91 | ENV/ 92 | 93 | # Spyder project settings 94 | .spyderproject 95 | 96 | # Rope project settings 97 | .ropeproject 98 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/diff_controller.h: -------------------------------------------------------------------------------- 1 | /* Functions and type-defs for PID control. 2 | 3 | Taken mostly from Mike Ferguson's ArbotiX code which lives at: 4 | 5 | http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/ 6 | */ 7 | 8 | /* PID setpoint info For a Motor */ 9 | typedef struct { 10 | double TargetTicksPerFrame; // target speed in ticks per frame 11 | long Encoder; // encoder count 12 | long PrevEnc; // last encoder count 13 | int PrevErr; // last error 14 | int Ierror; // integrated error 15 | int output; // last motor setting 16 | } 17 | SetPointInfo; 18 | 19 | SetPointInfo leftPID, rightPID; 20 | 21 | /* PID Parameters */ 22 | int Kp = 20; 23 | int Kd = 12; 24 | int Ki = 0; 25 | int Ko = 50; 26 | 27 | unsigned char moving = 0; // is the base in motion? 28 | 29 | /* PID routine to compute the next motor commands */ 30 | void doPID(SetPointInfo * p) { 31 | long Perror; 32 | long output; 33 | 34 | Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); 35 | 36 | // Derivative error is the delta Perror 37 | output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; 38 | p->PrevErr = Perror; 39 | p->PrevEnc = p->Encoder; 40 | 41 | output += p->output; 42 | // Accumulate Integral error *or* Limit output. 43 | // Stop accumulating when output saturates 44 | if (output >= MAX_PWM) 45 | output = MAX_PWM; 46 | else if (output <= -MAX_PWM) 47 | output = -MAX_PWM; 48 | else 49 | p->Ierror += Perror; 50 | 51 | p->output = output; 52 | } 53 | 54 | /* Read the encoder values and call the PID routine */ 55 | void updatePID() { 56 | /* Read the encoders */ 57 | leftPID.Encoder = readEncoder(0); 58 | rightPID.Encoder = readEncoder(1); 59 | 60 | /* If we're not moving there is nothing more to do */ 61 | if (!moving) 62 | return; 63 | 64 | /* Compute PID update for each motor */ 65 | doPID(&rightPID); 66 | doPID(&leftPID); 67 | 68 | /* Set the motor speeds accordingly */ 69 | setMotorSpeeds(leftPID.output, rightPID.output); 70 | } 71 | 72 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/servos.ino: -------------------------------------------------------------------------------- 1 | /*************************************************************** 2 | Servo Sweep - by Nathaniel Gallinger 3 | 4 | Sweep servos one degree step at a time with a user defined 5 | delay in between steps. Supports changing direction 6 | mid-sweep. Important for applications such as robotic arms 7 | where the stock servo speed is too fast for the strength 8 | of your system. 9 | 10 | *************************************************************/ 11 | 12 | #ifdef USE_SERVOS 13 | 14 | 15 | // Constructor 16 | SweepServo::SweepServo() 17 | { 18 | this->currentPositionDegrees = 0; 19 | this->targetPositionDegrees = 0; 20 | this->lastSweepCommand = 0; 21 | } 22 | 23 | 24 | // Init 25 | void SweepServo::initServo( 26 | int servoPin, 27 | int stepDelayMs, 28 | int initPosition) 29 | { 30 | this->servo.attach(servoPin); 31 | this->stepDelayMs = stepDelayMs; 32 | this->currentPositionDegrees = initPosition; 33 | this->targetPositionDegrees = initPosition; 34 | this->lastSweepCommand = millis(); 35 | } 36 | 37 | 38 | // Perform Sweep 39 | void SweepServo::doSweep() 40 | { 41 | 42 | // Get ellapsed time 43 | int delta = millis() - this->lastSweepCommand; 44 | 45 | // Check if time for a step 46 | if (delta > this->stepDelayMs) { 47 | // Check step direction 48 | if (this->targetPositionDegrees > this->currentPositionDegrees) { 49 | this->currentPositionDegrees++; 50 | this->servo.write(this->currentPositionDegrees); 51 | } 52 | else if (this->targetPositionDegrees < this->currentPositionDegrees) { 53 | this->currentPositionDegrees--; 54 | this->servo.write(this->currentPositionDegrees); 55 | } 56 | // if target == current position, do nothing 57 | 58 | // reset timer 59 | this->lastSweepCommand = millis(); 60 | } 61 | } 62 | 63 | 64 | // Set a new target position 65 | void SweepServo::setTargetPosition(int position) 66 | { 67 | this->targetPositionDegrees = position; 68 | } 69 | 70 | 71 | // Accessor for servo object 72 | Servo SweepServo::getServo() 73 | { 74 | return this->servo; 75 | } 76 | 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.ino: -------------------------------------------------------------------------------- 1 | /*************************************************************** 2 | Motor driver definitions 3 | 4 | Add a "#elif defined" block to this file to include support 5 | for a particular motor driver. Then add the appropriate 6 | #define near the top of the main ROSArduinoBridge.ino file. 7 | 8 | *************************************************************/ 9 | 10 | #ifdef USE_BASE 11 | 12 | #ifdef POLOLU_VNH5019 13 | /* Include the Pololu library */ 14 | #include "DualVNH5019MotorShield.h" 15 | 16 | /* Create the motor driver object */ 17 | DualVNH5019MotorShield drive; 18 | 19 | /* Wrap the motor driver initialization */ 20 | void initMotorController() { 21 | drive.init(); 22 | } 23 | 24 | /* Wrap the drive motor set speed function */ 25 | void setMotorSpeed(int i, int spd) { 26 | if (i == LEFT) drive.setM1Speed(spd); 27 | else drive.setM2Speed(spd); 28 | } 29 | 30 | // A convenience function for setting both motor speeds 31 | void setMotorSpeeds(int leftSpeed, int rightSpeed) { 32 | setMotorSpeed(LEFT, leftSpeed); 33 | setMotorSpeed(RIGHT, rightSpeed); 34 | } 35 | #elif defined POLOLU_MC33926 36 | /* Include the Pololu library */ 37 | #include "DualMC33926MotorShield.h" 38 | 39 | /* Create the motor driver object */ 40 | DualMC33926MotorShield drive; 41 | 42 | /* Wrap the motor driver initialization */ 43 | void initMotorController() { 44 | drive.init(); 45 | } 46 | 47 | /* Wrap the drive motor set speed function */ 48 | void setMotorSpeed(int i, int spd) { 49 | if (i == LEFT) drive.setM1Speed(spd); 50 | else drive.setM2Speed(spd); 51 | } 52 | 53 | // A convenience function for setting both motor speeds 54 | void setMotorSpeeds(int leftSpeed, int rightSpeed) { 55 | setMotorSpeed(LEFT, leftSpeed); 56 | setMotorSpeed(RIGHT, rightSpeed); 57 | } 58 | 59 | #elif defined L298N_DUAL_HBRIDGE 60 | 61 | boolean directionLeft = false; 62 | boolean directionRight = false; 63 | 64 | boolean direction(int i){ 65 | if(i == LEFT){ 66 | return directionLeft; 67 | }else{ 68 | return directionRight; 69 | } 70 | } 71 | void initMotorController() { 72 | // set all the motor control pins to outputs 73 | pinMode(ENA, OUTPUT); 74 | pinMode(ENB, OUTPUT); 75 | pinMode(IN1, OUTPUT); 76 | pinMode(IN2, OUTPUT); 77 | pinMode(IN3, OUTPUT); 78 | pinMode(IN4, OUTPUT); 79 | } 80 | 81 | void setMotorSpeed(int i, int spd) { 82 | if(spd>MAX_PWM){ 83 | spd=MAX_PWM; 84 | } 85 | if(spd<-MAX_PWM){ 86 | spd=-1*MAX_PWM; 87 | } 88 | if (i == LEFT){ 89 | if(spd>=0){ 90 | directionLeft = FORWARDS; 91 | digitalWrite(IN2, HIGH); 92 | digitalWrite(IN1, LOW); 93 | analogWrite(ENA, spd); 94 | }else if(spd < 0){ 95 | directionLeft = BACKWARDS; 96 | digitalWrite(IN1, HIGH); 97 | digitalWrite(IN2, LOW); 98 | analogWrite(ENA, -spd); 99 | } 100 | } 101 | else { 102 | if(spd>=0){ 103 | directionRight = FORWARDS; 104 | digitalWrite(IN4, HIGH); 105 | digitalWrite(IN3, LOW); 106 | analogWrite(ENB, spd); 107 | }else if(spd<0){ 108 | directionRight = BACKWARDS; 109 | digitalWrite(IN3, HIGH); 110 | digitalWrite(IN4, LOW); 111 | analogWrite(ENB, -spd); 112 | } 113 | } 114 | } 115 | 116 | void setMotorSpeeds(int leftSpeed, int rightSpeed) { 117 | setMotorSpeed(LEFT, leftSpeed); 118 | setMotorSpeed(RIGHT, rightSpeed); 119 | } 120 | #else 121 | #error A motor driver must be selected! 122 | #endif 123 | 124 | #endif 125 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/diff_controller.h: -------------------------------------------------------------------------------- 1 | /* Functions and type-defs for PID control. 2 | 3 | Taken mostly from Mike Ferguson's ArbotiX code which lives at: 4 | 5 | http://vanadium-ros-pkg.googlecode.com/svn/trunk/arbotix/ 6 | */ 7 | 8 | /* PID setpoint info For a Motor */ 9 | typedef struct { 10 | double TargetTicksPerFrame; // target speed in ticks per frame 11 | long Encoder; // encoder count 12 | long PrevEnc; // last encoder count 13 | 14 | /* 15 | * Using previous input (PrevInput) instead of PrevError to avoid derivative kick, 16 | * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ 17 | */ 18 | int PrevInput; // last input 19 | //int PrevErr; // last error 20 | 21 | /* 22 | * Using integrated term (ITerm) instead of integrated error (Ierror), 23 | * to allow tuning changes, 24 | * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ 25 | */ 26 | //int Ierror; 27 | int ITerm; //integrated term 28 | 29 | long output; // last motor setting 30 | } 31 | SetPointInfo; 32 | 33 | SetPointInfo leftPID, rightPID; 34 | 35 | /* PID Parameters */ 36 | int Kp = 20; 37 | int Kd = 12; 38 | int Ki = 0; 39 | int Ko = 50; 40 | 41 | unsigned char moving = 0; // is the base in motion? 42 | 43 | /* 44 | * Initialize PID variables to zero to prevent startup spikes 45 | * when turning PID on to start moving 46 | * In particular, assign both Encoder and PrevEnc the current encoder value 47 | * See http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/ 48 | * Note that the assumption here is that PID is only turned on 49 | * when going from stop to moving, that's why we can init everything on zero. 50 | */ 51 | void resetPID(){ 52 | leftPID.TargetTicksPerFrame = 0.0; 53 | leftPID.Encoder = readEncoder(LEFT); 54 | leftPID.PrevEnc = leftPID.Encoder; 55 | leftPID.output = 0; 56 | leftPID.PrevInput = 0; 57 | leftPID.ITerm = 0; 58 | 59 | rightPID.TargetTicksPerFrame = 0.0; 60 | rightPID.Encoder = readEncoder(RIGHT); 61 | rightPID.PrevEnc = rightPID.Encoder; 62 | rightPID.output = 0; 63 | rightPID.PrevInput = 0; 64 | rightPID.ITerm = 0; 65 | } 66 | 67 | /* PID routine to compute the next motor commands */ 68 | void doPID(SetPointInfo * p) { 69 | long Perror; 70 | long output; 71 | int input; 72 | 73 | //Perror = p->TargetTicksPerFrame - (p->Encoder - p->PrevEnc); 74 | input = p->Encoder - p->PrevEnc; 75 | Perror = p->TargetTicksPerFrame - input; 76 | 77 | 78 | /* 79 | * Avoid derivative kick and allow tuning changes, 80 | * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/ 81 | * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ 82 | */ 83 | //output = (Kp * Perror + Kd * (Perror - p->PrevErr) + Ki * p->Ierror) / Ko; 84 | // p->PrevErr = Perror; 85 | output = (Kp * Perror - Kd * (input - p->PrevInput) + p->ITerm) / Ko; 86 | p->PrevEnc = p->Encoder; 87 | 88 | output += p->output; 89 | // Accumulate Integral error *or* Limit output. 90 | // Stop accumulating when output saturates 91 | if (output >= MAX_PWM) 92 | output = MAX_PWM; 93 | else if (output <= -MAX_PWM) 94 | output = -MAX_PWM; 95 | else 96 | /* 97 | * allow turning changes, see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-tuning-changes/ 98 | */ 99 | p->ITerm += Ki * Perror; 100 | 101 | p->output = output; 102 | p->PrevInput = input; 103 | } 104 | 105 | /* Read the encoder values and call the PID routine */ 106 | void updatePID() { 107 | /* Read the encoders */ 108 | leftPID.Encoder = readEncoder(LEFT); 109 | rightPID.Encoder = readEncoder(RIGHT); 110 | 111 | /* If we're not moving there is nothing more to do */ 112 | if (!moving){ 113 | /* 114 | * Reset PIDs once, to prevent startup spikes, 115 | * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/ 116 | * PrevInput is considered a good proxy to detect 117 | * whether reset has already happened 118 | */ 119 | if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID(); 120 | return; 121 | } 122 | 123 | /* Compute PID update for each motor */ 124 | doPID(&rightPID); 125 | doPID(&leftPID); 126 | 127 | /* Set the motor speeds accordingly */ 128 | setMotorSpeeds(leftPID.output, rightPID.output); 129 | } 130 | 131 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/encoder_driver.ino: -------------------------------------------------------------------------------- 1 | /* ************************************************************* 2 | Encoder definitions 3 | 4 | Add an "#ifdef" block to this file to include support for 5 | a particular encoder board or library. Then add the appropriate 6 | #define near the top of the main ROSArduinoBridge.ino file. 7 | 8 | ************************************************************ */ 9 | 10 | #ifdef USE_BASE 11 | 12 | #ifdef ROBOGAIA 13 | /* The Robogaia Mega Encoder shield */ 14 | #include "MegaEncoderCounter.h" 15 | 16 | /* Create the encoder shield object */ 17 | MegaEncoderCounter encoders = MegaEncoderCounter(4); // Initializes the Mega Encoder Counter in the 4X Count mode 18 | 19 | /* Wrap the encoder reading function */ 20 | long readEncoder(int i) { 21 | if (i == LEFT) return encoders.YAxisGetCount(); 22 | else return encoders.XAxisGetCount(); 23 | } 24 | 25 | /* Wrap the encoder reset function */ 26 | void resetEncoder(int i) { 27 | if (i == LEFT) return encoders.YAxisReset(); 28 | else return encoders.XAxisReset(); 29 | } 30 | #elif defined(ARDUINO_ENC_COUNTER) 31 | volatile long left_enc_pos = 0L; 32 | volatile long right_enc_pos = 0L; 33 | static const int8_t ENC_STATES [] = {0,1,-1,0,-1,0,0,1,1,0,0,-1,0,-1,1,0}; //encoder lookup table 34 | 35 | /* Interrupt routine for LEFT encoder, taking care of actual counting */ 36 | ISR (PCINT2_vect){ 37 | static uint8_t enc_last=0; 38 | 39 | enc_last <<=2; //shift previous state two places 40 | enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits 41 | 42 | left_enc_pos += ENC_STATES[(enc_last & 0x0f)]; 43 | } 44 | 45 | /* Interrupt routine for RIGHT encoder, taking care of actual counting */ 46 | ISR (PCINT1_vect){ 47 | static uint8_t enc_last=0; 48 | 49 | enc_last <<=2; //shift previous state two places 50 | enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits 51 | 52 | right_enc_pos += ENC_STATES[(enc_last & 0x0f)]; 53 | } 54 | 55 | /* Wrap the encoder reading function */ 56 | long readEncoder(int i) { 57 | if (i == LEFT) return left_enc_pos; 58 | else return right_enc_pos; 59 | } 60 | 61 | /* Wrap the encoder reset function */ 62 | void resetEncoder(int i) { 63 | if (i == LEFT){ 64 | left_enc_pos=0L; 65 | return; 66 | } else { 67 | right_enc_pos=0L; 68 | return; 69 | } 70 | } 71 | #elif defined(ARDUINO_MY_COUNTER) 72 | volatile long left_enc_pos = 0L; 73 | volatile long right_enc_pos = 0L; 74 | 75 | void initEncoders(){ 76 | pinMode(LEFT_ENC_A, INPUT); 77 | pinMode(LEFT_ENC_B, INPUT); 78 | pinMode(RIGHT_ENC_A, INPUT); 79 | pinMode(RIGHT_ENC_B, INPUT); 80 | 81 | attachInterrupt(0, leftEncoderEvent, CHANGE); 82 | attachInterrupt(2, rightEncoderEvent, CHANGE); 83 | } 84 | 85 | // encoder event for the interrupt call 86 | void leftEncoderEvent() { 87 | if (digitalRead(LEFT_ENC_A) == HIGH) { 88 | if (digitalRead(LEFT_ENC_B) == LOW) { 89 | left_enc_pos++; 90 | } 91 | else { 92 | left_enc_pos--; 93 | } 94 | } 95 | else { 96 | if (digitalRead(LEFT_ENC_B) == LOW) { 97 | left_enc_pos--; 98 | } 99 | else { 100 | left_enc_pos++; 101 | } 102 | } 103 | } 104 | 105 | // encoder event for the interrupt call 106 | void rightEncoderEvent() { 107 | if (digitalRead(RIGHT_ENC_A) == HIGH) { 108 | if (digitalRead(RIGHT_ENC_B) == LOW) { 109 | right_enc_pos++; 110 | } 111 | else { 112 | right_enc_pos--; 113 | } 114 | } 115 | else { 116 | if (digitalRead(RIGHT_ENC_B) == LOW) { 117 | right_enc_pos--; 118 | } 119 | else { 120 | right_enc_pos++; 121 | } 122 | } 123 | } 124 | 125 | /* Wrap the encoder reading function */ 126 | long readEncoder(int i) { 127 | if (i == LEFT) return left_enc_pos; 128 | else return -right_enc_pos; // It's just because my right encoder get reverse value so if yours is normal, don't add "-" 129 | } 130 | 131 | /* Wrap the encoder reset function */ 132 | void resetEncoder(int i) { 133 | if (i == LEFT){ 134 | left_enc_pos=0L; 135 | return; 136 | } else { 137 | right_enc_pos=0L; 138 | return; 139 | } 140 | } 141 | #else 142 | #error A encoder driver must be selected! 143 | #endif 144 | 145 | /* Wrap the encoder reset function */ 146 | void resetEncoders() { 147 | resetEncoder(LEFT); 148 | resetEncoder(RIGHT); 149 | } 150 | 151 | #endif 152 | -------------------------------------------------------------------------------- /ros_arduino_python/nodes/arduino_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | A ROS Node for the Arduino microcontroller 5 | 6 | Created for the Pi Robot Project: http://www.pirobot.org 7 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 8 | 9 | This program is free software; you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation; either version 2 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details at: 18 | 19 | http://www.gnu.org/licenses/gpl.html 20 | """ 21 | 22 | import rospy 23 | from ros_arduino_python.arduino_driver import Arduino 24 | from ros_arduino_python.arduino_sensors import * 25 | from ros_arduino_msgs.srv import * 26 | from ros_arduino_python.base_controller import BaseController 27 | from geometry_msgs.msg import Twist 28 | import os, time 29 | import thread 30 | 31 | class ArduinoROS(): 32 | def __init__(self): 33 | rospy.init_node('Arduino', log_level=rospy.DEBUG) 34 | 35 | # Cleanup when termniating the node 36 | rospy.on_shutdown(self.shutdown) 37 | 38 | self.port = rospy.get_param("~port", "/dev/ttyACM0") 39 | self.baud = int(rospy.get_param("~baud", 57600)) 40 | self.timeout = rospy.get_param("~timeout", 0.5) 41 | self.base_frame = rospy.get_param("~base_frame", 'base_link') 42 | 43 | # Overall loop rate: should be faster than fastest sensor rate 44 | self.rate = int(rospy.get_param("~rate", 50)) 45 | r = rospy.Rate(self.rate) 46 | 47 | # Rate at which summary SensorState message is published. Individual sensors publish 48 | # at their own rates. 49 | self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10)) 50 | 51 | self.use_base_controller = rospy.get_param("~use_base_controller", False) 52 | 53 | # Set up the time for publishing the next SensorState message 54 | now = rospy.Time.now() 55 | self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate) 56 | self.t_next_sensors = now + self.t_delta_sensors 57 | 58 | # Initialize a Twist message 59 | self.cmd_vel = Twist() 60 | 61 | # A cmd_vel publisher so we can stop the robot when shutting down 62 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 63 | 64 | # The SensorState publisher periodically publishes the values of all sensors on 65 | # a single topic. 66 | self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5) 67 | 68 | # A service to position a PWM servo 69 | rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler) 70 | 71 | # A service to read the position of a PWM servo 72 | rospy.Service('~servo_read', ServoRead, self.ServoReadHandler) 73 | 74 | # A service to turn set the direction of a digital pin (0 = input, 1 = output) 75 | rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler) 76 | 77 | # A service to turn a digital sensor on or off 78 | rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler) 79 | 80 | # A service to read the value of a digital sensor 81 | rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler) 82 | 83 | # A service to set pwm values for the pins 84 | rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler) 85 | 86 | # A service to read the value of an analog sensor 87 | rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler) 88 | 89 | # Initialize the controlller 90 | self.controller = Arduino(self.port, self.baud, self.timeout) 91 | 92 | # Make the connection 93 | self.controller.connect() 94 | 95 | rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud") 96 | 97 | # Reserve a thread lock 98 | mutex = thread.allocate_lock() 99 | 100 | # Initialize any sensors 101 | self.mySensors = list() 102 | 103 | sensor_params = rospy.get_param("~sensors", dict({})) 104 | 105 | for name, params in sensor_params.iteritems(): 106 | # Set the direction to input if not specified 107 | try: 108 | params['direction'] 109 | except: 110 | params['direction'] = 'input' 111 | 112 | if params['type'] == "Ping": 113 | sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame) 114 | elif params['type'] == "GP2D12": 115 | sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame) 116 | elif params['type'] == 'Digital': 117 | sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) 118 | elif params['type'] == 'Analog': 119 | sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction']) 120 | elif params['type'] == 'PololuMotorCurrent': 121 | sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) 122 | elif params['type'] == 'PhidgetsVoltage': 123 | sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame) 124 | elif params['type'] == 'PhidgetsCurrent': 125 | sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame) 126 | 127 | # if params['type'] == "MaxEZ1": 128 | # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin'] 129 | # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin'] 130 | 131 | self.mySensors.append(sensor) 132 | rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name) 133 | 134 | # Initialize the base controller if used 135 | if self.use_base_controller: 136 | self.myBaseController = BaseController(self.controller, self.base_frame) 137 | 138 | # Start polling the sensors and base controller 139 | while not rospy.is_shutdown(): 140 | for sensor in self.mySensors: 141 | mutex.acquire() 142 | sensor.poll() 143 | mutex.release() 144 | 145 | if self.use_base_controller: 146 | mutex.acquire() 147 | self.myBaseController.poll() 148 | mutex.release() 149 | 150 | # Publish all sensor values on a single topic for convenience 151 | now = rospy.Time.now() 152 | 153 | if now > self.t_next_sensors: 154 | msg = SensorState() 155 | msg.header.frame_id = self.base_frame 156 | msg.header.stamp = now 157 | for i in range(len(self.mySensors)): 158 | msg.name.append(self.mySensors[i].name) 159 | msg.value.append(self.mySensors[i].value) 160 | try: 161 | self.sensorStatePub.publish(msg) 162 | except: 163 | pass 164 | 165 | self.t_next_sensors = now + self.t_delta_sensors 166 | 167 | r.sleep() 168 | 169 | # Service callback functions 170 | def ServoWriteHandler(self, req): 171 | self.controller.servo_write(req.id, req.value) 172 | return ServoWriteResponse() 173 | 174 | def ServoReadHandler(self, req): 175 | pos = self.controller.servo_read(req.id) 176 | return ServoReadResponse(pos) 177 | 178 | def DigitalSetDirectionHandler(self, req): 179 | self.controller.pin_mode(req.pin, req.direction) 180 | return DigitalSetDirectionResponse() 181 | 182 | def DigitalWriteHandler(self, req): 183 | self.controller.digital_write(req.pin, req.value) 184 | return DigitalWriteResponse() 185 | 186 | def DigitalReadHandler(self, req): 187 | value = self.controller.digital_read(req.pin) 188 | return DigitalReadResponse(value) 189 | 190 | def AnalogWriteHandler(self, req): 191 | self.controller.analog_write(req.pin, req.value) 192 | return AnalogWriteResponse() 193 | 194 | def AnalogReadHandler(self, req): 195 | value = self.controller.analog_read(req.pin) 196 | return AnalogReadResponse(value) 197 | 198 | def shutdown(self): 199 | # Stop the robot 200 | try: 201 | rospy.loginfo("Stopping the robot...") 202 | self.cmd_vel_pub.Publish(Twist()) 203 | rospy.sleep(2) 204 | except: 205 | pass 206 | rospy.loginfo("Shutting down Arduino Node...") 207 | 208 | if __name__ == '__main__': 209 | myArduino = ArduinoROS() 210 | -------------------------------------------------------------------------------- /ros_arduino_python/src/ros_arduino_python/arduino_sensors.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Sensor class for the arudino_python package 5 | 6 | Created for the Pi Robot Project: http://www.pirobot.org 7 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 8 | 9 | This program is free software; you can redistribute it and/or modify 10 | it under the terms of the GNU General Public License as published by 11 | the Free Software Foundation; either version 2 of the License, or 12 | (at your option) any later version. 13 | 14 | This program is distributed in the hope that it will be useful, 15 | but WITHOUT ANY WARRANTY; without even the implied warranty of 16 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 17 | GNU General Public License for more details at: 18 | 19 | http://www.gnu.org/licenses/gpl.html 20 | """ 21 | 22 | import roslib; roslib.load_manifest('ros_arduino_python') 23 | import rospy 24 | from sensor_msgs.msg import Range 25 | from ros_arduino_msgs.msg import * 26 | 27 | LOW = 0 28 | HIGH = 1 29 | 30 | INPUT = 0 31 | OUTPUT = 1 32 | 33 | class MessageType: 34 | ANALOG = 0 35 | DIGITAL = 1 36 | RANGE = 2 37 | FLOAT = 3 38 | INT = 4 39 | BOOL = 5 40 | 41 | class Sensor(object): 42 | def __init__(self, controller, name, pin, rate, frame_id, direction="input", **kwargs): 43 | self.controller = controller 44 | self.name = name 45 | self.pin = pin 46 | self.rate = rate 47 | self.direction = direction 48 | 49 | self.frame_id = frame_id 50 | self.value = None 51 | 52 | self.t_delta = rospy.Duration(1.0 / self.rate) 53 | self.t_next = rospy.Time.now() + self.t_delta 54 | 55 | def poll(self): 56 | now = rospy.Time.now() 57 | if now > self.t_next: 58 | if self.direction == "input": 59 | try: 60 | self.value = self.read_value() 61 | except: 62 | return 63 | else: 64 | try: 65 | self.ack = self.write_value() 66 | except: 67 | return 68 | 69 | # For range sensors, assign the value to the range message field 70 | if self.message_type == MessageType.RANGE: 71 | self.msg.range = self.value 72 | else: 73 | self.msg.value = self.value 74 | 75 | # Add a timestamp and publish the message 76 | self.msg.header.stamp = rospy.Time.now() 77 | self.pub.publish(self.msg) 78 | 79 | self.t_next = now + self.t_delta 80 | 81 | class AnalogSensor(Sensor): 82 | def __init__(self, *args, **kwargs): 83 | super(AnalogSensor, self).__init__(*args, **kwargs) 84 | 85 | self.message_type = MessageType.ANALOG 86 | 87 | self.msg = Analog() 88 | self.msg.header.frame_id = self.frame_id 89 | 90 | self.pub = rospy.Publisher("~sensor/" + self.name, Analog, queue_size=5) 91 | 92 | if self.direction == "output": 93 | self.controller.pin_mode(self.pin, OUTPUT) 94 | else: 95 | self.controller.pin_mode(self.pin, INPUT) 96 | 97 | self.value = LOW 98 | 99 | def read_value(self): 100 | return self.controller.analog_read(self.pin) 101 | 102 | def write_value(self, value): 103 | return self.controller.analog_write(self.pin, value) 104 | 105 | class AnalogFloatSensor(Sensor): 106 | def __init__(self, *args, **kwargs): 107 | super(AnalogFloatSensor, self).__init__(*args, **kwargs) 108 | 109 | self.message_type = MessageType.ANALOG 110 | 111 | self.msg = AnalogFloat() 112 | self.msg.header.frame_id = self.frame_id 113 | 114 | self.pub = rospy.Publisher("~sensor/" + self.name, AnalogFloat, queue_size=5) 115 | 116 | if self.direction == "output": 117 | self.controller.pin_mode(self.pin, OUTPUT) 118 | else: 119 | self.controller.pin_mode(self.pin, INPUT) 120 | 121 | self.value = LOW 122 | 123 | def read_value(self): 124 | return self.controller.analog_read(self.pin) 125 | 126 | def write_value(self, value): 127 | return self.controller.analog_write(self.pin, value) 128 | 129 | 130 | class DigitalSensor(Sensor): 131 | def __init__(self, *args, **kwargs): 132 | super(DigitalSensor, self).__init__(*args, **kwargs) 133 | 134 | self.message_type = MessageType.BOOL 135 | 136 | self.msg = Digital() 137 | self.msg.header.frame_id = self.frame_id 138 | 139 | self.pub = rospy.Publisher("~sensor/" + self.name, Digital, queue_size=5) 140 | 141 | if self.direction == "output": 142 | self.controller.pin_mode(self.pin, OUTPUT) 143 | else: 144 | self.controller.pin_mode(self.pin, INPUT) 145 | 146 | self.value = LOW 147 | 148 | def read_value(self): 149 | return self.controller.digital_read(self.pin) 150 | 151 | def write_value(self): 152 | # Alternate HIGH/LOW when writing at a fixed rate 153 | self.value = not self.value 154 | return self.controller.digital_write(self.pin, self.value) 155 | 156 | 157 | class RangeSensor(Sensor): 158 | def __init__(self, *args, **kwargs): 159 | super(RangeSensor, self).__init__(*args, **kwargs) 160 | 161 | self.message_type = MessageType.RANGE 162 | 163 | self.msg = Range() 164 | self.msg.header.frame_id = self.frame_id 165 | 166 | self.pub = rospy.Publisher("~sensor/" + self.name, Range, queue_size=5) 167 | 168 | def read_value(self): 169 | self.msg.header.stamp = rospy.Time.now() 170 | 171 | 172 | class SonarSensor(RangeSensor): 173 | def __init__(self, *args, **kwargs): 174 | super(SonarSensor, self).__init__(*args, **kwargs) 175 | 176 | self.msg.radiation_type = Range.ULTRASOUND 177 | 178 | 179 | class IRSensor(RangeSensor): 180 | def __init__(self, *args, **kwargs): 181 | super(IRSensor, self).__init__(*args, **kwargs) 182 | 183 | self.msg.radiation_type = Range.INFRARED 184 | 185 | class Ping(SonarSensor): 186 | def __init__(self,*args, **kwargs): 187 | super(Ping, self).__init__(*args, **kwargs) 188 | 189 | self.msg.field_of_view = 0.785398163 190 | self.msg.min_range = 0.02 191 | self.msg.max_range = 3.0 192 | 193 | def read_value(self): 194 | # The Arduino Ping code returns the distance in centimeters 195 | cm = self.controller.ping(self.pin) 196 | 197 | # Convert it to meters for ROS 198 | distance = cm / 100.0 199 | 200 | return distance 201 | 202 | 203 | class GP2D12(IRSensor): 204 | def __init__(self, *args, **kwargs): 205 | super(GP2D12, self).__init__(*args, **kwargs) 206 | 207 | self.msg.field_of_view = 0.001 208 | self.msg.min_range = 0.10 209 | self.msg.max_range = 0.80 210 | 211 | def read_value(self): 212 | value = self.controller.analog_read(self.pin) 213 | 214 | if value <= 3.0: 215 | return self.msg.max_range 216 | 217 | try: 218 | distance = (6787.0 / (float(value) - 3.0)) - 4.0 219 | except: 220 | return self.msg.max_range 221 | 222 | # Convert to meters 223 | distance /= 100.0 224 | 225 | # If we get a spurious reading, set it to the max_range 226 | if distance > self.msg.max_range: distance = self.msg.max_range 227 | if distance < self.msg.min_range: distance = self.msg.max_range 228 | 229 | return distance 230 | 231 | class PololuMotorCurrent(AnalogFloatSensor): 232 | def __init__(self, *args, **kwargs): 233 | super(PololuMotorCurrent, self).__init__(*args, **kwargs) 234 | 235 | def read_value(self): 236 | # From the Pololu source code 237 | milliamps = self.controller.analog_read(self.pin) * 34 238 | return milliamps / 1000.0 239 | 240 | class PhidgetsVoltage(AnalogFloatSensor): 241 | def __init__(self, *args, **kwargs): 242 | super(PhidgetsVoltage, self).__init__(*args, **kwargs) 243 | 244 | def read_value(self): 245 | # From the Phidgets documentation 246 | voltage = 0.06 * (self.controller.analog_read(self.pin) - 500.) 247 | return voltage 248 | 249 | class PhidgetsCurrent(AnalogFloatSensor): 250 | def __init__(self, *args, **kwargs): 251 | super(PhidgetsCurrent, self).__init__(*args, **kwargs) 252 | 253 | def read_value(self): 254 | # From the Phidgets documentation for the 20 amp DC sensor 255 | current = 0.05 * (self.controller.analog_read(self.pin) - 500.) 256 | return current 257 | 258 | class MaxEZ1Sensor(SonarSensor): 259 | def __init__(self, *args, **kwargs): 260 | super(MaxEZ1Sensor, self).__init__(*args, **kwargs) 261 | 262 | self.trigger_pin = kwargs['trigger_pin'] 263 | self.output_pin = kwargs['output_pin'] 264 | 265 | self.msg.field_of_view = 0.785398163 266 | self.msg.min_range = 0.02 267 | self.msg.max_range = 3.0 268 | 269 | def read_value(self): 270 | return self.controller.get_MaxEZ1(self.trigger_pin, self.output_pin) 271 | 272 | 273 | if __name__ == '__main__': 274 | myController = Controller() 275 | mySensor = SonarSensor(myController, "My Sonar", type=Type.PING, pin=0, rate=10) 276 | -------------------------------------------------------------------------------- /ros_arduino_python/src/ros_arduino_python/base_controller.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | A base controller class for the Arduino microcontroller 5 | 6 | Borrowed heavily from Mike Feguson's ArbotiX base_controller.py code. 7 | 8 | Created for the Pi Robot Project: http://www.pirobot.org 9 | Copyright (c) 2010 Patrick Goebel. All rights reserved. 10 | 11 | This program is free software; you can redistribute it and/or modify 12 | it under the terms of the GNU General Public License as published by 13 | the Free Software Foundation; either version 2 of the License, or 14 | (at your option) any later version. 15 | 16 | This program is distributed in the hope that it will be useful, 17 | but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | GNU General Public License for more details at: 20 | 21 | http://www.gnu.org/licenses 22 | """ 23 | import roslib; roslib.load_manifest('ros_arduino_python') 24 | import rospy 25 | import os 26 | 27 | from math import sin, cos, pi 28 | from geometry_msgs.msg import Quaternion, Twist, Pose 29 | from nav_msgs.msg import Odometry 30 | from tf.broadcaster import TransformBroadcaster 31 | 32 | """ Class to receive Twist commands and publish Odometry data """ 33 | class BaseController: 34 | def __init__(self, arduino, base_frame): 35 | self.arduino = arduino 36 | self.base_frame = base_frame 37 | self.rate = float(rospy.get_param("~base_controller_rate", 10)) 38 | self.timeout = rospy.get_param("~base_controller_timeout", 1.0) 39 | self.stopped = False 40 | 41 | pid_params = dict() 42 | pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "") 43 | pid_params['wheel_track'] = rospy.get_param("~wheel_track", "") 44 | pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "") 45 | pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0) 46 | pid_params['Kp'] = rospy.get_param("~Kp", 20) 47 | pid_params['Kd'] = rospy.get_param("~Kd", 12) 48 | pid_params['Ki'] = rospy.get_param("~Ki", 0) 49 | pid_params['Ko'] = rospy.get_param("~Ko", 50) 50 | 51 | self.accel_limit = rospy.get_param('~accel_limit', 0.1) 52 | self.motors_reversed = rospy.get_param("~motors_reversed", False) 53 | 54 | # Set up PID parameters and check for missing values 55 | self.setup_pid(pid_params) 56 | 57 | # How many encoder ticks are there per meter? 58 | self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi) 59 | 60 | # What is the maximum acceleration we will tolerate when changing wheel speeds? 61 | self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate 62 | 63 | # Track how often we get a bad encoder count (if any) 64 | self.bad_encoder_count = 0 65 | 66 | now = rospy.Time.now() 67 | self.then = now # time for determining dx/dy 68 | self.t_delta = rospy.Duration(1.0 / self.rate) 69 | self.t_next = now + self.t_delta 70 | 71 | # Internal data 72 | self.enc_left = None # encoder readings 73 | self.enc_right = None 74 | self.x = 0 # position in xy plane 75 | self.y = 0 76 | self.th = 0 # rotation in radians 77 | self.v_left = 0 78 | self.v_right = 0 79 | self.v_des_left = 0 # cmd_vel setpoint 80 | self.v_des_right = 0 81 | self.last_cmd_vel = now 82 | 83 | # Subscriptions 84 | rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback) 85 | 86 | # Clear any old odometry info 87 | self.arduino.reset_encoders() 88 | 89 | # Set up the odometry broadcaster 90 | self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5) 91 | self.odomBroadcaster = TransformBroadcaster() 92 | 93 | rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev") 94 | rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame") 95 | 96 | def setup_pid(self, pid_params): 97 | # Check to see if any PID parameters are missing 98 | missing_params = False 99 | for param in pid_params: 100 | if pid_params[param] == "": 101 | print("*** PID Parameter " + param + " is missing. ***") 102 | missing_params = True 103 | 104 | if missing_params: 105 | os._exit(1) 106 | 107 | self.wheel_diameter = pid_params['wheel_diameter'] 108 | self.wheel_track = pid_params['wheel_track'] 109 | self.encoder_resolution = pid_params['encoder_resolution'] 110 | self.gear_reduction = pid_params['gear_reduction'] 111 | 112 | self.Kp = pid_params['Kp'] 113 | self.Kd = pid_params['Kd'] 114 | self.Ki = pid_params['Ki'] 115 | self.Ko = pid_params['Ko'] 116 | 117 | self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko) 118 | 119 | def poll(self): 120 | now = rospy.Time.now() 121 | if now > self.t_next: 122 | # Read the encoders 123 | try: 124 | left_enc, right_enc = self.arduino.get_encoder_counts() 125 | except: 126 | self.bad_encoder_count += 1 127 | rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count)) 128 | return 129 | 130 | dt = now - self.then 131 | self.then = now 132 | dt = dt.to_sec() 133 | 134 | # Calculate odometry 135 | if self.enc_left == None: 136 | dright = 0 137 | dleft = 0 138 | else: 139 | dright = (right_enc - self.enc_right) / self.ticks_per_meter 140 | dleft = (left_enc - self.enc_left) / self.ticks_per_meter 141 | 142 | self.enc_right = right_enc 143 | self.enc_left = left_enc 144 | 145 | dxy_ave = (dright + dleft) / 2.0 146 | dth = (dright - dleft) / self.wheel_track 147 | vxy = dxy_ave / dt 148 | vth = dth / dt 149 | 150 | if (dxy_ave != 0): 151 | dx = cos(dth) * dxy_ave 152 | dy = -sin(dth) * dxy_ave 153 | self.x += (cos(self.th) * dx - sin(self.th) * dy) 154 | self.y += (sin(self.th) * dx + cos(self.th) * dy) 155 | 156 | if (dth != 0): 157 | self.th += dth 158 | 159 | quaternion = Quaternion() 160 | quaternion.x = 0.0 161 | quaternion.y = 0.0 162 | quaternion.z = sin(self.th / 2.0) 163 | quaternion.w = cos(self.th / 2.0) 164 | 165 | # Create the odometry transform frame broadcaster. 166 | self.odomBroadcaster.sendTransform( 167 | (self.x, self.y, 0), 168 | (quaternion.x, quaternion.y, quaternion.z, quaternion.w), 169 | rospy.Time.now(), 170 | self.base_frame, 171 | "odom" 172 | ) 173 | 174 | odom = Odometry() 175 | odom.header.frame_id = "odom" 176 | odom.child_frame_id = self.base_frame 177 | odom.header.stamp = now 178 | odom.pose.pose.position.x = self.x 179 | odom.pose.pose.position.y = self.y 180 | odom.pose.pose.position.z = 0 181 | odom.pose.pose.orientation = quaternion 182 | odom.twist.twist.linear.x = vxy 183 | odom.twist.twist.linear.y = 0 184 | odom.twist.twist.angular.z = vth 185 | 186 | self.odomPub.publish(odom) 187 | 188 | if now > (self.last_cmd_vel + rospy.Duration(self.timeout)): 189 | self.v_des_left = 0 190 | self.v_des_right = 0 191 | 192 | if self.v_left < self.v_des_left: 193 | self.v_left += self.max_accel 194 | if self.v_left > self.v_des_left: 195 | self.v_left = self.v_des_left 196 | else: 197 | self.v_left -= self.max_accel 198 | if self.v_left < self.v_des_left: 199 | self.v_left = self.v_des_left 200 | 201 | if self.v_right < self.v_des_right: 202 | self.v_right += self.max_accel 203 | if self.v_right > self.v_des_right: 204 | self.v_right = self.v_des_right 205 | else: 206 | self.v_right -= self.max_accel 207 | if self.v_right < self.v_des_right: 208 | self.v_right = self.v_des_right 209 | 210 | # Set motor speeds in encoder ticks per PID loop 211 | if not self.stopped: 212 | self.arduino.drive(self.v_left, self.v_right) 213 | 214 | self.t_next = now + self.t_delta 215 | 216 | def stop(self): 217 | self.stopped = True 218 | self.arduino.drive(0, 0) 219 | 220 | def cmdVelCallback(self, req): 221 | # Handle velocity-based movement requests 222 | self.last_cmd_vel = rospy.Time.now() 223 | 224 | x = req.linear.x # m/s 225 | th = req.angular.z # rad/s 226 | 227 | if x == 0: 228 | # Turn in place 229 | right = th * self.wheel_track * self.gear_reduction / 2.0 230 | left = -right 231 | elif th == 0: 232 | # Pure forward/backward motion 233 | left = right = x 234 | else: 235 | # Rotation about a point in space 236 | left = x - th * self.wheel_track * self.gear_reduction / 2.0 237 | right = x + th * self.wheel_track * self.gear_reduction / 2.0 238 | 239 | self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE) 240 | self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE) 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/MegaRobogaiaPololu/MegaRobogaiaPololu.ino: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * ROSArduinoBridge 3 | 4 | A set of simple serial commands to control a differential drive 5 | robot and receive back sensor and odometry data. Default 6 | configuration assumes use of an Arduino Mega + Pololu motor 7 | controller shield + Robogaia Mega Encoder shield. Edit the 8 | readEncoder() and setMotorSpeed() wrapper functions if using 9 | different motor controller or encoder method. 10 | 11 | Created for the Pi Robot Project: http://www.pirobot.org 12 | 13 | Inspired and modeled after the ArbotiX driver by Michael Ferguson 14 | 15 | Software License Agreement (BSD License) 16 | 17 | Copyright (c) 2012, Patrick Goebel. 18 | All rights reserved. 19 | 20 | Redistribution and use in source and binary forms, with or without 21 | modification, are permitted provided that the following conditions 22 | are met: 23 | 24 | * Redistributions of source code must retain the above copyright 25 | notice, this list of conditions and the following disclaimer. 26 | * Redistributions in binary form must reproduce the above 27 | copyright notice, this list of conditions and the following 28 | disclaimer in the documentation and/or other materials provided 29 | with the distribution. 30 | 31 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 32 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 33 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 34 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 35 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 36 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 37 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 38 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 39 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 40 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 41 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 42 | * POSSIBILITY OF SUCH DAMAGE. 43 | *********************************************************************/ 44 | 45 | #define USE_BASE // Enable the base controller code 46 | //#undef USE_BASE // Disable the base controller code 47 | 48 | //#define USE_SERVOS // Enable use of PWM servos as defined in servos.h 49 | #undef USE_SERVOS // Disable use of PWM servos 50 | 51 | /* Serial port baud rate */ 52 | #define BAUDRATE 57600 53 | 54 | /* Maximum PWM signal */ 55 | #define MAX_PWM 255 56 | 57 | #if defined(ARDUINO) && ARDUINO >= 100 58 | #include "Arduino.h" 59 | #else 60 | #include "WProgram.h" 61 | #endif 62 | 63 | /* Include definition of serial commands */ 64 | #include "commands.h" 65 | 66 | /* Sensor functions */ 67 | #include "sensors.h" 68 | 69 | /* Include servo support if required */ 70 | #ifdef USE_SERVOS 71 | #include 72 | #include "servos.h" 73 | #endif 74 | 75 | #ifdef USE_BASE 76 | /* The Pololu motor driver shield */ 77 | #include "DualVNH5019MotorShield.h" 78 | 79 | /* The Robogaia Mega Encoder shield */ 80 | #include "MegaEncoderCounter.h" 81 | 82 | /* Create the motor driver object */ 83 | DualVNH5019MotorShield drive; 84 | 85 | /* Create the encoder shield object */ 86 | MegaEncoderCounter encoders(4); // Initializes the Mega Encoder Counter in the 4X Count mode 87 | 88 | /* PID parameters and functions */ 89 | #include "diff_controller.h" 90 | 91 | /* Run the PID loop at 30 times per second */ 92 | #define PID_RATE 30 // Hz 93 | 94 | /* Convert the rate into an interval */ 95 | const int PID_INTERVAL = 1000 / PID_RATE; 96 | 97 | /* Track the next time we make a PID calculation */ 98 | unsigned long nextPID = PID_INTERVAL; 99 | 100 | /* Stop the robot if it hasn't received a movement command 101 | in this number of milliseconds */ 102 | #define AUTO_STOP_INTERVAL 2000 103 | long lastMotorCommand = AUTO_STOP_INTERVAL; 104 | 105 | /* Wrap the encoder reading function */ 106 | long readEncoder(int i) { 107 | if (i == LEFT) return encoders.YAxisGetCount(); 108 | else return encoders.XAxisGetCount(); 109 | } 110 | 111 | /* Wrap the encoder reset function */ 112 | void resetEncoder(int i) { 113 | if (i == LEFT) return encoders.YAxisReset(); 114 | else return encoders.XAxisReset(); 115 | } 116 | 117 | /* Wrap the encoder reset function */ 118 | void resetEncoders() { 119 | resetEncoder(LEFT); 120 | resetEncoder(RIGHT); 121 | } 122 | 123 | /* Wrap the motor driver initialization */ 124 | void initMotorController() { 125 | drive.init(); 126 | } 127 | 128 | /* Wrap the drive motor set speed function */ 129 | void setMotorSpeed(int i, int spd) { 130 | if (i == LEFT) drive.setM1Speed(spd); 131 | else drive.setM2Speed(spd); 132 | } 133 | 134 | // A convenience function for setting both motor speeds 135 | void setMotorSpeeds(int leftSpeed, int rightSpeed) { 136 | setMotorSpeed(LEFT, leftSpeed); 137 | setMotorSpeed(RIGHT, rightSpeed); 138 | } 139 | #endif 140 | 141 | /* Variable initialization */ 142 | 143 | // A pair of varibles to help parse serial commands (thanks Fergs) 144 | int arg = 0; 145 | int index = 0; 146 | 147 | // Variable to hold an input character 148 | char chr; 149 | 150 | // Variable to hold the current single-character command 151 | char cmd; 152 | 153 | // Character arrays to hold the first and second arguments 154 | char argv1[16]; 155 | char argv2[16]; 156 | 157 | // The arguments converted to integers 158 | long arg1; 159 | long arg2; 160 | 161 | /* Clear the current command parameters */ 162 | void resetCommand() { 163 | cmd = NULL; 164 | memset(argv1, 0, sizeof(argv1)); 165 | memset(argv2, 0, sizeof(argv2)); 166 | arg1 = 0; 167 | arg2 = 0; 168 | arg = 0; 169 | index = 0; 170 | } 171 | 172 | /* Run a command. Commands are defined in commands.h */ 173 | int runCommand() { 174 | int i = 0; 175 | char *p = argv1; 176 | char *str; 177 | int pid_args[4]; 178 | arg1 = atoi(argv1); 179 | arg2 = atoi(argv2); 180 | 181 | switch(cmd) { 182 | case GET_BAUDRATE: 183 | Serial.println(BAUDRATE); 184 | break; 185 | case ANALOG_READ: 186 | Serial.println(analogRead(arg1)); 187 | break; 188 | case DIGITAL_READ: 189 | Serial.println(digitalRead(arg1)); 190 | break; 191 | case ANALOG_WRITE: 192 | analogWrite(arg1, arg2); 193 | Serial.println("OK"); 194 | break; 195 | case DIGITAL_WRITE: 196 | if (arg2 == 0) digitalWrite(arg1, LOW); 197 | else if (arg2 == 1) digitalWrite(arg1, HIGH); 198 | Serial.println("OK"); 199 | break; 200 | case PIN_MODE: 201 | if (arg2 == 0) pinMode(arg1, INPUT); 202 | else if (arg2 == 1) pinMode(arg1, OUTPUT); 203 | Serial.println("OK"); 204 | break; 205 | case PING: 206 | Serial.println(Ping(arg1)); 207 | break; 208 | #ifdef USE_SERVOS 209 | case SERVO_WRITE: 210 | servos[arg1].write(arg2); 211 | Serial.println("OK"); 212 | break; 213 | case SERVO_READ: 214 | Serial.println(servos[arg1].read()); 215 | break; 216 | #endif 217 | 218 | #ifdef USE_BASE 219 | case READ_ENCODERS: 220 | Serial.print(readEncoder(LEFT)); 221 | Serial.print(" "); 222 | Serial.println(readEncoder(RIGHT)); 223 | break; 224 | case RESET_ENCODERS: 225 | resetEncoders(); 226 | Serial.println("OK"); 227 | break; 228 | case MOTOR_SPEEDS: 229 | /* Reset the auto stop timer */ 230 | lastMotorCommand = millis(); 231 | if (arg1 == 0 && arg2 == 0) { 232 | setMotorSpeeds(0, 0); 233 | moving = 0; 234 | } 235 | else moving = 1; 236 | leftPID.TargetTicksPerFrame = arg1; 237 | rightPID.TargetTicksPerFrame = arg2; 238 | Serial.println("OK"); 239 | break; 240 | case UPDATE_PID: 241 | while ((str = strtok_r(p, ":", &p)) != '\0') { 242 | pid_args[i] = atoi(str); 243 | i++; 244 | } 245 | Kp = pid_args[0]; 246 | Kd = pid_args[1]; 247 | Ki = pid_args[2]; 248 | Ko = pid_args[3]; 249 | Serial.println("OK"); 250 | break; 251 | #endif 252 | default: 253 | Serial.println("Invalid Command"); 254 | break; 255 | } 256 | } 257 | 258 | /* Setup function--runs once at startup. */ 259 | void setup() { 260 | Serial.begin(BAUDRATE); 261 | 262 | // Initialize the motor controller if used */ 263 | #ifdef USE_BASE 264 | initMotorController(); 265 | #endif 266 | 267 | /* Attach servos if used */ 268 | #ifdef USE_SERVOS 269 | int i; 270 | for (i = 0; i < N_SERVOS; i++) { 271 | servos[i].attach(servoPins[i]); 272 | } 273 | #endif 274 | } 275 | 276 | /* Enter the main loop. Read and parse input from the serial port 277 | and run any valid commands. Run a PID calculation at the target 278 | interval and check for auto-stop conditions. 279 | */ 280 | void loop() { 281 | while (Serial.available() > 0) { 282 | 283 | // Read the next character 284 | chr = Serial.read(); 285 | 286 | // Terminate a command with a CR 287 | if (chr == 13) { 288 | if (arg == 1) argv1[index] = NULL; 289 | else if (arg == 2) argv2[index] = NULL; 290 | runCommand(); 291 | resetCommand(); 292 | } 293 | // Use spaces to delimit parts of the command 294 | else if (chr == ' ') { 295 | // Step through the arguments 296 | if (arg == 0) arg = 1; 297 | else if (arg == 1) { 298 | argv1[index] = NULL; 299 | arg = 2; 300 | index = 0; 301 | } 302 | continue; 303 | } 304 | else { 305 | if (arg == 0) { 306 | // The first arg is the single-letter command 307 | cmd = chr; 308 | } 309 | else if (arg == 1) { 310 | // Subsequent arguments can be more than one character 311 | argv1[index] = chr; 312 | index++; 313 | } 314 | else if (arg == 2) { 315 | argv2[index] = chr; 316 | index++; 317 | } 318 | } 319 | } 320 | 321 | // If we are using base control, run a PID calculation at the appropriate intervals 322 | #ifdef USE_BASE 323 | if (millis() > nextPID) { 324 | updatePID(); 325 | nextPID += PID_INTERVAL; 326 | } 327 | 328 | // Check to see if we have exceeded the auto-stop interval 329 | if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {; 330 | setMotorSpeeds(0, 0); 331 | moving = 0; 332 | } 333 | 334 | #endif 335 | } 336 | 337 | 338 | 339 | 340 | 341 | 342 | -------------------------------------------------------------------------------- /ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * ROSArduinoBridge 3 | 4 | A set of simple serial commands to control a differential drive 5 | robot and receive back sensor and odometry data. Default 6 | configuration assumes use of an Arduino Mega + Pololu motor 7 | controller shield + Robogaia Mega Encoder shield. Edit the 8 | readEncoder() and setMotorSpeed() wrapper functions if using 9 | different motor controller or encoder method. 10 | 11 | Created for the Pi Robot Project: http://www.pirobot.org 12 | and the Home Brew Robotics Club (HBRC): http://hbrobotics.org 13 | 14 | Authors: Patrick Goebel, James Nugen 15 | 16 | Inspired and modeled after the ArbotiX driver by Michael Ferguson 17 | 18 | Software License Agreement (BSD License) 19 | 20 | Copyright (c) 2012, Patrick Goebel. 21 | All rights reserved. 22 | 23 | Redistribution and use in source and binary forms, with or without 24 | modification, are permitted provided that the following conditions 25 | are met: 26 | 27 | * Redistributions of source code must retain the above copyright 28 | notice, this list of conditions and the following disclaimer. 29 | * Redistributions in binary form must reproduce the above 30 | copyright notice, this list of conditions and the following 31 | disclaimer in the documentation and/or other materials provided 32 | with the distribution. 33 | 34 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 35 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 36 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 37 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 38 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 39 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 40 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 41 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 42 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 43 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 44 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 45 | * POSSIBILITY OF SUCH DAMAGE. 46 | *********************************************************************/ 47 | 48 | #define USE_BASE // Enable the base controller code 49 | //#undef USE_BASE // Disable the base controller code 50 | 51 | /* Define the motor controller and encoder library you are using */ 52 | #ifdef USE_BASE 53 | /* The Pololu VNH5019 dual motor driver shield */ 54 | //#define POLOLU_VNH5019 55 | 56 | /* The Pololu MC33926 dual motor driver shield */ 57 | //#define POLOLU_MC33926 58 | 59 | /* The RoboGaia encoder shield */ 60 | //#define ROBOGAIA 61 | 62 | /* L298N Dual H-Bridge Motor Controller */ 63 | #define L298N_DUAL_HBRIDGE 64 | 65 | /* Encoders directly attached to Arduino board */ 66 | //#define ARDUINO_ENC_COUNTER 67 | /* My customized encoders */ 68 | #define ARDUINO_MY_COUNTER 69 | #endif 70 | 71 | #define USE_SERVOS // Enable use of PWM servos as defined in servos.h 72 | //#undef USE_SERVOS // Disable use of PWM servos 73 | 74 | /* Serial port baud rate */ 75 | #define BAUDRATE 57600 76 | 77 | /* Maximum PWM signal */ 78 | #define MAX_PWM 255 79 | 80 | #if defined(ARDUINO) && ARDUINO >= 100 81 | #include "Arduino.h" 82 | #else 83 | #include "WProgram.h" 84 | #endif 85 | 86 | /* Include definition of serial commands */ 87 | #include "commands.h" 88 | 89 | /* Sensor functions */ 90 | #include "sensors.h" 91 | 92 | /* Include servo support if required */ 93 | #ifdef USE_SERVOS 94 | #include 95 | #include "servos.h" 96 | #endif 97 | 98 | #ifdef USE_BASE 99 | /* Motor driver function definitions */ 100 | #include "motor_driver.h" 101 | 102 | /* Encoder driver function definitions */ 103 | #include "encoder_driver.h" 104 | 105 | /* PID parameters and functions */ 106 | #include "diff_controller.h" 107 | 108 | /* Run the PID loop at 30 times per second */ 109 | #define PID_RATE 30 // Hz 110 | 111 | /* Convert the rate into an interval */ 112 | const int PID_INTERVAL = 1000 / PID_RATE; 113 | 114 | /* Track the next time we make a PID calculation */ 115 | unsigned long nextPID = PID_INTERVAL; 116 | 117 | /* Stop the robot if it hasn't received a movement command 118 | in this number of milliseconds */ 119 | #define AUTO_STOP_INTERVAL 2000 120 | long lastMotorCommand = AUTO_STOP_INTERVAL; 121 | #endif 122 | 123 | /* Variable initialization */ 124 | 125 | // A pair of varibles to help parse serial commands (thanks Fergs) 126 | int arg = 0; 127 | int index = 0; 128 | 129 | // Variable to hold an input character 130 | char chr; 131 | 132 | // Variable to hold the current single-character command 133 | char cmd; 134 | 135 | // Character arrays to hold the first and second arguments 136 | char argv1[16]; 137 | char argv2[16]; 138 | 139 | // The arguments converted to integers 140 | long arg1; 141 | long arg2; 142 | 143 | /* Clear the current command parameters */ 144 | void resetCommand() { 145 | cmd = NULL; 146 | memset(argv1, 0, sizeof(argv1)); 147 | memset(argv2, 0, sizeof(argv2)); 148 | arg1 = 0; 149 | arg2 = 0; 150 | arg = 0; 151 | index = 0; 152 | } 153 | 154 | /* Run a command. Commands are defined in commands.h */ 155 | int runCommand() { 156 | int i = 0; 157 | char *p = argv1; 158 | char *str; 159 | int pid_args[4]; 160 | arg1 = atoi(argv1); 161 | arg2 = atoi(argv2); 162 | 163 | switch(cmd) { 164 | case GET_BAUDRATE: 165 | Serial.println(BAUDRATE); 166 | break; 167 | case ANALOG_READ: 168 | Serial.println(analogRead(arg1)); 169 | break; 170 | case DIGITAL_READ: 171 | Serial.println(digitalRead(arg1)); 172 | break; 173 | case ANALOG_WRITE: 174 | analogWrite(arg1, arg2); 175 | Serial.println("OK"); 176 | break; 177 | case DIGITAL_WRITE: 178 | if (arg2 == 0) digitalWrite(arg1, LOW); 179 | else if (arg2 == 1) digitalWrite(arg1, HIGH); 180 | Serial.println("OK"); 181 | break; 182 | case PIN_MODE: 183 | if (arg2 == 0) pinMode(arg1, INPUT); 184 | else if (arg2 == 1) pinMode(arg1, OUTPUT); 185 | Serial.println("OK"); 186 | break; 187 | case PING: 188 | Serial.println(Ping(arg1)); 189 | break; 190 | #ifdef USE_SERVOS 191 | case SERVO_WRITE: 192 | servos[arg1].setTargetPosition(arg2); 193 | Serial.println("OK"); 194 | break; 195 | case SERVO_READ: 196 | Serial.println(servos[arg1].getServo().read()); 197 | break; 198 | #endif 199 | 200 | #ifdef USE_BASE 201 | case READ_ENCODERS: 202 | Serial.print(readEncoder(LEFT)); 203 | Serial.print(" "); 204 | Serial.println(readEncoder(RIGHT)); 205 | break; 206 | case RESET_ENCODERS: 207 | resetEncoders(); 208 | resetPID(); 209 | Serial.println("OK"); 210 | break; 211 | case MOTOR_SPEEDS: 212 | /* Reset the auto stop timer */ 213 | lastMotorCommand = millis(); 214 | if (arg1 == 0 && arg2 == 0) { 215 | setMotorSpeeds(0, 0); 216 | resetPID(); 217 | moving = 0; 218 | } 219 | else moving = 1; 220 | leftPID.TargetTicksPerFrame = arg1; 221 | rightPID.TargetTicksPerFrame = arg2; 222 | Serial.println("OK"); 223 | break; 224 | case UPDATE_PID: 225 | while ((str = strtok_r(p, ":", &p)) != '\0') { 226 | pid_args[i] = atoi(str); 227 | i++; 228 | } 229 | Kp = pid_args[0]; 230 | Kd = pid_args[1]; 231 | Ki = pid_args[2]; 232 | Ko = pid_args[3]; 233 | Serial.println("OK"); 234 | break; 235 | #endif 236 | default: 237 | Serial.println("Invalid Command"); 238 | break; 239 | } 240 | } 241 | 242 | /* Setup function--runs once at startup. */ 243 | void setup() { 244 | Serial.begin(BAUDRATE); 245 | 246 | // Initialize the motor controller if used */ 247 | #ifdef USE_BASE 248 | #ifdef ARDUINO_ENC_COUNTER 249 | //set as inputs 250 | DDRD &= ~(1< 0) { 292 | 293 | // Read the next character 294 | chr = Serial.read(); 295 | 296 | // Terminate a command with a CR 297 | if (chr == 13) { 298 | if (arg == 1) argv1[index] = NULL; 299 | else if (arg == 2) argv2[index] = NULL; 300 | runCommand(); 301 | resetCommand(); 302 | } 303 | // Use spaces to delimit parts of the command 304 | else if (chr == ' ') { 305 | // Step through the arguments 306 | if (arg == 0) arg = 1; 307 | else if (arg == 1) { 308 | argv1[index] = NULL; 309 | arg = 2; 310 | index = 0; 311 | } 312 | continue; 313 | } 314 | else { 315 | if (arg == 0) { 316 | // The first arg is the single-letter command 317 | cmd = chr; 318 | } 319 | else if (arg == 1) { 320 | // Subsequent arguments can be more than one character 321 | argv1[index] = chr; 322 | index++; 323 | } 324 | else if (arg == 2) { 325 | argv2[index] = chr; 326 | index++; 327 | } 328 | } 329 | } 330 | 331 | // If we are using base control, run a PID calculation at the appropriate intervals 332 | #ifdef USE_BASE 333 | if (millis() > nextPID) { 334 | updatePID(); 335 | nextPID += PID_INTERVAL; 336 | } 337 | 338 | // Check to see if we have exceeded the auto-stop interval 339 | if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) { 340 | setMotorSpeeds(0, 0); 341 | moving = 0; 342 | } 343 | #endif 344 | 345 | // Sweep servos 346 | #ifdef USE_SERVOS 347 | int i; 348 | for (i = 0; i < N_SERVOS; i++) { 349 | servos[i].doSweep(); 350 | } 351 | #endif 352 | } 353 | -------------------------------------------------------------------------------- /ros_arduino_python/src/ros_arduino_python/arduino_driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | A Python driver for the Arduino microcontroller running the 5 | ROSArduinoBridge firmware. 6 | 7 | Created for the Pi Robot Project: http://www.pirobot.org 8 | Copyright (c) 2012 Patrick Goebel. All rights reserved. 9 | 10 | This program is free software; you can redistribute it and/or modify 11 | it under the terms of the GNU General Public License as published by 12 | the Free Software Foundation; either version 2 of the License, or 13 | (at your option) any later version. 14 | 15 | This program is distributed in the hope that it will be useful, 16 | but WITHOUT ANY WARRANTY; without even the implied warranty of 17 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 18 | GNU General Public License for more details at: 19 | 20 | http://www.gnu.org/licenses/gpl.html 21 | 22 | """ 23 | 24 | import thread 25 | from math import pi as PI, degrees, radians 26 | import os, time, sys, traceback 27 | from serial.serialutil import SerialException, SerialTimeoutException 28 | from serial import Serial 29 | import rospy 30 | 31 | SERVO_MAX = 180 32 | SERVO_MIN = 0 33 | 34 | class Arduino: 35 | ''' Configuration Parameters 36 | ''' 37 | N_ANALOG_PORTS = 6 38 | N_DIGITAL_PORTS = 12 39 | 40 | def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5): 41 | 42 | self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller. 43 | self.PID_INTERVAL = 1000 / 30 44 | 45 | self.port = port 46 | self.baudrate = baudrate 47 | self.timeout = timeout 48 | self.encoder_count = 0 49 | self.writeTimeout = timeout 50 | self.interCharTimeout = timeout / 30. 51 | self.motors_reversed = rospy.get_param("~motors_reversed", False) 52 | # Keep things thread safe 53 | self.mutex = thread.allocate_lock() 54 | 55 | # An array to cache analog sensor readings 56 | self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS 57 | 58 | # An array to cache digital sensor readings 59 | self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS 60 | 61 | def connect(self): 62 | try: 63 | print "Connecting to Arduino on port", self.port, "..." 64 | self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout) 65 | # The next line is necessary to give the firmware time to wake up. 66 | time.sleep(1) 67 | test = self.get_baud() 68 | if test != self.baudrate: 69 | time.sleep(1) 70 | test = self.get_baud() 71 | if test != self.baudrate: 72 | raise SerialException 73 | print "Connected at", self.baudrate 74 | print "Arduino is ready." 75 | 76 | except SerialException: 77 | print "Serial Exception:" 78 | print sys.exc_info() 79 | print "Traceback follows:" 80 | traceback.print_exc(file=sys.stdout) 81 | print "Cannot connect to Arduino!" 82 | os._exit(1) 83 | 84 | def open(self): 85 | ''' Open the serial port. 86 | ''' 87 | self.port.open() 88 | 89 | def close(self): 90 | ''' Close the serial port. 91 | ''' 92 | self.port.close() 93 | 94 | def send(self, cmd): 95 | ''' This command should not be used on its own: it is called by the execute commands 96 | below in a thread safe manner. 97 | ''' 98 | self.port.write(cmd + '\r') 99 | 100 | def recv(self, timeout=0.5): 101 | timeout = min(timeout, self.timeout) 102 | ''' This command should not be used on its own: it is called by the execute commands 103 | below in a thread safe manner. Note: we use read() instead of readline() since 104 | readline() tends to return garbage characters from the Arduino 105 | ''' 106 | c = '' 107 | value = '' 108 | attempts = 0 109 | while c != '\r': 110 | c = self.port.read(1) 111 | value += c 112 | attempts += 1 113 | if attempts * self.interCharTimeout > timeout: 114 | return None 115 | 116 | value = value.strip('\r') 117 | 118 | return value 119 | 120 | def recv_ack(self): 121 | ''' This command should not be used on its own: it is called by the execute commands 122 | below in a thread safe manner. 123 | ''' 124 | ack = self.recv(self.timeout) 125 | return ack == 'OK' 126 | 127 | def recv_int(self): 128 | ''' This command should not be used on its own: it is called by the execute commands 129 | below in a thread safe manner. 130 | ''' 131 | value = self.recv(self.timeout) 132 | try: 133 | return int(value) 134 | except: 135 | return None 136 | 137 | def recv_array(self): 138 | ''' This command should not be used on its own: it is called by the execute commands 139 | below in a thread safe manner. 140 | ''' 141 | try: 142 | values = self.recv(self.timeout * self.N_ANALOG_PORTS).split() 143 | return map(int, values) 144 | except: 145 | return [] 146 | 147 | def execute(self, cmd): 148 | ''' Thread safe execution of "cmd" on the Arduino returning a single integer value. 149 | ''' 150 | self.mutex.acquire() 151 | 152 | try: 153 | self.port.flushInput() 154 | except: 155 | pass 156 | 157 | ntries = 1 158 | attempts = 0 159 | 160 | try: 161 | self.port.write(cmd + '\r') 162 | value = self.recv(self.timeout) 163 | while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None): 164 | try: 165 | self.port.flushInput() 166 | self.port.write(cmd + '\r') 167 | value = self.recv(self.timeout) 168 | except: 169 | print "Exception executing command: " + cmd 170 | attempts += 1 171 | except: 172 | self.mutex.release() 173 | print "Exception executing command: " + cmd 174 | value = None 175 | 176 | self.mutex.release() 177 | return int(value) 178 | 179 | def execute_array(self, cmd): 180 | ''' Thread safe execution of "cmd" on the Arduino returning an array. 181 | ''' 182 | self.mutex.acquire() 183 | 184 | try: 185 | self.port.flushInput() 186 | except: 187 | pass 188 | 189 | ntries = 1 190 | attempts = 0 191 | 192 | try: 193 | self.port.write(cmd + '\r') 194 | values = self.recv_array() 195 | while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None): 196 | try: 197 | self.port.flushInput() 198 | self.port.write(cmd + '\r') 199 | values = self.recv_array() 200 | except: 201 | print("Exception executing command: " + cmd) 202 | attempts += 1 203 | except: 204 | self.mutex.release() 205 | print "Exception executing command: " + cmd 206 | raise SerialException 207 | return [] 208 | 209 | try: 210 | values = map(int, values) 211 | except: 212 | values = [] 213 | 214 | self.mutex.release() 215 | return values 216 | 217 | def execute_ack(self, cmd): 218 | ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK. 219 | ''' 220 | self.mutex.acquire() 221 | 222 | try: 223 | self.port.flushInput() 224 | except: 225 | pass 226 | 227 | ntries = 1 228 | attempts = 0 229 | 230 | try: 231 | self.port.write(cmd + '\r') 232 | ack = self.recv(self.timeout) 233 | while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None): 234 | try: 235 | self.port.flushInput() 236 | self.port.write(cmd + '\r') 237 | ack = self.recv(self.timeout) 238 | except: 239 | print "Exception executing command: " + cmd 240 | attempts += 1 241 | except: 242 | self.mutex.release() 243 | print "execute_ack exception when executing", cmd 244 | print sys.exc_info() 245 | return 0 246 | 247 | self.mutex.release() 248 | return ack == 'OK' 249 | 250 | def update_pid(self, Kp, Kd, Ki, Ko): 251 | ''' Set the PID parameters on the Arduino 252 | ''' 253 | print "Updating PID parameters" 254 | cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko) 255 | self.execute_ack(cmd) 256 | 257 | def get_baud(self): 258 | ''' Get the current baud rate on the serial port. 259 | ''' 260 | try: 261 | return int(self.execute('b')); 262 | except: 263 | return None 264 | 265 | def get_encoder_counts(self): 266 | values = self.execute_array('e') 267 | if len(values) != 2: 268 | print "Encoder count was not 2" 269 | raise SerialException 270 | return None 271 | else: 272 | if self.motors_reversed: 273 | values[0], values[1] = -values[0], -values[1] 274 | return values 275 | 276 | def reset_encoders(self): 277 | ''' Reset the encoder counts to 0 278 | ''' 279 | return self.execute_ack('r') 280 | 281 | def drive(self, right, left): 282 | ''' Speeds are given in encoder ticks per PID interval 283 | ''' 284 | if self.motors_reversed: 285 | left, right = -left, -right 286 | return self.execute_ack('m %d %d' %(right, left)) 287 | 288 | def drive_m_per_s(self, right, left): 289 | ''' Set the motor speeds in meters per second. 290 | ''' 291 | left_revs_per_second = float(left) / (self.wheel_diameter * PI) 292 | right_revs_per_second = float(right) / (self.wheel_diameter * PI) 293 | 294 | left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) 295 | right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction) 296 | 297 | self.drive(right_ticks_per_loop , left_ticks_per_loop ) 298 | 299 | def stop(self): 300 | ''' Stop both motors. 301 | ''' 302 | self.drive(0, 0) 303 | 304 | def analog_read(self, pin): 305 | return self.execute('a %d' %pin) 306 | 307 | def analog_write(self, pin, value): 308 | return self.execute_ack('x %d %d' %(pin, value)) 309 | 310 | def digital_read(self, pin): 311 | return self.execute('d %d' %pin) 312 | 313 | def digital_write(self, pin, value): 314 | return self.execute_ack('w %d %d' %(pin, value)) 315 | 316 | def pin_mode(self, pin, mode): 317 | return self.execute_ack('c %d %d' %(pin, mode)) 318 | 319 | def servo_write(self, id, pos): 320 | ''' Usage: servo_write(id, pos) 321 | Position is given in radians and converted to degrees before sending 322 | ''' 323 | return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos))))) 324 | 325 | def servo_read(self, id): 326 | ''' Usage: servo_read(id) 327 | The returned position is converted from degrees to radians 328 | ''' 329 | return radians(self.execute('t %d' %id)) 330 | 331 | def ping(self, pin): 332 | ''' The srf05/Ping command queries an SRF05/Ping sonar sensor 333 | connected to the General Purpose I/O line pinId for a distance, 334 | and returns the range in cm. Sonar distance resolution is integer based. 335 | ''' 336 | return self.execute('p %d' %pin); 337 | 338 | # def get_maxez1(self, triggerPin, outputPin): 339 | # ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar 340 | # sensor connected to the General Purpose I/O lines, triggerPin, and 341 | # outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE 342 | # SURE there's nothing directly in front of the MaxSonar-EZ1 upon 343 | # power up, otherwise it wont range correctly for object less than 6 344 | # inches away! The sensor reading defaults to use English units 345 | # (inches). The sensor distance resolution is integer based. Also, the 346 | # maxsonar trigger pin is RX, and the echo pin is PW. 347 | # ''' 348 | # return self.execute('z %d %d' %(triggerPin, outputPin)) 349 | 350 | """ Basic test for connectivity """ 351 | if __name__ == "__main__": 352 | if os.name == "posix": 353 | portName = "/dev/ttyACM0" 354 | else: 355 | portName = "COM43" # Windows style COM port. 356 | 357 | baudRate = 57600 358 | 359 | myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5) 360 | myArduino.connect() 361 | 362 | print "Sleeping for 1 second..." 363 | time.sleep(1) 364 | 365 | print "Reading on analog port 0", myArduino.analog_read(0) 366 | print "Reading on digital port 0", myArduino.digital_read(0) 367 | print "Blinking the LED 3 times" 368 | for i in range(3): 369 | myArduino.digital_write(13, 1) 370 | time.sleep(1.0) 371 | #print "Current encoder counts", myArduino.encoders() 372 | 373 | print "Connection test successful.", 374 | 375 | myArduino.stop() 376 | myArduino.close() 377 | 378 | print "Shutting down Arduino." 379 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 写在前面 2 | ----- 3 | 本文是根据ros\_arduino\_bridge功能包的官方文档翻译修改而来，目的是为了便于广大网友更方便的学习使用该功能包。本文主要针对indigo-devel版本进行介绍，也会捎带介绍一下其他的版本。文中存在我的一些主观看法，如您有其他观点可以与我交流，我的email：。这篇博文篇幅有些冗长，希望各位网友耐心阅读。 4 | 5 | # 概述 6 | ------ 7 | 使用这个功能包时，注意选择相应的版本，因为不同的版本之间有些并不能兼容。indigo-devel branch是针对ROS Indigo以及更高版本的，它使用的是Catkin编译系统，它对于ROS Hydro也是兼容的；而master 分支目前还在测试中，可能在某些板子或电脑上无法正常运行，我曾今使用过这个分支，但是出了很多问题，如串口（serial）就无法与主机正常交流、波特率的大小设置等问题。本文主要针对indigo-devel版本进行介绍，也会捎带介绍一下其他的版本。 8 | 9 | 这个ROS功能包集包括了Arduino库（ROSArduinoBridge）和一系列用来控制基于Arduino的ROS功能包，它使用的是标准的ROS消息和服务。这个功能包集并不依赖于ROS串口。 10 | 这个功能包集的功能包括： 11 | 12 | - 可以直接支持ping声呐和Sharp红外线传感器 13 | - 也可以从通用的模拟和数字信号的传感器读取数据 14 | - 可以控制数字信号的输出 15 | - 支持PWM伺服机 16 | - 如果使用所要求的硬件的话，可以配置基本的控制 17 | - 如果你的Arduino编程基础好的话，并且具有python基础的话，你就可以很自由的改动代码来满足你的硬件要求 18 | 19 | 这个功能包集包括一个兼容不同驱动的机器人的基本控制器（`base controller`），它可以接收ROS Twist类型的消息，可以发布里程数据到个人电脑。这个控制器（`base controller`）要求使用一个电机控制器和编码器来读取里程数据。目前indigo-devel版本的功能包集支持下面的控制器硬件： 20 | 21 | - [Pololu VNH5019 dual motor controller shield](http://www.pololu.com/catalog/product/2502) or [Pololu MC33926 dual motor shield](http://www.pololu.com/catalog/product/2503). 22 | - [Robogaia Mega Encoder shield](http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters. 23 | 24 | **NOTE:** Robogaia Mega Encoder shield 仅适用于Arduino Mega，而板上编码计数器(ARDUINO_ENC_COUNTER)目前仅支持Arduino Uno，这些是官方文档上的说法。但是实际上只要你有一定的编程基础，你就可以随心所欲，想让它支持什么硬件就支持什么硬件。 25 | 26 | # 关于ROS官方文档 27 | ----------- 28 | 在这个ROS wiki [ros\_arduino\_bridge](http://www.ros.org/wiki/ros_arduino_bridge)里可以找到一个标准的ROS风格的文档，但是貌似这个wiki里面的信息并不全面，应该说很不全面，不会有什么实质性的帮助。也许是因为在它的github仓库里有详细的说明，所以它才偷懒了吧！所以如果你想要了解关于该功能包的详细内容，请直接查看它的github仓库里的[README文件](https://github.com/hbrobotics/ros_arduino_bridge)。当然里面全是E文，所以如果你觉得有困难的话，可以委屈一下继续阅读我的拙文，当然这也是我写这篇博文的一个原因，我十分乐意为广大网友翻译英文资料，如有翻译不当的地方，欢迎各位批评指正，我的e-mail是。 29 | 30 | # 系统要求 31 | --------- 32 | Python 串口：使用下面的命令可以在Ubuntu下安装python-serial功能包 33 | ```bash 34 | $ sudo apt-get install python-serial 35 | ``` 36 | 在非Ubuntu系统下，可以使用： 37 | ```bash 38 | $ sudo pip install --upgrade pyserial 39 | ``` 40 | 或者 41 | ```bash 42 | $ sudo easy_install -U pyserial 43 | ``` 44 | 45 | 这个功能包集可以在兼容Arduino的控制器上进行读取传感器上的数据，以及控制[PWM](https://en.wikipedia.org/wiki/Pulse-width_modulation)伺服机。但是你必须具备上面所说的被支持的硬件（电动机控制器和编码器），你才能使用这个功能包集中的基本控制器（`base controller`）。 46 | 47 | **NOTE**:如果你没有这些硬件设备，你仍然有办法使用这个功能包，当然这需要一些相应的编程基础，具体方法请查看本文档结尾的NOTES部分。 48 | 49 | 要使用这个基本控制器（`base controller`），你还得为你的电动机控制器和编码器安装合适的库。对于上面提到过的Pololu VNH5019 Dual Motor Shield，它的相应的库可以在这里找到： 50 | 51 | 52 | 53 | 对于Pololu MC33926 Dual Motor Shield，则在这里可以找到对应的库： 54 | 55 | 56 | 57 | Robogaia Mega Encoder shield的库可以在这里找到； 58 | 59 | 60 | 61 | 这些库应该被安装到你的标准Arduino sketchbook/libraries路径下面。最后，这个功能包集假设你使用的Arduino IDE的版本是1.0或以上，我使用的是1.6.8。 62 | 63 | # 在你的Linux下准备串口 64 | ------------ 65 | 你的Arduino很可能是通过接口`/dev/ttyACM#` 或者 `/dev/ttyUSB#`来连接你的Linux系统的。这里的`#`可以是0,1,2等数字，当然这根据你连接的设备数量而定。得到这个数字`#`最简单的方式就是拔掉所有的USB设备，然后插上你的Arduino，然后运行下面这个命令： 66 | 67 | ```bash 68 | $ ls /dev/ttyACM* 69 | ``` 70 | 71 | 或者 72 | 73 | ```bash 74 | $ ls /dev/ttyUSB* 75 | ``` 76 | 77 | 但愿上面这两个命令的有一个可以返回你想要的结果（例如/dev/ttyACM0），而另一个可能会返回这个错误"No such file or directory"。反正只要有一个命令可以得到结果就行。 78 | 79 | 然后你需要确保你对这个接口有访问的权限。假设你的Arduino连接的是`/dev/ttyACM0`，那么就运行下面这个命令： 80 | 81 | ```bash 82 | $ ls -l /dev/ttyACM0 83 | ``` 84 | 85 | 然后你就可以看到类似于下面的输出结果： 86 | 87 | > crw-rw---- 1 root dialout 166, 0 2013-02-24 08:31 /dev/ttyACM0 88 | 89 | 我们注意到在上面的结果中，只有root和"dialout"组才有读写权限。因此，你需要成为`dialout`（关于dialout你可以参考[这里](http://bbs.csdn.net/topics/391944286)）组的一个成员。你仅仅需要马上加入这组中，并且它会对你之后插入的所有的USB设备都起作用。 90 | 91 | 你可以运行这个命令来加入dialout组： 92 | 93 | ```bash 94 | $ sudo usermod -a -G dialout your_user_name 95 | ``` 96 | 97 | 在这个命令中`your_user_name`就是你在Linux下登录的用户名。之后你可能需要注销登录，然后再重新登录进去，或者就简单的重启一下电脑。 98 | 99 | 当你执行完上面的操作之后，你可以运行下面的命令查看一下： 100 | 101 | ```bash 102 | $ groups 103 | ``` 104 | 105 | 然后如果你可以在列出的组中找到dialout，这就说明你已经加入到dialout中了。 106 | 107 | 108 | # 安装ros\_arduino\_bridge功能包集 109 | ----------- 110 | 111 | ```bash 112 | $ cd ~/catkin_workspace/src 113 | $ git clone https://github.com/hbrobotics/ros_arduino_bridge.git 114 | $ cd ~/catkin_workspace 115 | $ catkin_make 116 | ``` 117 | 118 | 这个被提供的Arduino库叫做ROSArduinoBridge（前面已经提到过），你可以在 ros\_arduino\_firmware功能包中找到。这个sketch（这是arduino对使用arduino IDE编辑出来的程序的专门叫法）是针对上面所要求的硬件，但是也可以被用于Arduino其他版本的板子（例如Uno），你可以不使用基础控制器（`base controller`），你可以在文档末尾的NOTES部分找到详细的说明。 119 | 120 | 你可以按照下面的步骤安装ROSArduinoBridge库： 121 | 122 | ```bash 123 | $ cd SKETCHBOOK_PATH 124 | ``` 125 | 这里`SKETCHBOOK_PATH`是指你的Arduino sketchbook路径。 126 | 127 | ```bash 128 | $ cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge ROSArduinoBridge 129 | ``` 130 | 131 | 最后一个命令就是把ROSArduinoBridge sketch文件拷贝到你的sketchbook文件夹下面。如果你之前已经拷贝过这个sketch，那么下次拷贝的时候请先删除之前的sketch： 132 | 133 | cd SKETCHBOOK_PATH 134 | rm -R ROSArduinoBridge 135 | 136 | 下一节描述的是如何配置，编译并上传这个sketch。 137 | 138 | # 加载ROSArduinoBridge的Sketch 139 | ----------- 140 | 141 | - 如果你正在使用基础控制器功能包（`base controller`），那么你必须确保你已经在Arduino sketchbook/libraries文件夹里面安装了合适的电动机控制器和编码器的相关库。 142 | - 启动ArduinoIDE(1.0及以上)，然后载入ROSArduinoBridgesketch。你可以这样找到他们File->Sketchbook->ROSArduinoBridge 143 | 144 | **NOTE:**如果你还没有安装要求的基础控制硬件，但是你仍然想使用这些代码，那么你可以参考文档末尾的NOTES部分。 145 | 146 | 你可以通过去掉或保留相关的宏定义声明，来选择你想用的电动机控制器，同时你还要将其他电动机控制器的宏声明注释掉。默认选择的是 Pololu VNH5019 driver。 147 | 148 | 同样，你也可以用相同的方法选择你想使用的编码器。默认选择的是Pololu VNH5019 driver。 149 | 150 | 如果你想让你的基础控制器控制PWM伺服机的话，就把下面这两行： 151 | 152 | ```c 153 | //#define USE_SERVOS 154 | #undef USE_SERVOS 155 | ``` 156 | 改成： 157 | ```c 158 | #define USE_SERVOS 159 | //#undef USE_SERVOS 160 | ``` 161 | 之后你就必须修改头文件`servos.h`改变其中的N_SERVOS参数，另外你还需要根据你的伺服机所接的引脚修改相应的引脚数字。 162 | 163 | 一切都准备好后你就可以把这个sketch编译并上传到你的Arduino板子上上了。 164 | 165 | # 固件程序命令 166 | ---------- 167 | 这个ROSArduino库接受单字母命令来轮询传感器、控制伺服机、驱动机器人以及读取编码器。这些命令可以通过串口接口来发送给Arduino，比如Arduino的串口监视器。 168 | 169 | **NOTE:**在尝试这些命令之前，把串口监视器的波特率设置为57600然后把行结束符设置为“Carriage return（回车）”和“Both NL & CR”（NL和CR）。这些设置选项在串口监视器右下角的两个下拉菜单中。如下图所示： 170 | 171 | ![serial_monitor](http://img.blog.csdn.net/20160508122310948) 172 | 173 | 命令列表可以在头文件commands.h中找到。目前的列表包括这些命令： 174 | 175 | ```c 176 | #define ANALOG_READ 'a' 177 | #define GET_BAUDRATE 'b' 178 | #define PIN_MODE 'c' 179 | #define DIGITAL_READ 'd' 180 | #define READ_ENCODERS 'e' 181 | #define MOTOR_SPEEDS 'm' 182 | #define PING 'p' 183 | #define RESET_ENCODERS 'r' 184 | #define SERVO_WRITE 's' 185 | #define SERVO_READ 't' 186 | #define UPDATE_PID 'u' 187 | #define DIGITAL_WRITE 'w' 188 | #define ANALOG_WRITE 'x' 189 | ``` 190 | 191 | 例如如果你想从模拟引脚pin3读取数据，你就可以使用这个命令： 192 | 193 | > a 3 194 | 195 | 如果你想改变数字引脚pin3的模式为`OUTPUT`，就发送这个命令： 196 | 197 | > c 3 1 198 | 199 | 要得到目前编码器的计数，你可以用这个命令： 200 | 201 | > e 202 | 203 | 把机器人以每秒20个encoder ticks的速度向前移动，可以使用命令： 204 | 205 | > m 20 20 206 | 207 | **NOTE:**这些命令有些需要有对应的参数，比如`a 3`中`3`代表模拟引脚3，其实这些命令的参数很多都是不言自明的，如果有些命令比较隐晦（比如`m 20 20`中的两个参数究竟那个参数是左轮，那个是右轮并未说明），你可以通过查看源码了解相应参数的意义，这里也就不在一一赘述了。 208 | 209 | PS:**我在此强调一遍，务必要进行[这一节](#固件程序命令)以及[下一节](#测试你的电线连接)的测试，否则你不能保证python-serial（后面会说到）与主机交流正常。** 210 | 211 | # 测试你的电线连接 212 | 在一个差速轮式机器人上，电动机使用两个极性相反的接头来连接到电动机控制器上的。同样地，连接到编码器上的A/B端也是互斥的。然而，你仍然需要确保这样两个要求： 213 | 214 | - 当给一个正向的速度，轮子向前移动； 215 | - 而当轮子向前移动时，编码器计数增加。 216 | 217 | 把你的机器人放在一个小块儿上（**此处原文为：*placing your robot on blocks*，按我的理解是这样的：把机器人放在一个块儿上，使得它的轮子可以悬空，以便于测试**），你可以使用串口监视器来测试上述的两个要求。你可以使用`m`命令来启动电动机，使用`e`命令来获取编码器计数，以及使用`r`命令来将编码器重置为0。你要明白，在固件层，电动机的速度是按照编码器每秒的tick数来表示的，这样可以使编码器很容易解析（理解）它。假如轮子转速是每秒4000个编码器计数，那么命令`m 20 20`会以一个非常小的速度移动小车。（默认设置轮子仅仅移动2秒，你可以通过修改源码中的`AUTO_STOP_INTERVAL`改变这个值）。另外你还要知道，第一个参数是左轮的速度，另一个参数是右轮的速度。类似地，当使用`e`命令时，第一个被返回的数是左轮的编码器计数，第二个数是右轮的编码器计数。 218 | 219 | 最后，你可以先使用`r`命令把编码器计数清零，然后通过手动粗略地旋转轮子一整圈，再通过`e`命令来验证获取到的编码器计数是否为预期的结果。 220 | 221 | # 配置ros\_arduino\_python节点 222 | 223 | 既然你的Arduino已经可以按要求运行sketch了，那么现在你就可以在你的PC上把它配置为ROS节点。你可以在通过编辑ros\_arduino\_python/config路径下的YAML文件定义的机器人尺寸，PID参数，以及传感器配置信息。所以首先进入到这个路径中： 224 | 225 | ```bash 226 | $ roscd ros_arduino_python/config 227 | ``` 228 | 229 | 你可以把这个提供的配置文件拷贝一份然后再修改（名字可以自定义）： 230 | 231 | ```bash 232 | $ cp arduino_params.yaml my_arduino_params.yaml 233 | ``` 234 | 235 | 现在你可以使用任何你所喜欢的编辑器来编辑这个配置文件的副本（my_arduino_params.yaml），打开之后应该是这个样子： 236 | 237 | ```YAML 238 | # For a direct USB cable connection, the port name is typically 239 | # /dev/ttyACM# where is # is a number such as 0, 1, 2, etc 240 | # For a wireless connection like XBee, the port is typically 241 | # /dev/ttyUSB# where # is a number such as 0, 1, 2, etc. 242 | 243 | port: /dev/ttyACM0 244 | baud: 57600 245 | timeout: 0.1 246 | 247 | rate: 50 248 | sensorstate_rate: 10 249 | 250 | use_base_controller: False 251 | base_controller_rate: 10 252 | 253 | # For a robot that uses base_footprint, change base_frame to base_footprint 254 | base_frame: base_link 255 | 256 | # === Robot drivetrain parameters 257 | #wheel_diameter: 0.146 258 | #wheel_track: 0.2969 259 | #encoder_resolution: 8384 # from Pololu for 131:1 motors 260 | #gear_reduction: 1.0 261 | #motors_reversed: True 262 | 263 | # === PID parameters 264 | #Kp: 10 265 | #Kd: 12 266 | #Ki: 0 267 | #Ko: 50 268 | #accel_limit: 1.0 269 | 270 | # === Sensor definitions. Examples only - edit for your robot. 271 | # Sensor type can be one of the following: 272 | # * Ping 273 | # * GP2D12 274 | # * Analog 275 | # * Digital 276 | # * PololuMotorCurrent 277 | # * PhidgetsVoltage 278 | # * PhidgetsCurrent (20 Amp, DC) 279 | 280 | 281 | sensors: { 282 | #motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5}, 283 | #motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5}, 284 | #ir_front_center: {pin: 2, type: GP2D12, rate: 10}, 285 | #sonar_front_center: {pin: 5, type: Ping, rate: 10}, 286 | onboard_led: {pin: 13, type: Digital, rate: 5, direction: output} 287 | } 288 | 289 | # Joint name and configuration is an example only 290 | joints: { 291 | head_pan_joint: {pin: 3, init_position: 0, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False}, 292 | head_tilt_joint: {pin: 5, init_position: 0, init_speed: 90, neutral: 90, min_position: -90, max_position: 90, invert: False, continuous: False} 293 | } 294 | ``` 295 | **NOTE:**不要在你的.yaml文件里使用tab，否则的话这个语法解析器会无法加载它。如果你要缩进的话必须用空格代替tab键。 296 | 297 | **ALSO:**当定义你的传感器参数时，列表中的最后一个传感器的行尾（花括号后面）后没有逗号，但是其余的行尾必须有逗号。 298 | 299 | 现在让我们一起浏览一下这个文件的每一部分。 300 | 301 | ***接口设置*** 302 | 303 | 这个接口要么是/dev/ttyACM0，要么是/dev/ttyUSB0，视情况而定，而且前面我们也已经提到过这个。 304 | 305 | 其中MegaRobogaiaPololu的Arudino sketch默认是以57600的波特率连接的。 306 | 307 | ***轮询速率*** 308 | 309 | **跳出ROS loop运行的速率主要就取决于这个速率参数（默认为50Hz），这个默认值足以满足大多数情况。在任何情况下，它应该至少与你的传感器的最大速率（下面我们会说到）一样快才行。** 310 | 311 | *sensorstate_rate*决定了多久发布一个所有传感器的集合列表，每个传感器也以各自的速率在各自的主题上发布消息。 312 | 313 | *use_base_controller*参数默认为False。你可以把它设置为True（假设你有文中所要求的硬件设施）。下面你将必须设置PID参数。 314 | 315 | 316 | *base_controller_rate*参数决定了多久发布一次里程计读取信息。 317 | 318 | ***定义传感器*** 319 | 320 | *sensors*参数按照定义了传感器的名字和参数的字典。（你可以给你的传感器起任何你喜欢的名字，但是你必须要知道传感器的名字也会成为那个传感器所对应主题的名字。 321 | 322 | 这四个最重要的参数分别是：*pin*，*type*，*rate*，*direction*。*rate*定义了你每秒想轮询一次那个传感器多少次。例如，一个电压传感器可能每秒仅仅被轮询一次（或者仅仅每两秒一次），而一个声呐传感器可能每秒被轮询20次。*type*必须是列表中被列出来的（**注意区分大小写！**）。*direction*默认是*input*，所以如果你想将它定义为*output*，就将这个direction单独设为*output*。在上面的例子中，Arduino LED（pin13）将会以每秒两次的速率被点亮或熄灭。 323 | 324 | ***设置Drivetrain（驱动系统）和PID参数*** 325 | 326 | 为了使用基础控制器（base controller），你必须去掉它的注释并且设置机器人的drivetrain和PID参数。示例中drivetrain参数是直径6英寸的驱动轮，距离11.5英寸。注意在ROS中使用米作为距离单位，所以一定要换算单位。示例中的编码器的分辨率（每转的tick数）规格来自于Pololu 131:1电动机。为你的电动机/编码器组合设置合理的数值。如果你的轮子可以向后转，那么就把* motors_reversed*设置为true，否则的话就设置为False。 327 | 328 | PID参数比较难设置，你可以先按照示例中的值设置。但是在你第一次发送转弯命令的时候，一定要把你的机器人放在小块儿上。 329 | 330 | # 启动ros_arduino_python节点 331 | 332 | 你现在看一下路径ros\_arduino\_python/launch下的文件arduino.launch。正如你所看到的，它指向一个名叫my_arduino_params.yaml的文件（之前创建的）。 333 | 334 | ```xml 335 | 336 | 337 | 338 | 339 | 340 | ``` 341 | 342 | 如果你的配置文件命名不同，那么就把这里的文件名参数(my_arduino_params.yaml)修改成你的配置文件的名字。 343 | 344 | 当你连接好Arduino并运行MegaRobogaiaPololu sketch时，使用下面的参数来启动 ros_arduino_python node节点： 345 | 346 | ```bash 347 | $ roslaunch ros_arduino_python arduino.launch 348 | ``` 349 | 350 | **NOTE:**启动之前，千万不要打开Arduino IDE 的串口监视器，因为串口监视器会与该节点争夺串口资源。 351 | 352 | 你应该看到下面这样的输出： 353 | 354 | > process[arduino-1]: started with pid [6098] 355 | > Connecting to Arduino on port /dev/ttyUSB0 ... 356 | > Connected at 57600 357 | > Arduino is ready. 358 | > [INFO] [WallTime: 1355498525.954491] Connected to Arduino on port / > dev/ttyUSB0 at 57600 baud 359 | > [INFO] [WallTime: 1355498525.966825] motor\_current\_right {'rate': 5, > 'type': 'PololuMotorCurrent', 'pin': 1} 360 | > [INFO] 361 | > etc 362 | 363 | 如果你在你的机器人上装的有Ping声呐而且你也在配置文件里面定义了它们，它们就会闪一下来告诉你已经连接成功。 364 | 365 | # 查看传感器数据 366 | 367 | 你可以回显传感器状态主题来查看所有传感器的数据： 368 | 369 | ```bash 370 | $ rostopic echo /arduino/sensor_state 371 | ``` 372 | 373 | 回显相应传感器的主题名字你就可以查看任何指定的传感器数据： 374 | 375 | ```bash 376 | rostopic echo /arduino/sensor/sensor_name 377 | ``` 378 | 379 | 例如，你有一个叫做 380 | ir\_front\_center的传感器，那么你可以运行下面的命令来查看相应的数据： 381 | 382 | ```bash 383 | $ rostopic echo /arduino/sensor/ir_front_center 384 | ``` 385 | 386 | 你也可以使用`rxqrt`来将这系列数据用图像的形式表示出来： 387 | 388 | ```bash 389 | $ rxplot -p 60 /arduino/sensor/ir_front_center/range 390 | ``` 391 | # 发送Twist命令与查看里程计数据 392 | 393 | ros\_arduino\_python功能包也定义了一些ROS服务，如下所示： 394 | 395 | **digital\_set\_direction**-设置数字引脚的方向 396 | 397 | ```bash 398 | $ rosservice call /arduino/digital_set_direction pin direction 399 | ``` 400 | 401 | 这里`pin`是引脚数字，`direction`为0代表输入，1代表输出。 402 | 403 | **digital\_write**-给数字引脚发送高低电平（LOW为0，HIGH为1） 404 | 405 | ```bash 406 | $ rosservice call /arduino/digital_write pin value 407 | ``` 408 | 409 | 同样，这里`pin`是引脚数字，`value`是电平高低（LOW为0，HIGH为1）。 410 | 411 | **servo\_write**-设置伺服机位置 412 | 413 | ```bash 414 | $ rosservice call /arduino/servo_write id pos 415 | ``` 416 | 417 | 这里id是伺服机的索引号（定义在Arduino sketch中的servos.h）并且`pos`是以弧度为单位（0-3.14），头文件servos.h中具体是这样写的： 418 | 419 | ```bash 420 | byte servoInitPosition [N_SERVOS] = { 90, 90 }; // [0, 180] degrees 421 | ``` 422 | **servo_read **-读取伺服机的位置 423 | 424 | ```bash 425 | $ rosservice call /arduino/servo_read id 426 | ``` 427 | 428 | # 使用板上编码器计数（仅支持ArduinoUno） 429 | 430 | 对于Arduino Uno，这个固件程序支持板上的编码器计数。这样的话，编码器就直接可以连接到Arduino板上，而不用借助任何额外的编码器设备（例如RoboGaia encoder shield） 431 | 432 | 对于速度，这个代码可以直接找到Atmega328p的接口和中断管脚，而这一功能的实现必须依赖Atmega328p（Arduino Uno）。（尽管它也可能兼容其他电路板或AVR单片机芯片） 433 | 434 | 为了使用这个板上编码器计数，按照下面的要求来将编码器连接到Arduino Uno： 435 | 436 | ``` 437 | Left wheel encoder A output -- Arduino UNO pin 2 438 | Left wheel encoder B output -- Arduino UNO pin 3 439 | 440 | Right wheel encoder A output -- Arduino UNO pin A4 441 | Right wheel encoder B output -- Arduino UNO pin A5 442 | ``` 443 | 这时你需要在Arduino sketch中做相应的修改，通过下面的方式来取消使用RoboGaia encoder shield的代码，启用板上编码器的代码。 444 | 445 | ```c 446 | /* The RoboGaia encoder shield */ 447 | //#define ROBOGAIA 448 | /* Encoders directly attached to Arduino board */ 449 | #define ARDUINO_ENC_COUNTER 450 | ``` 451 | 452 | 这时你就可以编译并上传到Arduino上了。 453 | 454 | # 注意 455 | ------- 456 | 如果你没有文档中所要求的硬件来运行这个基础控制器，但是你仍然想使用其他兼容Arduino的控制器来读取传感器以及控制PWM伺服机，那么请按照下面的步骤： 457 | 458 | 首先，你需要编辑你的ROSArduinoBridge sketch，在文件的最前面，将这两行： 459 | 460 | ```c 461 | #define USE_BASE 462 | //#undef USE_BASE 463 | ``` 464 | 465 | 改成这样: 466 | 467 | ```c 468 | //#define USE_BASE 469 | #undef USE_BASE 470 | ``` 471 | 472 | **NOTE**:你还需要将文件encoder_driver.ino中的这一行注释掉： 473 | 474 | ```c 475 | #include "MegaEncoderCounter.h" 476 | ``` 477 | 这时你就可以编译并上传你的代码了。 478 | 479 | 然后，编辑你的 my\_arduino\_params.yaml文件，确保参数`use_base_controller`被设为False。这样你就完成了。 480 | 481 | # 可能遇到的错误 482 | 483 | 我在使用这个功能包的时候遇到了不少错误，因此我特意将这些错误及解决方法总结出来并与大家分享:[ros_arduino_bridge功能包集的使用错误及解决方法总结](http://blog.csdn.net/github_30605157/article/details/51621965)。 484 | --------------------------------------------------------------------------------