├── README.md
├── ros_arduino_bridge_Python3
├── README.md
├── ros_arduino_bridge
│ ├── CMakeLists.txt
│ └── package.xml
├── ros_arduino_firmware
│ ├── CMakeLists.txt
│ ├── package.xml
│ └── src
│ │ └── libraries
│ │ ├── MegaRobogaiaPololu
│ │ ├── MegaRobogaiaPololu.ino
│ │ ├── commands.h
│ │ ├── diff_controller.h
│ │ ├── sensors.h
│ │ └── servos.h
│ │ └── ROSArduinoBridge
│ │ ├── ROSArduinoBridge.ino
│ │ ├── commands.h
│ │ ├── diff_controller.h
│ │ ├── encoder_driver.h
│ │ ├── encoder_driver.ino
│ │ ├── motor_driver.h
│ │ ├── motor_driver.ino
│ │ ├── sensors.h
│ │ ├── servos.h
│ │ └── servos.ino
├── ros_arduino_msgs
│ ├── CMakeLists.txt
│ ├── msg
│ │ ├── Analog.msg
│ │ ├── AnalogFloat.msg
│ │ ├── ArduinoConstants.msg
│ │ ├── Digital.msg
│ │ └── SensorState.msg
│ ├── package.xml
│ └── srv
│ │ ├── AnalogRead.srv
│ │ ├── AnalogWrite.srv
│ │ ├── DigitalRead.srv
│ │ ├── DigitalSetDirection.srv
│ │ ├── DigitalWrite.srv
│ │ ├── ServoRead.srv
│ │ └── ServoWrite.srv
└── ros_arduino_python
│ ├── CMakeLists.txt
│ ├── config
│ └── my_arduino_params.yaml
│ ├── launch
│ └── arduino.launch
│ ├── nodes
│ └── arduino_node.py
│ ├── package.xml
│ ├── setup.py
│ └── src
│ └── ros_arduino_python
│ ├── arduino_driver.py
│ ├── arduino_sensors.py
│ └── base_controller.py
└── ros小车.jpeg
/README.md:
--------------------------------------------------------------------------------
1 | # ROS_Arduino_bridge
2 | 原始文件来自https://github.com/hbrobotics/ros_arduino_bridge
3 | ~~注：项目中缺少了部分必要文件，但已丢失，详见issue~~
4 | 最新版本中已经找回文件，可放心使用
5 |
6 | 本程序
7 | + 适配了淘宝TB6612驱动板以及对应的直流电机
8 | + 只适用于Python3版本，Python2只需使用原作者代码。基于Python3版本的ros-noetic可以使用，测试通过。
9 |
10 | 硬件组成
11 | + 树莓派4B ubuntu-mate20.04
12 | + arduino mega2560
13 | + TB6612驱动板
14 | + 直流减速电机
15 | + 万向轮
16 |
17 |
18 | 一些debug的细节
19 | + pyserial库可能读写速度比较快，程序串口调试一致失败，后来在串口通讯前调用一次readline解决问题
20 | + python2和python3语法不协同，包括但不限于thread库命名、dict.items遍历、str编码
21 | + 尝试使用arduino nano板子去代替mega2560，但是nano上可以自由控制的引脚数量不足，而且使用两个中断后，板子的pwm似乎不好用，该方案待解决。
22 | + 报错error encoder出现与数据类型，map无法使用list操作（解包，取长度等等），把返回值map改成list可行（很可能是从Python3语法与2不兼容导致）
23 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/README.md:
--------------------------------------------------------------------------------
1 | Overview
2 | --------
3 | This branch (indigo-devel) is intended for ROS Indigo and above, and uses the Catkin buildsystem. It may also be compatible with ROS Hydro.
4 |
5 | This ROS stack includes an Arduino library (called ROSArduinoBridge) and a collection of ROS packages for controlling an Arduino-based robot using standard ROS messages and services. The stack does **not** depend on ROS Serial.
6 |
7 | Features of the stack include:
8 |
9 | * Direct support for Ping sonar and Sharp infrared (GP2D12) sensors
10 |
11 | * Can also read data from generic analog and digital sensors
12 |
13 | * Can control digital outputs (e.g. turn a switch or LED on and off)
14 |
15 | * Support for PWM servos
16 | * Configurable base controller if using the required hardware
17 |
18 | The stack includes a base controller for a differential drive
19 | robot that accepts ROS Twist messages and publishes odometry data back to
20 | the PC. The base controller requires the use of a motor controller and encoders for reading odometry data. The current version of the stack provides support for the following base controller hardware:
21 |
22 | * 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).
23 |
24 | * Robogaia Mega Encoder shield
25 | (http://www.robogaia.com/two-axis-encoder-counter-mega-shield-version-2.html) or on-board wheel encoder counters.
26 |
27 | **NOTE:** The Robogaia Mega Encoder shield can only be used with an Arduino Mega. The on-board wheel encoder counters are currently only supported by Arduino Uno.
28 |
29 | * L298 motor driver
30 |
31 | * The library can be easily extended to include support for other motor controllers and encoder hardware or libraries.
32 |
33 | Official ROS Documentation
34 | --------------------------
35 | A standard ROS-style version of this documentation can be found on the ROS wiki at:
36 |
37 | http://www.ros.org/wiki/ros_arduino_bridge
38 |
39 |
40 | System Requirements
41 | -------------------
42 | **Python Serial:** To install the python-serial package under Ubuntu, use the command:
43 |
44 | $ sudo apt-get install python-serial
45 |
46 | On non-Ubuntu systems, use either:
47 |
48 | $ sudo pip install --upgrade pyserial
49 |
50 | or
51 |
52 | $ sudo easy_install -U pyserial
53 |
54 | **Arduino IDE 1.6.6 or Higher:**
55 | Note that the preprocessing of conditional #include statements is broken in earlier versions of the Arduino IDE. To ensure that the ROS Arduino Bridge firmware compiles correctly, be sure to install version 1.6.6 or higher of the Arduino IDE. You can download the IDE from https://www.arduino.cc/en/Main/Software.
56 |
57 | **Hardware:**
58 | The firmware should work with any Arduino-compatible controller for reading sensors and controlling PWM servos. However, to use the base controller, you will need a supported motor controller and encoder hardware as described above. If you do not have this hardware, you can still try the package for reading sensors and controlling servos. See the NOTES section at the end of this document for instructions on how to do this.
59 |
60 | To use the base controller you must also install the appropriate libraries for your motor controller and encoders. For the Pololu VNH5019 Dual Motor Shield, the library can be found at:
61 |
62 | https://github.com/pololu/Dual-VNH5019-Motor-Shield
63 |
64 | For the Pololu MC33926 Dual Motor Shield, the library can be found at:
65 |
66 | https://github.com/pololu/dual-mc33926-motor-shield
67 |
68 | The Robogaia Mega Encoder library can be found at:
69 |
70 | http://www.robogaia.com/uploads/6/8/0/9/6809982/__megaencodercounter-1.3.tar.gz
71 |
72 | L298 Motor Driver doesn't require any libraries
73 |
74 | These libraries should be installed in your standard Arduino
75 | sketchbook/libraries directory.
76 |
77 | Finally, it is assumed you are using version 1.0 or greater of the
78 | Arduino IDE.
79 |
80 | Preparing your Serial Port under Linux
81 | --------------------------------------
82 | Your Arduino will likely connect to your Linux computer as port /dev/ttyACM# or /dev/ttyUSB# where # is a number like 0, 1, 2, etc., depending on how many other devices are connected. The easiest way to make the determination is to unplug all other USB devices, plug in your Arduino, then run the command:
83 |
84 | $ ls /dev/ttyACM*
85 |
86 | or
87 |
88 | $ ls /dev/ttyUSB*
89 |
90 | Hopefully, one of these two commands will return the result you're looking for (e.g. /dev/ttyACM0) and the other will return the error "No such file or directory".
91 |
92 | Next you need to make sure you have read/write access to the port. Assuming your Arduino is connected on /dev/ttyACM0, run the command:
93 |
94 | $ ls -l /dev/ttyACM0
95 |
96 | and you should see an output similar to the following:
97 |
98 | crw-rw---- 1 root dialout 166, 0 2013-02-24 08:31 /dev/ttyACM0
99 |
100 | Note that only root and the "dialout" group have read/write access. Therefore, you need to be a member of the dialout group. You only have to do this once and it should then work for all USB devices you plug in later on.
101 |
102 | To add yourself to the dialout group, run the command:
103 |
104 | $ sudo usermod -a -G dialout your_user_name
105 |
106 | where your\_user\_name is your Linux login name. You will likely have to log out of your X-window session then log in again, or simply reboot your machine if you want to be sure.
107 |
108 | When you log back in again, try the command:
109 |
110 | $ groups
111 |
112 | and you should see a list of groups you belong to including dialout.
113 |
114 | Installation of the ros\_arduino\_bridge Stack
115 | ----------------------------------------------
116 |
117 | $ cd ~/catkin_workspace/src
118 | $ git clone https://github.com/hbrobotics/ros_arduino_bridge.git
119 | $ cd ~/catkin_workspace
120 | $ catkin_make
121 |
122 | The provided Arduino library is called ROSArduinoBridge and is
123 | located in the ros\_arduino\_firmware package. This sketch is
124 | specific to the hardware requirements above but it can also be used
125 | with other Arduino-type boards (e.g. Uno) by turning off the base
126 | controller as described in the NOTES section at the end of this
127 | document.
128 |
129 | To install the ROSArduinoBridge library, follow these steps:
130 |
131 | $ cd SKETCHBOOK_PATH
132 |
133 | where SKETCHBOOK_PATH is the path to your Arduino sketchbook directory.
134 |
135 | $ cp -rp `rospack find ros_arduino_firmware`/src/libraries/ROSArduinoBridge ROSArduinoBridge
136 |
137 | This last command copies the ROSArduinoBridge sketch files into your sketchbook folder. The next section describes how to configure, compile and upload this sketch.
138 |
139 |
140 | Loading the ROSArduinoBridge Sketch
141 | -----------------------------------
142 |
143 | * If you are using the base controller, make sure you have already installed the appropriate motor controller and encoder libraries into your Arduino sketchbook/librariesfolder.
144 |
145 | * Launch the Arduino 1.0 IDE and load the ROSArduinoBridge sketch.
146 | You should be able to find it by going to:
147 |
148 | File->Sketchbook->ROSArduinoBridge
149 |
150 | NOTE: If you don't have the required base controller hardware but
151 | still want to try the code, see the notes at the end of the file.
152 |
153 | Choose one of the supported motor controllers by uncommenting its #define statement and commenting out any others. By default, the Pololu VNH5019 driver is chosen.
154 |
155 | Choose a supported encoder library by by uncommenting its #define statement and commenting out any others. the Robogaia Mega Encoder shield is chosen by default.
156 |
157 | If you want to control PWM servos attached to your controller, look for the line:
158 |
159 |
160 | #define USE_SERVOS
161 |
162 |
163 | and make sure it is not commented out like this:
164 |
165 |
166 | //#define USE_SERVOS
167 |
168 |
169 | You must then edit the include file servos.h and change the N_SERVOS
170 | parameter as well as the pin numbers for the servos you have attached.
171 |
172 | * Compile and upload the sketch to your Arduino.
173 |
174 | Firmware Commands
175 | -----------------
176 | The ROSArduinoLibrary accepts single-letter commands over the serial port for polling sensors, controlling servos, driving the robot, and reading encoders. These commands can be sent to the Arduino over any serial interface, including the Serial Monitor in the Arduino IDE.
177 |
178 | **NOTE:** Before trying these commands, set the Serial Monitor baudrate to 57600 and the line terminator to "Carriage return" or "Both NL & CR" using the two pulldown menus on the lower right of the Serial Monitor window.
179 |
180 | The list of commands can be found in the file commands.h. The current list includes:
181 |
182 |
183 | #define ANALOG_READ 'a'
184 | #define GET_BAUDRATE 'b'
185 | #define PIN_MODE 'c'
186 | #define DIGITAL_READ 'd'
187 | #define READ_ENCODERS 'e'
188 | #define MOTOR_SPEEDS 'm'
189 | #define PING 'p'
190 | #define RESET_ENCODERS 'r'
191 | #define SERVO_WRITE 's'
192 | #define SERVO_READ 't'
193 | #define UPDATE_PID 'u'
194 | #define DIGITAL_WRITE 'w'
195 | #define ANALOG_WRITE 'x'
196 |
197 |
198 | For example, to get the analog reading on pin 3, use the command:
199 |
200 | a 3
201 |
202 | To change the mode of digital pin 3 to OUTPUT, send the command:
203 |
204 | c 3 1
205 |
206 | To get the current encoder counts:
207 |
208 | e
209 |
210 | To move the robot forward at 20 encoder ticks per second:
211 |
212 | m 20 20
213 |
214 |
215 | Testing your Wiring Connections
216 | -------------------------------
217 | On a differential drive robot, the motors are connected to the motor controller terminals with opposite polarities to each other. Similarly, the A/B leads from the encoders are connected in the reverse sense to each other. However, you still need to make sure that (a) the wheels move forward when given a positive motor speed and (b) that the encoder counts increase when the wheels move forward.
218 |
219 | After **placing your robot on blocks**, you can use the Serial Monitor in the Arduino IDE to test both requirements. Use the 'm' command to activate the motors, the 'e' command to get the encoder counts, and the 'r' command to reset the encoders to 0. Remember that at the firmware level, motor speeds are given in encoder ticks per second so that for an encoder resolution of, say 4000 counts per wheel revolution, a command such as 'm 20 20' should move the wheels fairly slowly. (The wheels will only move for 2 seconds which is the default setting for the AUTO\_STOP\_INTERVAL.) Also remember that the first argument is the left motor speed and the second argument is the right motor speed. Similarly, when using the 'e' command, the first number returned is the left encoder count and the second number is the right encoder count.
220 |
221 | Finally, you can use the 'r' and 'e' commands to verify the expected encoder counts by rotating the wheels by hand roughly one full turn and checking the reported counts.
222 |
223 |
224 | Configuring the ros\_arduino\_python Node
225 | -----------------------------------------
226 | Now that your Arduino is running the required sketch, you can
227 | configure the ROS side of things on your PC. You define your robot's
228 | dimensions, PID parameters, and sensor configuration by editing the
229 | YAML file in the directory ros\_arduino\_python/config. So first move
230 | into that directory:
231 |
232 | $ roscd ros_arduino_python/config
233 |
234 | Now copy the provided config file to one you can modify:
235 |
236 | $ cp arduino_params.yaml my_arduino_params.yaml
237 |
238 | Bring up your copy of the params file (my\_arduino\_params.yaml) in
239 | your favorite text editor. It should start off looking like this:
240 |
241 |
242 | port: /dev/ttyUSB0
243 | baud: 57600
244 | timeout: 0.1
245 |
246 | rate: 50
247 | sensorstate_rate: 10
248 |
249 | use_base_controller: False
250 | base_controller_rate: 10
251 |
252 | # === Robot drivetrain parameters
253 | #wheel_diameter: 0.146
254 | #wheel_track: 0.2969
255 | #encoder_resolution: 8384 # from Pololu for 131:1 motors
256 | #gear_reduction: 1.0
257 | #motors_reversed: True
258 |
259 | # === PID parameters
260 | #Kp: 20
261 | #Kd: 12
262 | #Ki: 0
263 | #Ko: 50
264 | #accel_limit: 1.0
265 |
266 | # === Sensor definitions. Examples only - edit for your robot.
267 | # Sensor type can be one of the follow (case sensitive!):
268 | # * Ping
269 | # * GP2D12
270 | # * Analog
271 | # * Digital
272 | # * PololuMotorCurrent
273 | # * PhidgetsVoltage
274 | # * PhidgetsCurrent (20 Amp, DC)
275 |
276 | sensors: {
277 | #motor_current_left: {pin: 0, type: PololuMotorCurrent, rate: 5},
278 | #motor_current_right: {pin: 1, type: PololuMotorCurrent, rate: 5},
279 | #ir_front_center: {pin: 2, type: GP2D12, rate: 10},
280 | #sonar_front_center: {pin: 5, type: Ping, rate: 10},
281 | arduino_led: {pin: 13, type: Digital, rate: 5, direction: output}
282 | }
283 |
284 |
285 | **NOTE**: Do not use tabs in your .yaml file or the parser will barf it back out when it tries to load it. Always use spaces instead. **ALSO**: When defining your sensor parameters, the last sensor in the list does **not** get a comma (,) at the end of the line but all the rest **must** have a comma.
286 |
287 | Let's now look at each section of this file.
288 |
289 | _Port Settings_
290 |
291 | The port will likely be either /dev/ttyACM0 or /dev/ttyUSB0. Set accordingly.
292 |
293 | The MegaRobogaiaPololu Arudino sketch connects at 57600 baud by default.
294 |
295 | _Polling Rates_
296 |
297 | The main *rate* parameter (50 Hz by default) determines how fast the
298 | outside ROS loop runs. The default should suffice in most cases. In
299 | any event, it should be at least as fast as your fastest sensor rate
300 | (defined below).
301 |
302 | The *sensorstate\_rate* determines how often to publish an aggregated
303 | list of all sensor readings. Each sensor also publishes on its own
304 | topic and rate.
305 |
306 | The *use\_base\_controller* parameter is set to False by default. Set it to True to use base control (assuming you have the required hardware.) You will also have to set the PID paramters that follow.
307 |
308 | The *base\_controller\_rate* determines how often to publish odometry readings.
309 |
310 | _Defining Sensors_
311 |
312 | The *sensors* parameter defines a dictionary of sensor names and
313 | sensor parameters. (You can name each sensor whatever you like but
314 | remember that the name for a sensor will also become the topic name
315 | for that sensor.)
316 |
317 | The four most important parameters are *pin*, *type*, *rate* and *direction*.
318 | The *rate* defines how many times per second you want to poll that
319 | sensor. For example, a voltage sensor might only be polled once a
320 | second (or even once every 2 seconds: rate=0.5), whereas a sonar
321 | sensor might be polled at 20 times per second. The *type* must be one
322 | of those listed (case sensitive!). The default *direction* is input so
323 | to define an output pin, set the direction explicitly to output. In
324 | the example above, the Arduino LED (pin 13) will be turned on and off
325 | at a rate of 2 times per second.
326 |
327 | _Setting Drivetrain and PID Parameters_
328 |
329 | To use the base controller, you will have to uncomment and set the
330 | robot drivetrain and PID parameters. The sample drivetrain parameters
331 | are for 6" drive wheels that are 11.5" apart. Note that ROS uses
332 | meters for distance so convert accordingly. The sample encoder
333 | resolution (ticks per revolution) is from the specs for the Pololu
334 | 131:1 motor. Set the appropriate number for your motor/encoder
335 | combination. Set the motors_reversed to True if you find your wheels
336 | are turning backward, otherwise set to False.
337 |
338 | The PID parameters are trickier to set. You can start with the sample
339 | values but be sure to place your robot on blocks before sending it
340 | your first Twist command.
341 |
342 | Launching the ros\_arduino\_python Node
343 | ---------------------------------------
344 | Take a look at the launch file arduino.launch in the
345 | ros\_arduino\_python/launch directory. As you can see, it points to a
346 | config file called my\_arduino\_params.yaml. If you named your config
347 | file something different, change the name in the launch file.
348 |
349 | With your Arduino connected and running the MegaRobogaiaPololu sketch,
350 | launch the ros\_arduino\_python node with your parameters:
351 |
352 | $ roslaunch ros_arduino_python arduino.launch
353 |
354 | You should see something like the following output:
355 |
356 |
357 | process[arduino-1]: started with pid [6098]
358 | Connecting to Arduino on port /dev/ttyUSB0 ...
359 | Connected at 57600
360 | Arduino is ready.
361 | [INFO] [WallTime: 1355498525.954491] Connected to Arduino on port /dev/ttyUSB0 at 57600 baud
362 | [INFO] [WallTime: 1355498525.966825] motor_current_right {'rate': 5, 'type': 'PololuMotorCurrent', 'pin': 1}
363 | [INFO]
364 | etc
365 |
366 |
367 | If you have any Ping sonar sensors on your robot and you defined them
368 | in your config file, they should start flashing to indicate you have
369 | made the connection.
370 |
371 | Viewing Sensor Data
372 | -------------------
373 | To see the aggregated sensor data, echo the sensor state topic:
374 |
375 | $ rostopic echo /arduino/sensor_state
376 |
377 | To see the data on any particular sensor, echo its topic name:
378 |
379 | $ rostopic echo /arduino/sensor/sensor_name
380 |
381 | For example, if you have a sensor called ir\_front\_center, you can see
382 | its data using:
383 |
384 | $ rostopic echo /arduino/sensor/ir_front_center
385 |
386 | You can also graph the range data using rxplot:
387 |
388 | $ rxplot -p 60 /arduino/sensor/ir_front_center/range
389 |
390 |
391 | Sending Twist Commands and Viewing Odometry Data
392 | ------------------------------------------------
393 |
394 | Place your robot on blocks, then try publishing a Twist command:
395 |
396 | $ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{ angular: {z: 0.5} }'
397 |
398 | The wheels should turn in a direction consistent with a
399 | counter-clockwise rotation (right wheel forward, left wheel backward).
400 | If they turn in the opposite direction, set the motors_reversed
401 | parameter in your config file to the opposite of its current setting,
402 | then kill and restart the arduino.launch file.
403 |
404 | Stop the robot with the command:
405 |
406 | $ rostopic pub -1 /cmd_vel geometry_msgs/Twist '{}'
407 |
408 | To view odometry data:
409 |
410 | $ rostopic echo /odom
411 |
412 | or
413 |
414 | $ rxplot -p 60 /odom/pose/pose/position/x:y, /odom/twist/twist/linear/x, /odom/twist/twist/angular/z
415 |
416 | ROS Services
417 | ------------
418 | The ros\_arduino\_python package also defines a few ROS services as follows:
419 |
420 | **digital\_set\_direction** - set the direction of a digital pin
421 |
422 | $ rosservice call /arduino/digital_set_direction pin direction
423 |
424 | where pin is the pin number and direction is 0 for input and 1 for output.
425 |
426 | **digital\_write** - send a LOW (0) or HIGH (1) signal to a digital pin
427 |
428 | $ rosservice call /arduino/digital_write pin value
429 |
430 | where pin is the pin number and value is 0 for LOW and 1 for HIGH.
431 |
432 | **servo\_write** - set the position of a servo
433 |
434 | $ rosservice call /arduino/servo_write id pos
435 |
436 | where id is the index of the servo as defined in the Arduino sketch (servos.h) and pos is the position in radians (0 - 3.14).
437 |
438 | **servo\_read** - read the position of a servo
439 |
440 | $ rosservice call /arduino/servo_read id
441 |
442 | where id is the index of the servo as defined in the Arduino sketch (servos.h)
443 |
444 | Using the on-board wheel encoder counters (Arduino Uno only)
445 | ------------------------------------------------------------
446 | The firmware supports on-board wheel encoder counters for Arduino Uno.
447 | This allows connecting wheel encoders directly to the Arduino board, without the need for any additional wheel encoder counter equipment (such as a RoboGaia encoder shield).
448 |
449 | For speed, the code is directly addressing specific Atmega328p ports and interrupts, making this implementation Atmega328p (Arduino Uno) dependent. (It should be easy to adapt for other boards/AVR chips though.)
450 |
451 | To use the on-board wheel encoder counters, connect your wheel encoders to Arduino Uno as follows:
452 |
453 | Left wheel encoder A output -- Arduino UNO pin 2
454 | Left wheel encoder B output -- Arduino UNO pin 3
455 |
456 | Right wheel encoder A output -- Arduino UNO pin A4
457 | Right wheel encoder B output -- Arduino UNO pin A5
458 |
459 | Make the following changes in the ROSArduinoBridge sketch to disable the RoboGaia encoder shield, and enable the on-board one:
460 |
461 | /* The RoboGaia encoder shield */
462 | //#define ROBOGAIA
463 | /* Encoders directly attached to Arduino board */
464 | #define ARDUINO_ENC_COUNTER
465 |
466 | Compile the changes and upload to your controller.
467 |
468 | Using L298 Motor driver
469 | -----------------------
470 | the wiring between the L298 motor driver and arduino board is defined in motor_driver.h in the firmware as follow:
471 |
472 | #define RIGHT_MOTOR_BACKWARD 5
473 | #define LEFT_MOTOR_BACKWARD 6
474 | #define RIGHT_MOTOR_FORWARD 9
475 | #define LEFT_MOTOR_FORWARD 10
476 | #define RIGHT_MOTOR_ENABLE 12
477 | #define LEFT_MOTOR_ENABLE 13
478 |
479 | wire them this way or change them if you want, and make sure that the L298 motor driver is defined then compile and upload the firmware.
480 |
481 | NOTES
482 | -----
483 | If you do not have the hardware required to run the base controller,
484 | follow the instructions below so that you can still use your
485 | Arduino-compatible controller to read sensors and control PWM servos.
486 |
487 | First, you need to edit the ROSArduinoBridge sketch. At the top of
488 | the file comment out the line:
489 |
490 |
491 | #define USE_BASE
492 |
493 |
494 | so that it looks like this:
495 |
496 |
497 | //#define USE_BASE
498 |
499 |
500 | **NOTE:** If you are using a version of the Arduino IDE previous to 1.6.6, you also need to comment out the line that looks like this in the file encoder_driver.ino:
501 |
502 | #include "MegaEncoderCounter.h"
503 |
504 | so it looks like this:
505 |
506 | //#include "MegaEncoderCounter.h"
507 |
508 | Compile the changes and upload to your controller.
509 |
510 | Next, edit your my\_arduino_params.yaml file and make sure the
511 | use\_base\_controller parameter is set to False. That's all there is to it.
512 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/ros_arduino_firmware/src/libraries/ROSArduinoBridge/ROSArduinoBridge.ino:
--------------------------------------------------------------------------------
1 |
2 | /*********************************************************************
3 | * ROSArduinoBridge
4 |
5 | A set of simple serial commands to control a differential drive
6 | robot and receive back sensor and odometry data. Default
7 | configuration assumes use of an Arduino Mega + Pololu motor
8 | controller shield + Robogaia Mega Encoder shield. Edit the
9 | readEncoder() and setMotorSpeed() wrapper functions if using
10 | different motor controller or encoder method.
11 |
12 | Created for the Pi Robot Project: http://www.pirobot.org
13 | and the Home Brew Robotics Club (HBRC): http://hbrobotics.org
14 | .....
15 | Authors: Patrick Goebel, James Nugen
16 |
17 | Inspired and modeled after the ArbotiX driver by Michael Ferguson
18 |
19 | Software License Agreement (BSD License)
20 |
21 | Copyright (c) 2012, Patrick Goebel.
22 | All rights reserved.
23 |
24 | Redistribution and use in source and binary forms, with or without
25 | modification, are permitted provided that the following conditions
26 | are met:
27 |
28 | * Redistributions of source code must retain the above copyright
29 | notice, this list of conditions and the following disclaimer.
30 | * Redistributions in binary form must reproduce the above
31 | copyright notice, this list of conditions and the following
32 | disclaimer in the documentation and/or other materials provided
33 | with the distribution.
34 |
35 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
36 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
37 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
38 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
39 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
40 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
41 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
42 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
43 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
44 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
45 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
46 | * POSSIBILITY OF SUCH DAMAGE.
47 | *********************************************************************/
48 |
49 | #define USE_BASE // Enable the base controller code
50 | //#undef USE_BASE // Disable the base controller code
51 |
52 | /* Define the motor controller and encoder library you are using */
53 | #ifdef USE_BASE
54 | /* The Pololu VNH5019 dual motor driver shield */
55 | // #define POLOLU_VNH5019
56 |
57 | /* The Pololu MC33926 dual motor driver shield */
58 | //#define POLOLU_MC33926
59 |
60 | /* The RoboGaia encoder shield */
61 | // #define ROBOGAIA
62 |
63 | /* Encoders directly attached to Arduino board */
64 | //#define ARDUINO_ENC_COUNTER
65 | #define MY_ARDUINO_COUNTER
66 |
67 | /* L298 Motor driver*/
68 | // #define L298_MOTOR_DRIVER
69 | #define L298P_MOTOR_DRIVER
70 | #endif
71 |
72 | //#define USE_SERVOS // Enable use of PWM servos as defined in servos.h
73 | #undef USE_SERVOS // Disable use of PWM servos
74 |
75 | /* Serial port baud rate */
76 | #define BAUDRATE 57600
77 |
78 | /* Maximum PWM signal */
79 | #define MAX_PWM 255
80 |
81 | #if defined(ARDUINO) && ARDUINO >= 100
82 | #include "Arduino.h"
83 | #else
84 | #include "WProgram.h"
85 | #endif
86 |
87 | /* Include definition of serial commands */
88 | #include "commands.h"
89 |
90 | /* Sensor functions */
91 | #include "sensors.h"
92 |
93 | /* Include servo support if required */
94 | #ifdef USE_SERVOS
95 | #include
96 | #include "servos.h"
97 | #endif
98 |
99 | #ifdef USE_BASE
100 | /* Motor driver function definitions */
101 | #include "motor_driver.h"
102 |
103 | /* Encoder driver function definitions */
104 | #include "encoder_driver.h"
105 |
106 | /* PID parameters and functions */
107 | #include "diff_controller.h"
108 |
109 | /* Run the PID loop at 30 times per second */
110 | #define PID_RATE 30 // Hz
111 |
112 | /* Convert the rate into an interval */
113 | const int PID_INTERVAL = 1000 / PID_RATE;
114 |
115 | /* Track the next time we make a PID calculation */
116 | unsigned long nextPID = PID_INTERVAL;
117 |
118 | /* Stop the robot if it hasn't received a movement command
119 | in this number of milliseconds */
120 | #define AUTO_STOP_INTERVAL 2000
121 | long lastMotorCommand = AUTO_STOP_INTERVAL;
122 | #endif
123 |
124 | /* Variable initialization */
125 |
126 | // A pair of varibles to help parse serial commands (thanks Fergs)
127 | int arg = 0;
128 | int index = 0;
129 |
130 | // Variable to hold an input character
131 | char chr;
132 |
133 | // Variable to hold the current single-character command
134 | char cmd;
135 |
136 | // Character arrays to hold the first and second arguments
137 | char argv1[16];
138 | char argv2[16];
139 |
140 | // The arguments converted to integers
141 | long arg1;
142 | long arg2;
143 |
144 | /* Clear the current command parameters */
145 | void resetCommand() {
146 | cmd = NULL;
147 | memset(argv1, 0, sizeof(argv1));
148 | memset(argv2, 0, sizeof(argv2));
149 | arg1 = 0;
150 | arg2 = 0;
151 | arg = 0;
152 | index = 0;
153 | }
154 |
155 | /* Run a command. Commands are defined in commands.h */
156 | int runCommand() {
157 | int i = 0;
158 | char *p = argv1;
159 | char *str;
160 | int pid_args[4];
161 | arg1 = atoi(argv1);
162 | arg2 = atoi(argv2);
163 |
164 | switch(cmd) {
165 | case GET_BAUDRATE:
166 | Serial.println(BAUDRATE);
167 | break;
168 | case ANALOG_READ:
169 | Serial.println(analogRead(arg1));
170 | break;
171 | case DIGITAL_READ:
172 | Serial.println(digitalRead(arg1));
173 | break;
174 | case ANALOG_WRITE:
175 | analogWrite(arg1, arg2);
176 | Serial.println("OK");
177 | break;
178 | case DIGITAL_WRITE:
179 | if (arg2 == 0) digitalWrite(arg1, LOW);
180 | else if (arg2 == 1) digitalWrite(arg1, HIGH);
181 | Serial.println("OK");
182 | break;
183 | case PIN_MODE:
184 | if (arg2 == 0) pinMode(arg1, INPUT);
185 | else if (arg2 == 1) pinMode(arg1, OUTPUT);
186 | Serial.println("OK");
187 | break;
188 | case PING:
189 | Serial.println(Ping(arg1));
190 | break;
191 | #ifdef USE_SERVOS
192 | case SERVO_WRITE:
193 | servos[arg1].setTargetPosition(arg2);
194 | Serial.println("OK");
195 | break;
196 | case SERVO_READ:
197 | Serial.println(servos[arg1].getServo().read());
198 | break;
199 | #endif
200 |
201 | #ifdef USE_BASE
202 | case READ_ENCODERS:
203 | Serial.print(readEncoder(LEFT));
204 | Serial.print(" ");
205 | Serial.println(readEncoder(RIGHT));
206 | break;
207 | case RESET_ENCODERS:
208 | resetEncoders();
209 | resetPID();
210 | Serial.println("OK");
211 | break;
212 | case MOTOR_SPEEDS:
213 | /* Reset the auto stop timer */
214 | lastMotorCommand = millis();
215 | if (arg1 == 0 && arg2 == 0) {
216 | setMotorSpeeds(0, 0);
217 | resetPID();
218 | moving = 0;
219 | }
220 | else moving = 1;
221 | leftPID.TargetTicksPerFrame = arg1;
222 | rightPID.TargetTicksPerFrame = arg2;
223 | Serial.println("OK");
224 | break;
225 | case UPDATE_PID:
226 | while ((str = strtok_r(p, ":", &p)) != '\0') {
227 | pid_args[i] = atoi(str);
228 | i++;
229 | }
230 | Kp = pid_args[0];
231 | Kd = pid_args[1];
232 | Ki = pid_args[2];
233 | Ko = pid_args[3];
234 | Serial.println("OK");
235 | break;
236 | #endif
237 |
238 | default:
239 | Serial.println("Invalid Command");
240 | break;
241 | }
242 | }
243 |
244 | /* Setup function--runs once at startup. */
245 | void setup() {
246 | Serial.begin(BAUDRATE);
247 |
248 |
249 | // Initialize the motor controller if used */
250 | #ifdef USE_BASE
251 | #ifdef ARDUINO_ENC_COUNTER
252 | //set as inputs
253 | DDRD &= ~(1< 0) {
295 |
296 | // Read the next character
297 | chr = Serial.read();
298 |
299 | // Terminate a command with a CR
300 | if (chr == 13) {
301 | if (arg == 1) argv1[index] = NULL;
302 | else if (arg == 2) argv2[index] = NULL;
303 | runCommand();
304 | resetCommand();
305 | }
306 | // Use spaces to delimit parts of the command
307 | else if (chr == ' ') {
308 | // Step through the arguments
309 | if (arg == 0) arg = 1;
310 | else if (arg == 1) {
311 | argv1[index] = NULL;
312 | arg = 2;
313 | index = 0;
314 | }
315 | continue;
316 | }
317 | else {
318 | if (arg == 0) {
319 | // The first arg is the single-letter command
320 | cmd = chr;
321 | }
322 | else if (arg == 1) {
323 | // Subsequent arguments can be more than one character
324 | argv1[index] = chr;
325 | index++;
326 | }
327 | else if (arg == 2) {
328 | argv2[index] = chr;
329 | index++;
330 | }
331 | }
332 | }
333 |
334 | // If we are using base control, run a PID calculation at the appropriate intervals
335 | #ifdef USE_BASE
336 | if (millis() > nextPID) {
337 | updatePID();
338 | nextPID += PID_INTERVAL;
339 | }
340 | // Check to see if we have exceeded the auto-stop interval
341 | if ((millis() - lastMotorCommand) > AUTO_STOP_INTERVAL) {;
342 | setMotorSpeeds(0, 0);
343 | moving = 0;
344 | }
345 |
346 | #endif
347 |
348 | // Sweep servos
349 | #ifdef USE_SERVOS
350 | int i;
351 | for (i = 0; i < N_SERVOS; i++) {
352 | servos[i].doSweep();
353 | }
354 | #endif
355 |
356 |
357 |
358 | }
359 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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 LIDAR_BEGIN 'o'
24 | #define LIDAR_END 'i'
25 | #endif
26 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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 = 45;
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 | // Serial.println(analogRead(5));
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 | Serial.println(input);
101 | // Serial.println(p->PrevEnc);
102 | p->output = output;
103 | p->PrevInput = input;
104 | }
105 |
106 | /* Read the encoder values and call the PID routine */
107 | void updatePID() {
108 | /* Read the encoders */
109 | leftPID.Encoder = readEncoder(LEFT);
110 | rightPID.Encoder = readEncoder(RIGHT);
111 |
112 | /* If we're not moving there is nothing more to do */
113 | if (!moving){
114 | /*
115 | * Reset PIDs once, to prevent startup spikes,
116 | * see http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-initialization/
117 | * PrevInput is considered a good proxy to detect
118 | * whether reset has already happened
119 | */
120 | if (leftPID.PrevInput != 0 || rightPID.PrevInput != 0) resetPID();
121 | return;
122 | }
123 |
124 | /* Compute PID update for each motor */
125 | doPID(&rightPID);
126 | doPID(&leftPID);
127 |
128 | /* Set the motor speeds accordingly */
129 | // Serial.println(leftPID.output);
130 | // Serial.println(rightPID.output);
131 | setMotorSpeeds(leftPID.output, rightPID.output);
132 | }
133 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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 | #elif defined MY_ARDUINO_COUNTER
16 | #define LEFT_A 18
17 | #define LEFT_B 19
18 | #define RIGHT_A 20
19 | #define RIGHT_B 21
20 | #endif
21 |
22 | long readEncoder(int i);
23 | void resetEncoder(int i);
24 | void resetEncoders();
25 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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 MY_ARDUINO_COUNTER
31 | volatile long left_count = 0L;//左轮计数器
32 | volatile long right_count = 0L;//右轮计数器
33 | void initEncoders(){
34 | pinMode(LEFT_A, INPUT); //18 --- 5
35 | pinMode(LEFT_B, INPUT); //19 --- 4
36 | pinMode(RIGHT_A, INPUT);//3
37 | pinMode(RIGHT_B, INPUT);//2
38 |
39 | attachInterrupt(5, leftEncoderEventA, CHANGE);
40 | attachInterrupt(4, leftEncoderEventB, CHANGE);
41 | attachInterrupt(2, rightEncoderEventA, CHANGE);
42 | attachInterrupt(3, rightEncoderEventB, CHANGE);
43 | }
44 | void leftEncoderEventA(){
45 | if(digitalRead(LEFT_A) == HIGH){
46 | if(digitalRead(LEFT_B) == HIGH){
47 | left_count++;
48 | } else {
49 | left_count--;
50 | }
51 | } else {
52 | if(digitalRead(LEFT_B) == LOW){
53 | left_count++;
54 | } else {
55 | left_count--;
56 | }
57 | }
58 | }
59 | void leftEncoderEventB(){
60 | if(digitalRead(LEFT_B) == HIGH){
61 | if(digitalRead(LEFT_A) == LOW){
62 | left_count++;
63 | } else {
64 | left_count--;
65 | }
66 | } else {
67 | if(digitalRead(LEFT_A) == HIGH){
68 | left_count++;
69 | } else {
70 | left_count--;
71 | }
72 | }
73 | }
74 | void rightEncoderEventA(){
75 | if(digitalRead(RIGHT_A) == HIGH){
76 | if(digitalRead(RIGHT_B) == HIGH){
77 | right_count++;
78 | } else {
79 | right_count--;
80 | }
81 | } else {
82 | if(digitalRead(RIGHT_B) == LOW){
83 | right_count++;
84 | } else {
85 | right_count--;
86 | }
87 | }
88 | }
89 |
90 | void rightEncoderEventB(){
91 | if(digitalRead(RIGHT_B) == HIGH){
92 | if(digitalRead(RIGHT_A) == LOW){
93 | right_count++;
94 | } else {
95 | right_count--;
96 | }
97 | } else {
98 | if(digitalRead(RIGHT_A) == HIGH){
99 | right_count++;
100 | } else {
101 | right_count--;
102 | }
103 | }
104 | }
105 | /* Wrap the encoder reading function */
106 | long readEncoder(int i) {
107 | if (i == LEFT) return left_count;
108 | else return right_count; // It's just because my right encoder get reverse value so if yours is normal, don't add "-"
109 | }
110 |
111 | /* Wrap the encoder reset function */
112 | void resetEncoder(int i) {
113 | if (i == LEFT){
114 | left_count=0L;
115 | return;
116 | } else {
117 | right_count=0L;
118 | return;
119 | }
120 | }
121 | #elif defined(ARDUINO_ENC_COUNTER)
122 | volatile long left_enc_pos = 0L;
123 | volatile long right_enc_pos = 0L;
124 | 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
125 |
126 | /* Interrupt routine for LEFT encoder, taking care of actual counting */
127 | ISR (PCINT2_vect){
128 | static uint8_t enc_last=0;
129 |
130 | enc_last <<=2; //shift previous state two places
131 | enc_last |= (PIND & (3 << 2)) >> 2; //read the current state into lowest 2 bits
132 |
133 | left_enc_pos += ENC_STATES[(enc_last & 0x0f)];
134 | }
135 |
136 | /* Interrupt routine for RIGHT encoder, taking care of actual counting */
137 | ISR (PCINT1_vect){
138 | static uint8_t enc_last=0;
139 |
140 | enc_last <<=2; //shift previous state two places
141 | enc_last |= (PINC & (3 << 4)) >> 4; //read the current state into lowest 2 bits
142 |
143 | right_enc_pos += ENC_STATES[(enc_last & 0x0f)];
144 | }
145 |
146 | /* Wrap the encoder reading function */
147 | long readEncoder(int i) {
148 | if (i == LEFT) return left_enc_pos;
149 | else return right_enc_pos;
150 | }
151 |
152 | /* Wrap the encoder reset function */
153 | void resetEncoder(int i) {
154 | if (i == LEFT){
155 | left_enc_pos=0L;
156 | return;
157 | } else {
158 | right_enc_pos=0L;
159 | return;
160 | }
161 | }
162 | #else
163 | #error A encoder driver must be selected!
164 | #endif
165 |
166 | /* Wrap the encoder reset function */
167 | void resetEncoders() {
168 | resetEncoder(LEFT);
169 | resetEncoder(RIGHT);
170 | }
171 |
172 | #endif
173 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_firmware/src/libraries/ROSArduinoBridge/motor_driver.h:
--------------------------------------------------------------------------------
1 | /***************************************************************
2 | Motor driver function definitions - by James Nugen
3 | *************************************************************/
4 |
5 | #ifdef L298_MOTOR_DRIVER
6 | #define RIGHT_MOTOR_BACKWARD 5
7 | #define LEFT_MOTOR_BACKWARD 6
8 | #define RIGHT_MOTOR_FORWARD 9
9 | #define LEFT_MOTOR_FORWARD 10
10 | #define RIGHT_MOTOR_ENABLE 12
11 | #define LEFT_MOTOR_ENABLE 13
12 | #elif defined L298P_MOTOR_DRIVER
13 | #define DIRA 7
14 | #define PWMA 5
15 | #define DIRB 2
16 | #define PWMB 6
17 | #endif
18 |
19 | void initMotorController();
20 | void setMotorSpeed(int i, int spd);
21 | void setMotorSpeeds(int leftSpeed, int rightSpeed);
22 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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 | #elif defined L298_MOTOR_DRIVER
59 | void initMotorController() {
60 | digitalWrite(RIGHT_MOTOR_ENABLE, HIGH);
61 | digitalWrite(LEFT_MOTOR_ENABLE, HIGH);
62 | }
63 |
64 | void setMotorSpeed(int i, int spd) {
65 | unsigned char reverse = 0;
66 |
67 | if (spd < 0)
68 | {
69 | spd = -spd;
70 | reverse = 1;
71 | }
72 | if (spd > 255)
73 | spd = 255;
74 |
75 | if (i == LEFT) {
76 | if (reverse == 0) { analogWrite(RIGHT_MOTOR_FORWARD, spd); analogWrite(RIGHT_MOTOR_BACKWARD, 0); }
77 | else if (reverse == 1) { analogWrite(RIGHT_MOTOR_BACKWARD, spd); analogWrite(RIGHT_MOTOR_FORWARD, 0); }
78 | }
79 | else /*if (i == RIGHT) //no need for condition*/ {
80 | if (reverse == 0) { analogWrite(LEFT_MOTOR_FORWARD, spd); analogWrite(LEFT_MOTOR_BACKWARD, 0); }
81 | else if (reverse == 1) { analogWrite(LEFT_MOTOR_BACKWARD, spd); analogWrite(LEFT_MOTOR_FORWARD, 0); }
82 | }
83 | }
84 |
85 | void setMotorSpeeds(int leftSpeed, int rightSpeed) {
86 | setMotorSpeed(LEFT, leftSpeed);
87 | setMotorSpeed(RIGHT, rightSpeed);
88 | }
89 | #elif defined L298P_MOTOR_DRIVER
90 | void initMotorController() {
91 | pinMode(DIRA,OUTPUT);
92 | pinMode(PWMA,OUTPUT);
93 | pinMode(DIRB,OUTPUT);
94 | pinMode(PWMB,OUTPUT);
95 | pinMode(12,OUTPUT);
96 | pinMode(13,OUTPUT);
97 | }
98 |
99 | void setMotorSpeed(int i, int spd) {
100 | unsigned char reverse = 0;
101 |
102 | if (spd < 0)
103 | {
104 | spd = -spd;
105 | reverse = 1;
106 | }
107 | if (spd > 255){
108 | spd = 255;
109 | }
110 |
111 | if (i == LEFT) {
112 | if(reverse == 0) {
113 | digitalWrite(DIRA,HIGH);
114 | digitalWrite(13,LOW);
115 | analogWrite(PWMA,spd);
116 | }else if (reverse == 1) {
117 | digitalWrite(DIRA,LOW);
118 | digitalWrite(13,HIGH);
119 | analogWrite(PWMA,spd);
120 | }
121 | }else{
122 | if(reverse == 1) {
123 | digitalWrite(DIRB,LOW);
124 | digitalWrite(12,HIGH);
125 | analogWrite(PWMB,spd);
126 | } else if (reverse == 0) {
127 | digitalWrite(DIRB,HIGH);
128 | digitalWrite(12,LOW);
129 | analogWrite(PWMB,spd);
130 | }
131 | }
132 | }
133 |
134 | void setMotorSpeeds(int leftSpeed, int rightSpeed) {
135 | setMotorSpeed(LEFT, leftSpeed);
136 | setMotorSpeed(RIGHT, rightSpeed);
137 | }
138 | #else
139 | #error A motor driver must be selected!
140 | #endif
141 |
142 | #endif
143 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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_bridge_Python3/ros_arduino_msgs/msg/Analog.msg:
--------------------------------------------------------------------------------
1 | # Reading from a single analog IO pin.
2 | Header header
3 | uint16 value
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/msg/AnalogFloat.msg:
--------------------------------------------------------------------------------
1 | # Float message from a single analog IO pin.
2 | Header header
3 | float64 value
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_Python3/ros_arduino_msgs/msg/Digital.msg:
--------------------------------------------------------------------------------
1 | # Reading on a digital pin
2 | Header header
3 | uint8 value
4 |
5 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/msg/SensorState.msg:
--------------------------------------------------------------------------------
1 | Header header
2 |
3 | string[] name
4 | float32[] value
5 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_bridge_Python3/ros_arduino_msgs/srv/AnalogRead.srv:
--------------------------------------------------------------------------------
1 | uint8 pin
2 | ---
3 | uint16 value
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/srv/AnalogWrite.srv:
--------------------------------------------------------------------------------
1 | uint8 pin
2 | uint16 value
3 | ---
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/srv/DigitalRead.srv:
--------------------------------------------------------------------------------
1 | uint8 pin
2 | ---
3 | bool value
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/srv/DigitalSetDirection.srv:
--------------------------------------------------------------------------------
1 | uint8 pin
2 | bool direction
3 | ---
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/srv/DigitalWrite.srv:
--------------------------------------------------------------------------------
1 | uint8 pin
2 | bool value
3 | ---
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/srv/ServoRead.srv:
--------------------------------------------------------------------------------
1 | uint8 id
2 | ---
3 | float32 value
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_msgs/srv/ServoWrite.srv:
--------------------------------------------------------------------------------
1 | uint8 id
2 | float32 value
3 | ---
4 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_bridge_Python3/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/ttyUSB0
7 | baud: 57600
8 | timeout: 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.146
21 | wheel_track: 0.2969
22 | encoder_resolution: 260 # 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 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/ros_arduino_python/launch/arduino.launch:
--------------------------------------------------------------------------------
1 |
2 |
3 |
4 |
5 |
6 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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 as thread
30 | from serial.serialutil import SerialException
31 |
32 | class ArduinoROS():
33 | def __init__(self):
34 | rospy.init_node('arduino', log_level=rospy.INFO)
35 |
36 | # Get the actual node name in case it is set in the launch file
37 | self.name = rospy.get_name()
38 |
39 | # Cleanup when termniating the node
40 | rospy.on_shutdown(self.shutdown)
41 |
42 | self.port = rospy.get_param("~port", "/dev/ttyUSB0")
43 | self.baud = int(rospy.get_param("~baud", 57600))
44 | self.timeout = rospy.get_param("~timeout", 0.5)
45 | self.base_frame = rospy.get_param("~base_frame", 'base_link')
46 | self.motors_reversed = rospy.get_param("~motors_reversed", False)
47 | # Overall loop rate: should be faster than fastest sensor rate
48 | self.rate = int(rospy.get_param("~rate", 50))
49 | r = rospy.Rate(self.rate)
50 |
51 | # Rate at which summary SensorState message is published. Individual sensors publish
52 | # at their own rates.
53 | self.sensorstate_rate = int(rospy.get_param("~sensorstate_rate", 10))
54 |
55 | self.use_base_controller = rospy.get_param("~use_base_controller", False)
56 |
57 | # Set up the time for publishing the next SensorState message
58 | now = rospy.Time.now()
59 | self.t_delta_sensors = rospy.Duration(1.0 / self.sensorstate_rate)
60 | self.t_next_sensors = now + self.t_delta_sensors
61 |
62 | # Initialize a Twist message
63 | self.cmd_vel = Twist()
64 |
65 | # A cmd_vel publisher so we can stop the robot when shutting down
66 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
67 |
68 | # The SensorState publisher periodically publishes the values of all sensors on
69 | # a single topic.
70 | self.sensorStatePub = rospy.Publisher('~sensor_state', SensorState, queue_size=5)
71 |
72 | # A service to position a PWM servo
73 | rospy.Service('~servo_write', ServoWrite, self.ServoWriteHandler)
74 |
75 | # A service to read the position of a PWM servo
76 | rospy.Service('~servo_read', ServoRead, self.ServoReadHandler)
77 |
78 | # A service to turn set the direction of a digital pin (0 = input, 1 = output)
79 | rospy.Service('~digital_set_direction', DigitalSetDirection, self.DigitalSetDirectionHandler)
80 |
81 | # A service to turn a digital sensor on or off
82 | rospy.Service('~digital_write', DigitalWrite, self.DigitalWriteHandler)
83 |
84 | # A service to read the value of a digital sensor
85 | rospy.Service('~digital_read', DigitalRead, self.DigitalReadHandler)
86 |
87 | # A service to set pwm values for the pins
88 | rospy.Service('~analog_write', AnalogWrite, self.AnalogWriteHandler)
89 |
90 | # A service to read the value of an analog sensor
91 | rospy.Service('~analog_read', AnalogRead, self.AnalogReadHandler)
92 |
93 | # Initialize the controlller
94 | self.controller = Arduino(self.port, self.baud, self.timeout, self.motors_reversed)
95 |
96 | # Make the connection
97 | self.controller.connect()
98 |
99 | rospy.loginfo("Connected to Arduino on port " + self.port + " at " + str(self.baud) + " baud")
100 |
101 | # Reserve a thread lock
102 | mutex = thread.allocate_lock()
103 |
104 | # Initialize any sensors
105 | self.mySensors = list()
106 |
107 | sensor_params = rospy.get_param("~sensors", dict({}))
108 |
109 | for name, params in sensor_params.items():
110 | # Set the direction to input if not specified
111 | try:
112 | params['direction']
113 | except:
114 | params['direction'] = 'input'
115 |
116 | if params['type'] == "Ping":
117 | sensor = Ping(self.controller, name, params['pin'], params['rate'], self.base_frame)
118 | elif params['type'] == "GP2D12":
119 | sensor = GP2D12(self.controller, name, params['pin'], params['rate'], self.base_frame)
120 | elif params['type'] == 'Digital':
121 | sensor = DigitalSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
122 | elif params['type'] == 'Analog':
123 | sensor = AnalogSensor(self.controller, name, params['pin'], params['rate'], self.base_frame, direction=params['direction'])
124 | elif params['type'] == 'PololuMotorCurrent':
125 | sensor = PololuMotorCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
126 | elif params['type'] == 'PhidgetsVoltage':
127 | sensor = PhidgetsVoltage(self.controller, name, params['pin'], params['rate'], self.base_frame)
128 | elif params['type'] == 'PhidgetsCurrent':
129 | sensor = PhidgetsCurrent(self.controller, name, params['pin'], params['rate'], self.base_frame)
130 |
131 | # if params['type'] == "MaxEZ1":
132 | # self.sensors[len(self.sensors)]['trigger_pin'] = params['trigger_pin']
133 | # self.sensors[len(self.sensors)]['output_pin'] = params['output_pin']
134 | try:
135 | self.mySensors.append(sensor)
136 | rospy.loginfo(name + " " + str(params) + " published on topic " + rospy.get_name() + "/sensor/" + name)
137 | except:
138 | rospy.logerr("Sensor type " + str(params['type']) + " not recognized.")
139 |
140 | # Initialize the base controller if used
141 | if self.use_base_controller:
142 | self.myBaseController = BaseController(self.controller, self.base_frame, self.name + "_base_controller")
143 |
144 | # Start polling the sensors and base controller
145 | while not rospy.is_shutdown():
146 | for sensor in self.mySensors:
147 | mutex.acquire()
148 | sensor.poll()
149 | mutex.release()
150 |
151 | if self.use_base_controller:
152 | mutex.acquire()
153 | self.myBaseController.poll()
154 | mutex.release()
155 |
156 | # Publish all sensor values on a single topic for convenience
157 | now = rospy.Time.now()
158 |
159 | if now > self.t_next_sensors:
160 | msg = SensorState()
161 | msg.header.frame_id = self.base_frame
162 | msg.header.stamp = now
163 | for i in range(len(self.mySensors)):
164 | msg.name.append(self.mySensors[i].name)
165 | msg.value.append(self.mySensors[i].value)
166 | try:
167 | self.sensorStatePub.publish(msg)
168 | except:
169 | pass
170 |
171 | self.t_next_sensors = now + self.t_delta_sensors
172 |
173 | r.sleep()
174 |
175 | # Service callback functions
176 | def ServoWriteHandler(self, req):
177 | self.controller.servo_write(req.id, req.value)
178 | return ServoWriteResponse()
179 |
180 | def ServoReadHandler(self, req):
181 | pos = self.controller.servo_read(req.id)
182 | return ServoReadResponse(pos)
183 |
184 | def DigitalSetDirectionHandler(self, req):
185 | self.controller.pin_mode(req.pin, req.direction)
186 | return DigitalSetDirectionResponse()
187 |
188 | def DigitalWriteHandler(self, req):
189 | self.controller.digital_write(req.pin, req.value)
190 | return DigitalWriteResponse()
191 |
192 | def DigitalReadHandler(self, req):
193 | value = self.controller.digital_read(req.pin)
194 | return DigitalReadResponse(value)
195 |
196 | def AnalogWriteHandler(self, req):
197 | self.controller.analog_write(req.pin, req.value)
198 | return AnalogWriteResponse()
199 |
200 | def AnalogReadHandler(self, req):
201 | value = self.controller.analog_read(req.pin)
202 | return AnalogReadResponse(value)
203 |
204 | def shutdown(self):
205 | rospy.loginfo("Shutting down Arduino Node...")
206 |
207 | # Stop the robot
208 | try:
209 | rospy.loginfo("Stopping the robot...")
210 | self.cmd_vel_pub.Publish(Twist())
211 | rospy.sleep(2)
212 | except:
213 | pass
214 |
215 | # Close the serial port
216 | try:
217 | self.controller.close()
218 | except:
219 | pass
220 | finally:
221 | rospy.loginfo("Serial port closed.")
222 | os._exit(0)
223 |
224 | if __name__ == '__main__':
225 | try:
226 | myArduino = ArduinoROS()
227 | except SerialException:
228 | rospy.logerr("Serial exception trying to open port.")
229 | os._exit(0)
230 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_bridge_Python3/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_bridge_Python3/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 as thread
25 | from math import pi as PI, degrees, radians
26 | import os
27 | import time
28 | import sys, traceback
29 | from serial.serialutil import SerialException
30 | from serial import Serial
31 |
32 | SERVO_MAX = 180
33 | SERVO_MIN = 0
34 |
35 | class Arduino:
36 | ''' Configuration Parameters
37 | '''
38 | N_ANALOG_PORTS = 6
39 | N_DIGITAL_PORTS = 12
40 |
41 | def __init__(self, port="/dev/ttyUSB0", baudrate=57600, timeout=0.5, motors_reversed=False):
42 |
43 | self.PID_RATE = 30 # Do not change this! It is a fixed property of the Arduino PID controller.
44 | self.PID_INTERVAL = 1000 / 30
45 |
46 | self.port = port
47 | self.baudrate = baudrate
48 | self.timeout = timeout
49 | self.encoder_count = 0
50 | self.writeTimeout = timeout
51 | self.interCharTimeout = timeout / 30.
52 | self.motors_reversed = motors_reversed
53 | # Keep things thread safe
54 | self.mutex = thread.allocate_lock()
55 |
56 | # An array to cache analog sensor readings
57 | self.analog_sensor_cache = [None] * self.N_ANALOG_PORTS
58 |
59 | # An array to cache digital sensor readings
60 | self.digital_sensor_cache = [None] * self.N_DIGITAL_PORTS
61 |
62 | def connect(self):
63 | try:
64 | print("Connecting to Arduino on port", self.port, "...")
65 | self.port = Serial(port=self.port, baudrate=self.baudrate, timeout=self.timeout, writeTimeout=self.writeTimeout)
66 | # The next line is necessary to give the firmware time to wake up.
67 | time.sleep(1)
68 | self.port.readline()
69 | self.port.write(b"b \r")
70 | print(self.port.readline())
71 | test = self.get_baud()
72 | if test != self.baudrate:
73 | time.sleep(1)
74 | test = self.get_baud()
75 | if test != self.baudrate:
76 | raise SerialException
77 | print("Connected at", self.baudrate)
78 | print("Arduino is ready.")
79 |
80 | except SerialException:
81 | print("Serial Exception:")
82 | print(sys.exc_info())
83 | print("Traceback follows:")
84 | traceback.print_exc(file=sys.stdout)
85 | print("Cannot connect to Arduino!")
86 | os._exit(1)
87 |
88 | def open(self):
89 | ''' Open the serial port.
90 | '''
91 | self.port.open()
92 |
93 | def close(self):
94 | ''' Close the serial port.
95 | '''
96 | self.port.close()
97 |
98 | def send(self, cmd):
99 | ''' This command should not be used on its own: it is called by the execute commands
100 | below in a thread safe manner.
101 | '''
102 | self.port.write(cmd.encode() + b'\r')
103 |
104 | def recv(self, timeout=0.5):
105 | timeout = min(timeout, self.timeout)
106 | ''' This command should not be used on its own: it is called by the execute commands
107 | below in a thread safe manner. Note: we use read() instead of readline() since
108 | readline() tends to return garbage characters from the Arduino
109 | '''
110 | c = ''
111 | value = ''
112 | attempts = 0
113 | while c != '\r':
114 | c = self.port.read(1).decode()
115 | value += c
116 | attempts += 1
117 | if attempts * self.interCharTimeout > timeout:
118 | return None
119 |
120 | value = value.strip('\r')
121 |
122 | return value
123 |
124 | def recv_ack(self):
125 | ''' This command should not be used on its own: it is called by the execute commands
126 | below in a thread safe manner.
127 | '''
128 | ack = self.recv(self.timeout)
129 | return ack == 'OK'
130 |
131 | def recv_int(self):
132 | ''' This command should not be used on its own: it is called by the execute commands
133 | below in a thread safe manner.
134 | '''
135 | value = self.recv(self.timeout)
136 | try:
137 | return int(value)
138 | except:
139 | return None
140 |
141 | def recv_array(self):
142 | ''' This command should not be used on its own: it is called by the execute commands
143 | below in a thread safe manner.
144 | '''
145 |
146 | try:
147 | values = self.recv(self.timeout * self.N_ANALOG_PORTS).split()
148 | return map(int, values)
149 | except:
150 | return []
151 |
152 | def execute(self, cmd):
153 | ''' Thread safe execution of "cmd" on the Arduino returning a single integer value.
154 | '''
155 | self.mutex.acquire()
156 |
157 | try:
158 | self.port.flushInput()
159 | except:
160 | pass
161 |
162 | ntries = 3
163 | attempts = 0
164 | command=(cmd +'\r').encode()
165 | print(command)
166 | try:
167 | self.port.write(command)
168 | value = self.recv(self.timeout)
169 | while attempts < ntries and (value == '' or value == 'Invalid Command' or value == None):
170 | try:
171 | self.port.flushInput()
172 | self.port.write(command)
173 | value = self.recv(self.timeout)
174 | except:
175 | print("Exception executing command: " + cmd)
176 | attempts += 1
177 | except:
178 | self.mutex.release()
179 | print("Exception executing command: " + cmd)
180 | value = None
181 |
182 | self.mutex.release()
183 | return int(value)
184 |
185 | def execute_array(self, cmd):
186 | ''' Thread safe execution of "cmd" on the Arduino returning an array.
187 | '''
188 | self.mutex.acquire()
189 |
190 | try:
191 | self.port.flushInput()
192 | except:
193 | pass
194 |
195 | ntries = 1
196 | attempts = 0
197 |
198 | try:
199 | self.port.write(cmd.encode() + b'\r')
200 | values = self.recv_array()
201 | while attempts < ntries and (values == '' or values == 'Invalid Command' or values == [] or values == None):
202 | try:
203 | self.port.flushInput()
204 | self.port.write(cmd.encode()+ b'\r')
205 | values = self.recv_array()
206 | except:
207 | print("Exception executing command: " + cmd)
208 | attempts += 1
209 | except:
210 | self.mutex.release()
211 | print("Exception executing command: " + cmd)
212 | raise SerialException
213 | return []
214 |
215 | try:
216 | values = map(int, values)
217 | except:
218 | values = []
219 |
220 | self.mutex.release()
221 | return values
222 |
223 | def execute_ack(self, cmd):
224 | ''' Thread safe execution of "cmd" on the Arduino returning True if response is ACK.
225 | '''
226 | self.mutex.acquire()
227 |
228 | try:
229 | self.port.flushInput()
230 | except:
231 | pass
232 |
233 | ntries = 1
234 | attempts = 0
235 |
236 | try:
237 | self.port.write(cmd.encode() + b'\r')
238 | ack = self.recv(self.timeout)
239 | while attempts < ntries and (ack == '' or ack == 'Invalid Command' or ack == None):
240 | try:
241 | self.port.flushInput()
242 | self.port.write(cmd.encode() + b'\r')
243 | ack = self.recv(self.timeout)
244 | except:
245 | print("Exception executing command: " + cmd)
246 | attempts += 1
247 | except:
248 | self.mutex.release()
249 | print("execute_ack exception when executing", cmd)
250 | print(sys.exc_info())
251 | return 0
252 |
253 | self.mutex.release()
254 | return ack == 'OK'
255 |
256 | def update_pid(self, Kp, Kd, Ki, Ko):
257 | ''' Set the PID parameters on the Arduino
258 | '''
259 | print("Updating PID parameters")
260 | cmd = 'u ' + str(Kp) + ':' + str(Kd) + ':' + str(Ki) + ':' + str(Ko)
261 | self.execute_ack(cmd)
262 |
263 | def get_baud(self):
264 | ''' Get the current baud rate on the serial port.
265 | '''
266 | try:
267 | return int(self.execute('b'));
268 | except:
269 | return None
270 |
271 | def get_encoder_counts(self):
272 | values = list(self.execute_array('e'))
273 | if len(values) != 2:
274 | print("Encoder count was not 2")
275 | raise SerialException
276 | return None
277 | else:
278 | if self.motors_reversed:
279 | values[0], values[1] = -values[0], -values[1]
280 | return values
281 |
282 | def reset_encoders(self):
283 | ''' Reset the encoder counts to 0
284 | '''
285 | return self.execute_ack('r')
286 |
287 | def drive(self, right, left):
288 | ''' Speeds are given in encoder ticks per PID interval
289 | '''
290 | if self.motors_reversed:
291 | left, right = -left, -right
292 | return self.execute_ack('m %d %d' %(right, left))
293 |
294 | def drive_m_per_s(self, right, left):
295 | ''' Set the motor speeds in meters per second.
296 | '''
297 | left_revs_per_second = float(left) / (self.wheel_diameter * PI)
298 | right_revs_per_second = float(right) / (self.wheel_diameter * PI)
299 |
300 | left_ticks_per_loop = int(left_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
301 | right_ticks_per_loop = int(right_revs_per_second * self.encoder_resolution * self.PID_INTERVAL * self.gear_reduction)
302 |
303 | self.drive(right_ticks_per_loop , left_ticks_per_loop )
304 |
305 | def stop(self):
306 | ''' Stop both motors.
307 | '''
308 | self.drive(0, 0)
309 |
310 | def analog_read(self, pin):
311 | return self.execute('a %d' %pin)
312 |
313 | def analog_write(self, pin, value):
314 | return self.execute_ack('x %d %d' %(pin, value))
315 |
316 | def digital_read(self, pin):
317 | return self.execute('d %d' %pin)
318 |
319 | def digital_write(self, pin, value):
320 | return self.execute_ack('w %d %d' %(pin, value))
321 |
322 | def pin_mode(self, pin, mode):
323 | return self.execute_ack('c %d %d' %(pin, mode))
324 |
325 | def servo_write(self, id, pos):
326 | ''' Usage: servo_write(id, pos)
327 | Position is given in radians and converted to degrees before sending
328 | '''
329 | return self.execute_ack('s %d %d' %(id, min(SERVO_MAX, max(SERVO_MIN, degrees(pos)))))
330 |
331 | def servo_read(self, id):
332 | ''' Usage: servo_read(id)
333 | The returned position is converted from degrees to radians
334 | '''
335 | return radians(self.execute('t %d' %id))
336 |
337 | def ping(self, pin):
338 | ''' The srf05/Ping command queries an SRF05/Ping sonar sensor
339 | connected to the General Purpose I/O line pinId for a distance,
340 | and returns the range in cm. Sonar distance resolution is integer based.
341 | '''
342 | return self.execute('p %d' %pin);
343 |
344 | # def get_maxez1(self, triggerPin, outputPin):
345 | # ''' The maxez1 command queries a Maxbotix MaxSonar-EZ1 sonar
346 | # sensor connected to the General Purpose I/O lines, triggerPin, and
347 | # outputPin, for a distance, and returns it in Centimeters. NOTE: MAKE
348 | # SURE there's nothing directly in front of the MaxSonar-EZ1 upon
349 | # power up, otherwise it wont range correctly for object less than 6
350 | # inches away! The sensor reading defaults to use English units
351 | # (inches). The sensor distance resolution is integer based. Also, the
352 | # maxsonar trigger pin is RX, and the echo pin is PW.
353 | # '''
354 | # return self.execute('z %d %d' %(triggerPin, outputPin))
355 |
356 |
357 | """ Basic test for connectivity """
358 | if __name__ == "__main__":
359 | if os.name == "posix":
360 | portName = "/dev/ttyUSB0"
361 | else:
362 | portName = "COM43" # Windows style COM port.
363 |
364 | baudRate = 57600
365 |
366 | myArduino = Arduino(port=portName, baudrate=baudRate, timeout=0.5)
367 | myArduino.connect()
368 |
369 | print("Sleeping for 1 second...")
370 | time.sleep(1)
371 |
372 | print("Reading on analog port 0", myArduino.analog_read(0))
373 | print("Reading on digital port 0", myArduino.digital_read(0))
374 | print("Blinking the LED 3 times")
375 | for i in range(3):
376 | myArduino.digital_write(13, 1)
377 | time.sleep(1.0)
378 | #print "Current encoder counts", myArduino.encoders()
379 |
380 | print("Connection test successful.",)
381 |
382 | myArduino.stop()
383 | myArduino.close()
384 |
385 | print("Shutting down Arduino.")
386 |
--------------------------------------------------------------------------------
/ros_arduino_bridge_Python3/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_bridge_Python3/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, name="base_controllers"):
35 | self.arduino = arduino
36 | self.name = name
37 | self.base_frame = base_frame
38 | self.rate = float(rospy.get_param("~base_controller_rate", 10))
39 | self.timeout = rospy.get_param("~base_controller_timeout", 1.0)
40 | self.stopped = False
41 |
42 | pid_params = dict()
43 | pid_params['wheel_diameter'] = rospy.get_param("~wheel_diameter", "")
44 | pid_params['wheel_track'] = rospy.get_param("~wheel_track", "")
45 | pid_params['encoder_resolution'] = rospy.get_param("~encoder_resolution", "")
46 | pid_params['gear_reduction'] = rospy.get_param("~gear_reduction", 1.0)
47 | pid_params['Kp'] = rospy.get_param("~Kp", 20)
48 | pid_params['Kd'] = rospy.get_param("~Kd", 12)
49 | pid_params['Ki'] = rospy.get_param("~Ki", 0)
50 | pid_params['Ko'] = rospy.get_param("~Ko", 50)
51 |
52 | self.accel_limit = rospy.get_param('~accel_limit', 0.1)
53 | self.motors_reversed = rospy.get_param("~motors_reversed", False)
54 |
55 | # Set up PID parameters and check for missing values
56 | self.setup_pid(pid_params)
57 |
58 | # How many encoder ticks are there per meter?
59 | self.ticks_per_meter = self.encoder_resolution * self.gear_reduction / (self.wheel_diameter * pi)
60 |
61 | # What is the maximum acceleration we will tolerate when changing wheel speeds?
62 | self.max_accel = self.accel_limit * self.ticks_per_meter / self.rate
63 |
64 | # Track how often we get a bad encoder count (if any)
65 | self.bad_encoder_count = 0
66 |
67 | now = rospy.Time.now()
68 | self.then = now # time for determining dx/dy
69 | self.t_delta = rospy.Duration(1.0 / self.rate)
70 | self.t_next = now + self.t_delta
71 |
72 | # Internal data
73 | self.enc_left = None # encoder readings
74 | self.enc_right = None
75 | self.x = 0 # position in xy plane
76 | self.y = 0
77 | self.th = 0 # rotation in radians
78 | self.v_left = 0
79 | self.v_right = 0
80 | self.v_des_left = 0 # cmd_vel setpoint
81 | self.v_des_right = 0
82 | self.last_cmd_vel = now
83 |
84 | # Subscriptions
85 | rospy.Subscriber("cmd_vel", Twist, self.cmdVelCallback)
86 |
87 | # Clear any old odometry info
88 | self.arduino.reset_encoders()
89 |
90 | # Set up the odometry broadcaster
91 | self.odomPub = rospy.Publisher('odom', Odometry, queue_size=5)
92 | self.odomBroadcaster = TransformBroadcaster()
93 |
94 | rospy.loginfo("Started base controller for a base of " + str(self.wheel_track) + "m wide with " + str(self.encoder_resolution) + " ticks per rev")
95 | rospy.loginfo("Publishing odometry data at: " + str(self.rate) + " Hz using " + str(self.base_frame) + " as base frame")
96 |
97 | def setup_pid(self, pid_params):
98 | # Check to see if any PID parameters are missing
99 | missing_params = False
100 | for param in pid_params:
101 | if pid_params[param] == "":
102 | print("*** PID Parameter " + param + " is missing. ***")
103 | missing_params = True
104 |
105 | if missing_params:
106 | os._exit(1)
107 |
108 | self.wheel_diameter = pid_params['wheel_diameter']
109 | self.wheel_track = pid_params['wheel_track']
110 | self.encoder_resolution = pid_params['encoder_resolution']
111 | self.gear_reduction = pid_params['gear_reduction']
112 |
113 | self.Kp = pid_params['Kp']
114 | self.Kd = pid_params['Kd']
115 | self.Ki = pid_params['Ki']
116 | self.Ko = pid_params['Ko']
117 |
118 | self.arduino.update_pid(self.Kp, self.Kd, self.Ki, self.Ko)
119 |
120 | def poll(self):
121 | now = rospy.Time.now()
122 | if now > self.t_next:
123 | # Read the encoders
124 | left_enc, right_enc = self.arduino.get_encoder_counts()
125 | try:
126 | left_enc, right_enc = self.arduino.get_encoder_counts()
127 | except:
128 | self.bad_encoder_count += 1
129 | rospy.logerr("Encoder exception count: " + str(self.bad_encoder_count))
130 | return
131 |
132 | dt = now - self.then
133 | self.then = now
134 | dt = dt.to_sec()
135 |
136 | # Calculate odometry
137 | if self.enc_left == None:
138 | dright = 0
139 | dleft = 0
140 | else:
141 | dright = (right_enc - self.enc_right) / self.ticks_per_meter
142 | dleft = (left_enc - self.enc_left) / self.ticks_per_meter
143 |
144 | self.enc_right = right_enc
145 | self.enc_left = left_enc
146 |
147 | dxy_ave = (dright + dleft) / 2.0
148 | dth = (dright - dleft) / self.wheel_track
149 | vxy = dxy_ave / dt
150 | vth = dth / dt
151 |
152 | if (dxy_ave != 0):
153 | dx = cos(dth) * dxy_ave
154 | dy = -sin(dth) * dxy_ave
155 | self.x += (cos(self.th) * dx - sin(self.th) * dy)
156 | self.y += (sin(self.th) * dx + cos(self.th) * dy)
157 |
158 | if (dth != 0):
159 | self.th += dth
160 |
161 | quaternion = Quaternion()
162 | quaternion.x = 0.0
163 | quaternion.y = 0.0
164 | quaternion.z = sin(self.th / 2.0)
165 | quaternion.w = cos(self.th / 2.0)
166 |
167 | # Create the odometry transform frame broadcaster.
168 | self.odomBroadcaster.sendTransform(
169 | (self.x, self.y, 0),
170 | (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
171 | rospy.Time.now(),
172 | self.base_frame,
173 | "odom"
174 | )
175 |
176 | odom = Odometry()
177 | odom.header.frame_id = "odom"
178 | odom.child_frame_id = self.base_frame
179 | odom.header.stamp = now
180 | odom.pose.pose.position.x = self.x
181 | odom.pose.pose.position.y = self.y
182 | odom.pose.pose.position.z = 0
183 | odom.pose.pose.orientation = quaternion
184 | odom.twist.twist.linear.x = vxy
185 | odom.twist.twist.linear.y = 0
186 | odom.twist.twist.angular.z = vth
187 |
188 | self.odomPub.publish(odom)
189 |
190 | if now > (self.last_cmd_vel + rospy.Duration(self.timeout)):
191 | self.v_des_left = 0
192 | self.v_des_right = 0
193 |
194 | if self.v_left < self.v_des_left:
195 | self.v_left += self.max_accel
196 | if self.v_left > self.v_des_left:
197 | self.v_left = self.v_des_left
198 | else:
199 | self.v_left -= self.max_accel
200 | if self.v_left < self.v_des_left:
201 | self.v_left = self.v_des_left
202 |
203 | if self.v_right < self.v_des_right:
204 | self.v_right += self.max_accel
205 | if self.v_right > self.v_des_right:
206 | self.v_right = self.v_des_right
207 | else:
208 | self.v_right -= self.max_accel
209 | if self.v_right < self.v_des_right:
210 | self.v_right = self.v_des_right
211 |
212 | # Set motor speeds in encoder ticks per PID loop
213 | if not self.stopped:
214 | self.arduino.drive(self.v_left, self.v_right)
215 |
216 | self.t_next = now + self.t_delta
217 |
218 | def stop(self):
219 | self.stopped = True
220 | self.arduino.drive(0, 0)
221 |
222 | def cmdVelCallback(self, req):
223 | # Handle velocity-based movement requests
224 | self.last_cmd_vel = rospy.Time.now()
225 |
226 | x = req.linear.x # m/s
227 | th = req.angular.z # rad/s
228 |
229 | if x == 0:
230 | # Turn in place
231 | right = th * self.wheel_track * self.gear_reduction / 2.0
232 | left = -right
233 | elif th == 0:
234 | # Pure forward/backward motion
235 | left = right = x
236 | else:
237 | # Rotation about a point in space
238 | left = x - th * self.wheel_track * self.gear_reduction / 2.0
239 | right = x + th * self.wheel_track * self.gear_reduction / 2.0
240 |
241 | self.v_des_left = int(left * self.ticks_per_meter / self.arduino.PID_RATE)
242 | self.v_des_right = int(right * self.ticks_per_meter / self.arduino.PID_RATE)
243 |
244 |
245 |
246 |
247 |
248 |
249 |
250 |
--------------------------------------------------------------------------------
/ros小车.jpeg:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/LowPower-Center/ROS_Arduino_bridge/1a422db3e526f43bf0dbbe3f033570f97ef95e28/ros小车.jpeg
--------------------------------------------------------------------------------