Inspection robot
(→PCB Layout) |
(→Servomotor) |
||
(98 intermediate revisions by 2 users not shown) | |||
Line 9: | Line 9: | ||
==Mobotware on a Beaglebone Black== | ==Mobotware on a Beaglebone Black== | ||
− | Installation - ask Søren Hansen in 326/014 or Jens Christian Andersen in 326/ | + | Installation - ask Søren Hansen in 326/014 or Jens Christian Andersen in 326/008. |
A small guide has been made, describing how to connect to the Beaglebone Black, how to mount it's file system, change I<sup>2</sup>C frequency and how to run RHD and RHDtest. The guide can be downloaded here: [http://aut.elektro.dtu.dk/staff/sh/rsewiki/BBB_guide_101v3.pdf BBB_guide_101v3.pdf] | A small guide has been made, describing how to connect to the Beaglebone Black, how to mount it's file system, change I<sup>2</sup>C frequency and how to run RHD and RHDtest. The guide can be downloaded here: [http://aut.elektro.dtu.dk/staff/sh/rsewiki/BBB_guide_101v3.pdf BBB_guide_101v3.pdf] | ||
=== RHD Plugin === | === RHD Plugin === | ||
+ | |||
+ | ===== Features ===== | ||
+ | |||
+ | The plugin is configured by modifying the XML-file: rhdconfig.xml | ||
+ | |||
+ | * Setting of PID-controller gains from rhdconfig.xml | ||
+ | |||
+ | * Setting of position limits from rhdconfig.xml | ||
+ | |||
+ | * Scaling of position into degrees or radians in rhdconfig.xml | ||
+ | |||
+ | * Offsetting of position in rhdconfig.xml | ||
+ | |||
+ | * Changing of servo I<sup>2</sup>C address (range: 1-255) | ||
+ | |||
+ | * Automatically saving of gains, I<sup>2</sup>C address and position limits to servo EEPROM upon startup | ||
+ | |||
+ | * Keeps working if connection to one or more servomotors are lost | ||
+ | |||
+ | * Supports up to 24 servomotors | ||
+ | |||
+ | * Debug mode | ||
===== Starting ===== | ===== Starting ===== | ||
Line 23: | Line 45: | ||
Then navigate to the appropriate plugin folder. The exact path may vary depending on the installation. Most common paths are: | Then navigate to the appropriate plugin folder. The exact path may vary depending on the installation. Most common paths are: | ||
− | |||
rhd/plugins/6legged/ | rhd/plugins/6legged/ | ||
− | |||
rhd/trunk/plugins/6legged/ | rhd/trunk/plugins/6legged/ | ||
− | Then, the [[RHD]] | + | This folder contains the configuration file ''rhdconfig.xml'' which is used for configuring the plugin parameters such as sampling period, declaration of servomotors and their parameters. |
+ | Then, from the folder containing ''rhdconfig.xml'', the [[RHD]] is started and will load the plugin which will initiate I<sup>2</sup>C communication with the servo motor. To see/set variables, RHDtest can be started in another terminal window. | ||
'''NOTE:''' The RHD plugin needs to be compiled before using it for the first time after installation of Mobotware. This is done by navigating to the appropriate folder above, and writing the command "Make". | '''NOTE:''' The RHD plugin needs to be compiled before using it for the first time after installation of Mobotware. This is done by navigating to the appropriate folder above, and writing the command "Make". | ||
− | + | '''NOTE:''' When starting the RHD, controller gains and position limits specified in ''rhdconfig.xml'' are sent to the servomotor and saved to the onboard EEPROM. Thus if the servomotor experiences a power loss, these values will still be set when power returns (because controller gains and position limits are only sent to the servomotor then starting the RHD). | |
− | + | ===== rhdconfig.xml ===== | |
− | + | ||
+ | Servomotors are declared using the following XML syntax: | ||
+ | |||
+ | <servo id="5" minpos="100" maxpos="900" p="50" i="0" d="0" scale="1.0" offset="0" name="servo_name"/> | ||
+ | |||
+ | Where: | ||
+ | * ''id'' is the I<sup>2</sup>C address of the declared servomotor. | ||
+ | * ''minpos'' and ''maxpos'' are the position limits in A/D units, and are unaffected by the scaling factor. | ||
+ | * ''p'', ''i'', ''d'' are the controller gains. | ||
+ | * ''scale'' is the scaling factor used for scaling between A/D values and radians or degrees (applies only to the position and reference). ''See [[Inspection robot#Operation|Operation]] section. | ||
+ | *''offset'' is the position offset in A/D units and is unaffected by the scaling factor. | ||
+ | * ''name'' is a string where a user friendly name can be set. | ||
+ | |||
+ | More servomotors can be declared by adding more declarations. Remember to check that the right addresses are specified in the ''id'' field. | ||
+ | |||
+ | Debug mode is enabled / disabled by writing ''1/0'' in the ''debug'' field of the I<sup>2</sup>C tag: | ||
+ | |||
+ | <i2c | ||
+ | dev="/dev/i2c-1" | ||
+ | debug="1" | ||
+ | r2sScale="1" | ||
+ | r2sOffset="0" | ||
+ | /> | ||
+ | |||
+ | Logging of data is disabled when ''interval=0'', set to ''"1"'' for logging at full update rate: | ||
+ | |||
+ | <rhdlog | ||
+ | enable="true" | ||
+ | lib="rhdlog.so.1" | ||
+ | debug="true" | ||
+ | interval="0" | ||
+ | /> | ||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
− | |||
==Servomotor== | ==Servomotor== | ||
+ | |||
+ | [[Image:Hs646wp_mod.jpg|thumb|HS646WP servomotor with modified data cable]] | ||
===Hardware=== | ===Hardware=== | ||
− | The Hitec HS646-WP electric servo motor are used for the inspection robot. See Hitec's website for [http://hitecrcd.com/products/servos/waterproof-servos-2/hs-646wp-high-voltage-high-torque-analog-waterproof-servo/product specifications]. The servo does not provide feedback by default, and is therefore augmented with the OpenServo project's PCB and software/firmware. The PCB and firmware are modified to fit the purpose of the inspection robot. | + | The Hitec HS646-WP electric servo motor are used for the inspection robot. See Hitec's website for [http://hitecrcd.com/products/servos/waterproof-servos-2/hs-646wp-high-voltage-high-torque-analog-waterproof-servo/product specifications]. The servo does not provide feedback by default, and is therefore augmented with the OpenServo project's PCB and software/firmware. The PCB and firmware are modified to fit the purpose of the inspection robot, and utilizes an ATmega168 MCU ([http://www.atmel.com/images/Atmel-8271-8-bit-AVR-Microcontroller-ATmega48A-48PA-88A-88PA-168A-168PA-328-328P_datasheet_Complete.pdf datasheet, 34 MB PDF]). |
===== Potentiometer ===== | ===== Potentiometer ===== | ||
Mounting: Green towards motor, yellow in center and red away from motor, or maybe more likely, motor is turned 180 deg. | Mounting: Green towards motor, yellow in center and red away from motor, or maybe more likely, motor is turned 180 deg. | ||
− | NOTE: The A/D converter of the ATmega168 MCU is 10-bits, giving potentiometer readouts between 0-1023. | + | NOTE: The A/D converter of the ATmega168 MCU is 10-bits, giving potentiometer readouts between 0-1023. However, in practice the position range of the servomotor will typically be 20-980 (see [[Inspection robot#Operation|Operation]] section). |
If the potentiometer or motor terminals have been inverted, this can be compensated for by changing the macro SWAP_PWM_DIRECTION_ENABLED in the firmware source file config.h. This should only be a problem if new servomotors are augmented with the OpenServo PCB and the installer are not cautious. | If the potentiometer or motor terminals have been inverted, this can be compensated for by changing the macro SWAP_PWM_DIRECTION_ENABLED in the firmware source file config.h. This should only be a problem if new servomotors are augmented with the OpenServo PCB and the installer are not cautious. | ||
Line 68: | Line 112: | ||
A diagram/schematic of the modified OpenServo PCB can be seen [[Media:openservo_schematic_rev2.png|here]] | A diagram/schematic of the modified OpenServo PCB can be seen [[Media:openservo_schematic_rev2.png|here]] | ||
− | The | + | The modification to the standard OpenServo PCB consists of moving two capacitors to make room for the motor (visible on side A). |
<gallery> | <gallery> | ||
Line 77: | Line 121: | ||
===Firmware=== | ===Firmware=== | ||
− | |||
− | |||
− | |||
=====Features===== | =====Features===== | ||
As of August 2015, the servomotor firmware for the modified OpenServo PCB supports the following: | As of August 2015, the servomotor firmware for the modified OpenServo PCB supports the following: | ||
− | |||
− | |||
*Sampling of Back-EMF (only up to duty-cycle of 76%, due to discharge time of armature coil). | *Sampling of Back-EMF (only up to duty-cycle of 76%, due to discharge time of armature coil). | ||
Line 90: | Line 129: | ||
*Sampling of voltage, position and armature current. | *Sampling of voltage, position and armature current. | ||
− | *PID control (The integration is untested). | + | *PID control (The integration part is untested). |
+ | |||
+ | *500 Hz PWM frequency. | ||
+ | |||
+ | *500 Hz internal sampling rate (once per PWM cycle). | ||
+ | |||
+ | *Up to 400 kHz I<sup>2</sup>C bus frequency (Max. supported by the ATmega168 MCU). | ||
+ | |||
+ | * '''NOTE:''' Default I<sup>2</sup>C address is ''5'' (0x05) | ||
+ | |||
+ | * '''NOTE:''' Requires the August 2015 version of the RHD plugin to work. Earlier versions of the firmware are not compatible with this version of the RHD plugin, due to the placement and usage of MCU registers. Using earlier versions of the firmware will cause the PWM frequency divider registers to be overwritten with the wrong values! | ||
+ | |||
+ | =====Bugs and Issues===== | ||
+ | The February 2015 version of the firmware contains a bug that causes the firmware to crash if the PWM duty cycle becomes larger than 76% (196/255). When the firmware crashes, communication and control are lost and the servomotor will spin out of control. It is highly recommended to test if a servomotor has this bug, before use, as the gear train might be damaged! This is tested by removing a gear from the gear-train of the servomotor and then demand a large PWM duty cycle (a P-gain of 150 and error of about 500 achieves this). If the error is present, update the servo firmware to the August 2015 version (version can be seen in main.c). | ||
+ | |||
+ | |||
+ | Another bug in the servo firmware, is the inclusion of register 0x2F in the register range that is saved to the onboard EEPROM. Because register 0x2F is used as a communication timeout register, the value of this register changes very frequently. This implicates that in the time between a checksum has been created and register values are written to EEPROM, the value of this register has most likely changed. Thus, when restoring register values from EEPROM, the checksum will rarely match the stored data resulting in default register values being loaded. This causes the servomotor's I<sup>2</sup>C address to be reset to the default value of ''5'' (0x05). This bug was fixed in the February 2015 version. | ||
+ | |||
+ | |||
+ | A minor issue is loss of data during communication. In the February 2015 version, when manually setting controller gains and operating with an RHD time period of less than 4.0 ms, loss of I<sup>2</sup>C communication data would occur. As of the August 2015 version of the firmware, where controller gains are set automatically from ''rhdconfig.xml'' this issue seems to have minimized itself. When operating with an RHD time period of 2.0 ms and 1.0 ms, loss of I<sup>2</sup>C communication data rarely occurs. However, this phenomenon needs further investigation. | ||
− | + | =====Needs Investigation===== | |
− | + | Some features needs further testing/investigation to validate their functionality or finding errors: | |
− | + | *Effects of high D-gain. | |
*Integration part of build-in PID-controller. | *Integration part of build-in PID-controller. | ||
+ | *Anti-windup limits of integrator. | ||
+ | |||
+ | *On rare occasions, data loss when operating with an RHD time period of less than 4.0 ms. | ||
+ | |||
+ | *Write protection of registers 0x20 through 0x2E does not seem work. Perhaps not a necessary feature? | ||
+ | |||
+ | *Why servomotor moves to upper position limit (when narrower than full range, 0-1023), when being below the lower position limit on power up (when above the upper position limit on power up, it also moves to the upper position limit). | ||
===Programming of Servomotors=== | ===Programming of Servomotors=== | ||
− | In order to update the firmware of the servomotors, the backside is opened by unscrewing the four philips-head screws and removing the back panel. The OpenServo PCB should be visible with side B facing out. The servomotor is then CAREFULLY placed in the created programming stand. There is only one way to orient the servomotor correctly in the programming stand! When oriented correctly, the pins of the programmer will connect to terminals: MISO1, MOSI1, SCK1, RESET-P and GND-P. | + | In order to update the firmware of the servomotors, the backside is opened by unscrewing the four philips-head screws and removing the back panel. The OpenServo PCB should be visible with side B facing out. The servomotor is then CAREFULLY placed in the created programming stand. There is only one way to orient the servomotor correctly in the programming stand! When oriented correctly, the pins of the programmer will connect to terminals: MISO1, MOSI1, SCK1, RESET-P and GND-P of the modified OpenServo PCB (see diagram). |
+ | |||
+ | The servomotor is then programmed by connecting the programming stand to the ISP port of an STK500/600 programmer and writing 'Make install' while in the directory where the servo firmware source files are located (requires AVRdude to be installed). | ||
'''NOTE:''' The servomotor needs external power while being programmed. | '''NOTE:''' The servomotor needs external power while being programmed. | ||
+ | |||
+ | '''NOTE:''' The file ''Makefile'' needs to be configured according to the programmer used (STK500/600), by commenting/uncommenting the apprpriate lines at the end of the ''Makefile''. | ||
+ | |||
+ | [[File:prog_stand.jpg | thumb | Programming stand.]] | ||
==Operation== | ==Operation== | ||
===Worth Knowing=== | ===Worth Knowing=== | ||
+ | |||
+ | [[File:connector_pinmap.png | thumb | left | Servomotor connector pin assignments.]] | ||
+ | |||
+ | * '''NOTE:''' Make sure that servomotor firmware is updated to the August 2015 version! | ||
+ | |||
+ | * '''NOTE:''' Even though the position range theoretically is 0-1023, in practice it is not! Due to the internal end-stops on the output axle, the full motion range is about 20-980 (one servo had a range of 20-973, another 15-988). Beware of overshooting when operating near the limits. | ||
+ | |||
+ | *The full range of the servo motor is approximately 207° ≈ 3.6128 rad. | ||
+ | |||
+ | *in ''rhdconfig.xml'', PID gains are set, along with the servomotor I<sup>2</sup>C address, motion range limits, scaling factor, offset and name (the I<sup>2</sup>C address of the servomotor is not set to the value in the 'id' field, it only specifies which address to communicate with). | ||
*A P-gain is needed to make the servo run. A value of about 50 is a good starting point. | *A P-gain is needed to make the servo run. A value of about 50 is a good starting point. | ||
*A D-gain does not affect the precision/overshooting. See the "Controller Characteristics" section for test results. | *A D-gain does not affect the precision/overshooting. See the "Controller Characteristics" section for test results. | ||
+ | |||
+ | *For using milli-radians as the unit of angle in the RHD, a scaling factor α<sub>pos,mrad</sub> = 3.7909 should be applied as the scaling parameter in ''rhdconfig.xml''. For milli-degrees, use α<sub>pos,mdeg</sub> = 217.2088. Using a scaling factor of unity means using A/D values in the RHD. | ||
+ | |||
+ | *The ''offset'' parameter in ''rhdconfig.xml'' is used for offsetting the zero position. | ||
*Internal sampling of Back-EMF, voltage, position and current runs at 500 Hz (once per PWM cycle). | *Internal sampling of Back-EMF, voltage, position and current runs at 500 Hz (once per PWM cycle). | ||
− | * | + | *Obtainable external sampling rates with 400 kHz I<sup>2</sup>C bus frequency are approx. 259-312 Hz with four servomotors (a single robot leg) and approx. 41-43 Hz with 24 servomotors (six legs of four servos). |
+ | |||
+ | *If the servomotor is powered up while outside of the defined position range (if the position range is narrower than full range), it will automatically move to the ''upper'' position limit. If the servomotor is powered up while being within the position range, it will remain stationary until a reference is given. | ||
+ | |||
+ | *Consider deleting generated logfiles, as they take up space and can prevent the Beaglebone Black from booting, if storage becomes full. Logging can be enabled / disabled in ''rhdconfig.xml'' under the ''<rhdlog>'' tag. | ||
+ | |||
+ | ===Commands=== | ||
+ | |||
+ | By writing a command number to the ''scmd'' variable in RHDtest, the command is sent to the servomotor which performs special functions. The commands are listed in the table below. When written to ''scmd'' the format should be decimal. | ||
+ | |||
+ | {| class="wikitable" | ||
+ | |- | ||
+ | ! colspan=2| Cmd. No. !! Action | ||
+ | |- | ||
+ | | 0x80 || 128 || Restart the microcontroller | ||
+ | |- | ||
+ | | 0x81 || 129 || Write simple checksum to read/write registers | ||
+ | |- | ||
+ | | 0x82 || 130 || Enable PWM to motor | ||
+ | |- | ||
+ | | 0x83 || 131 || Disable PWM to motor | ||
+ | |- | ||
+ | | 0x84 || 132 || Enable write of read/write protected registers | ||
+ | |- | ||
+ | | 0x85 || 133 || Disable write of read/write protected registers | ||
+ | |- | ||
+ | | 0x86 || 134 || Save read/write protected registers to EEPROM | ||
+ | |- | ||
+ | | 0x87 || 135 || Load read/write protected registers from EEPROM | ||
+ | |- | ||
+ | | 0x88 || 136 || Restore read/write protected registers from EEPROM | ||
+ | |- | ||
+ | | 0x89 || 137 || Change servo I2C address to ''saddr'' | ||
+ | |- | ||
+ | |} | ||
+ | |||
+ | The registers saved to EEPROM are in the range 0x20 through 0x2E, which are the registers containing the I<sup>2</sup>C address, controller gains, PWM frequency divider and minimum/maximum position range of the servo (0-1023 is set by default). See ''registers.h'' for register assignments. | ||
+ | |||
+ | ===== Changing Servomotor I<sup>2</sup>C Address ===== | ||
+ | |||
+ | The I<sup>2</sup>C address of a servomotor can be changed by writing the desired address to the variable ''saddr'' in RHDtest and then writing command ''0x89'' to the variable ''scmd'' in RHDtest. When this is done, the servomotor MCU restarts and communication will be lost. To regain communication, the servomotor I<sup>2</sup>C address needs to be changed in ''rhdconfig.xml'' (the ID field) to the new value and the RHD restarted. If the I<sup>2</sup>C address is forgotten, the servomotor needs to be reprogrammed (the default address is 5 (0x05)). | ||
===Controller Characteristics=== | ===Controller Characteristics=== | ||
− | Tests have been conducted with regards to determining the precision of the servomotor depending on the PD-controller gains. The table below shows the results of different step responses. | + | Tests have been conducted with regards to determining the precision of the servomotor depending on the PD-controller gains. The table below shows the results of different step responses. The servomotor is tested while standing on its own without any load. |
+ | The steady-state position error e<sub>ss</sub>, and error e (the step size), are in A/D units. | ||
{| class="wikitable" | {| class="wikitable" | ||
Line 129: | Line 255: | ||
! D = 0 !! D = 50 !! D = 100 !! D = 150 | ! D = 0 !! D = 50 !! D = 100 !! D = 150 | ||
|- | |- | ||
− | | | + | | e<sub>ss</sub> = 12 || e<sub>ss</sub> = 15 || e<sub>ss</sub> = 15 || e<sub>ss</sub> = 15 |
|- | |- | ||
|} | |} | ||
− | In order to avoid overshooting, small steps have been performed with large P-gains. The results can be seen in the table below. | + | From these results it is seen that the D-gain has a very limited impact on the controller performance. |
+ | |||
+ | In order to avoid overshooting too much, small steps have been performed with large P-gains. The idea is to obtain a precise movement for a large step by making multiple small ones. The results can be seen in the table below. The steady-state position error e<sub>ss</sub>, and error e (the step size), are in A/D units (approximate). | ||
{| class="wikitable" | {| class="wikitable" | ||
Line 141: | Line 269: | ||
! e = 25 !! e = 50 !! e = 75 !! e = 100 !! e = 200 !! e = 500 | ! e = 25 !! e = 50 !! e = 75 !! e = 100 !! e = 200 !! e = 500 | ||
|- | |- | ||
− | | | + | | e<sub>ss</sub> = 7 || e<sub>ss</sub> = 2 || e<sub>ss</sub> = 6 || e<sub>ss</sub> = 5 || e<sub>ss</sub> = 6 || e<sub>ss</sub> = 9 |
|- | |- | ||
|} | |} | ||
− | == | + | {| class="wikitable" |
+ | |- | ||
+ | ! colspan="6"|P = 400, D = 50 | ||
+ | |- | ||
+ | ! e = 25 !! e = 50 !! e = 75 !! e = 100 !! e = 200 !! e = 500 | ||
+ | |- | ||
+ | | e<sub>ss</sub> = 7 || e<sub>ss</sub> = 2 || e<sub>ss</sub> = 7 || e<sub>ss</sub> = 8 || e<sub>ss</sub> = 7 || e<sub>ss</sub> = 8 | ||
+ | |- | ||
+ | |} | ||
+ | |||
+ | {| class="wikitable" | ||
+ | |- | ||
+ | ! colspan="6"|P = 400, D = 100 | ||
+ | |- | ||
+ | ! e = 25 !! e = 50 !! e = 75 !! e = 100 !! e = 200 !! e = 500 | ||
+ | |- | ||
+ | | e<sub>ss</sub> = 8 || e<sub>ss</sub> = 1 || e<sub>ss</sub> = 6 || e<sub>ss</sub> = 3 || e<sub>ss</sub> = 5 || e<sub>ss</sub> = 9 | ||
+ | |- | ||
+ | |} | ||
+ | |||
+ | These results are approximate as the steady state error varies a little between steps. From these results it can be seen that with a P-gain of 400 and a step-size of 50, good results are obtained (i.e. a small steady-state error). Again it is seen that the effect of the D-gain is negligible. | ||
+ | |||
+ | It is noticed that steady-state errors tend to be larger when stepping on the high end of the potentiometer (i.e. steps 750-775, 800-850, 700-900 etc.). It should also be noticed that steady-state errors tend to be larger when moving from a higher position to a lower one (i.e. a step from 900 to 700 yields a larger steady-state error than moving from 700 to 900). | ||
+ | |||
+ | ==Further Reading== | ||
+ | |||
+ | For more information, the reader is encouraged to see: | ||
+ | |||
+ | * "Haptic Sensor Based Navigation for autonomous Legged Robot", Master's Thesis, DTU, Mark Paw Hansen, March 2015 | ||
− | + | * "Control of 6-legged Inspection Robot for Iron Constructions", Bachelor's Thesis, DTU, Jan L. Svendsen, December 2013 | |
− | + | * "Control and Design of Magnetic Foot for Inspection Robot", Bachelor's Thesis, DTU, Jesper A. Jørgensen, August 2014 | |
− | + | ||
− | + | * The [http://www.openservo.org OpenServo] website. | |
− | + | ||
− | + | ||
− | + | ||
− | + | * The short guide to using the Beaglebone Black [http://aut.elektro.dtu.dk/staff/sh/rsewiki/BBB_guide_101v3.pdf BBB_guide_101v3.pdf] |
Latest revision as of 15:15, 11 August 2015
Installation and use noted for 4/6-legged inspection robot.
Contents |
[edit] Mobotware on a Raspberry PI
Installation - see How-to guide on main page.
[edit] Mobotware on a Beaglebone Black
Installation - ask Søren Hansen in 326/014 or Jens Christian Andersen in 326/008.
A small guide has been made, describing how to connect to the Beaglebone Black, how to mount it's file system, change I2C frequency and how to run RHD and RHDtest. The guide can be downloaded here: BBB_guide_101v3.pdf
[edit] RHD Plugin
[edit] Features
The plugin is configured by modifying the XML-file: rhdconfig.xml
- Setting of PID-controller gains from rhdconfig.xml
- Setting of position limits from rhdconfig.xml
- Scaling of position into degrees or radians in rhdconfig.xml
- Offsetting of position in rhdconfig.xml
- Changing of servo I2C address (range: 1-255)
- Automatically saving of gains, I2C address and position limits to servo EEPROM upon startup
- Keeps working if connection to one or more servomotors are lost
- Supports up to 24 servomotors
- Debug mode
[edit] Starting
When Mobotware has been installed, log on to the Beaglebone Black using a terminal with the following command:
ssh root@<BBB-ip>
Then navigate to the appropriate plugin folder. The exact path may vary depending on the installation. Most common paths are:
rhd/plugins/6legged/ rhd/trunk/plugins/6legged/
This folder contains the configuration file rhdconfig.xml which is used for configuring the plugin parameters such as sampling period, declaration of servomotors and their parameters. Then, from the folder containing rhdconfig.xml, the RHD is started and will load the plugin which will initiate I2C communication with the servo motor. To see/set variables, RHDtest can be started in another terminal window.
NOTE: The RHD plugin needs to be compiled before using it for the first time after installation of Mobotware. This is done by navigating to the appropriate folder above, and writing the command "Make".
NOTE: When starting the RHD, controller gains and position limits specified in rhdconfig.xml are sent to the servomotor and saved to the onboard EEPROM. Thus if the servomotor experiences a power loss, these values will still be set when power returns (because controller gains and position limits are only sent to the servomotor then starting the RHD).
[edit] rhdconfig.xml
Servomotors are declared using the following XML syntax:
<servo id="5" minpos="100" maxpos="900" p="50" i="0" d="0" scale="1.0" offset="0" name="servo_name"/>
Where:
- id is the I2C address of the declared servomotor.
- minpos and maxpos are the position limits in A/D units, and are unaffected by the scaling factor.
- p, i, d are the controller gains.
- scale is the scaling factor used for scaling between A/D values and radians or degrees (applies only to the position and reference). See Operation section.
- offset is the position offset in A/D units and is unaffected by the scaling factor.
- name is a string where a user friendly name can be set.
More servomotors can be declared by adding more declarations. Remember to check that the right addresses are specified in the id field.
Debug mode is enabled / disabled by writing 1/0 in the debug field of the I2C tag:
<i2c dev="/dev/i2c-1" debug="1" r2sScale="1" r2sOffset="0" />
Logging of data is disabled when interval=0, set to "1" for logging at full update rate:
<rhdlog enable="true" lib="rhdlog.so.1" debug="true" interval="0" />
[edit] Servomotor
[edit] Hardware
The Hitec HS646-WP electric servo motor are used for the inspection robot. See Hitec's website for specifications. The servo does not provide feedback by default, and is therefore augmented with the OpenServo project's PCB and software/firmware. The PCB and firmware are modified to fit the purpose of the inspection robot, and utilizes an ATmega168 MCU (datasheet, 34 MB PDF).
[edit] Potentiometer
Mounting: Green towards motor, yellow in center and red away from motor, or maybe more likely, motor is turned 180 deg. NOTE: The A/D converter of the ATmega168 MCU is 10-bits, giving potentiometer readouts between 0-1023. However, in practice the position range of the servomotor will typically be 20-980 (see Operation section).
If the potentiometer or motor terminals have been inverted, this can be compensated for by changing the macro SWAP_PWM_DIRECTION_ENABLED in the firmware source file config.h. This should only be a problem if new servomotors are augmented with the OpenServo PCB and the installer are not cautious.
[edit] Power
The supply voltage should be at least 6.5V to allow the regulator to properly produce 5V for the MCU. The maximum supported voltage is around 18V (the maximum rating of the weakest components). Operating with 10-12V is considered sufficient.
[edit] PCB Layout
The layout of the modified OpenServo PCB with connections can be seen in the pictures below (front and backside). A diagram/schematic of the modified OpenServo PCB can be seen here
The modification to the standard OpenServo PCB consists of moving two capacitors to make room for the motor (visible on side A).
[edit] Firmware
[edit] Features
As of August 2015, the servomotor firmware for the modified OpenServo PCB supports the following:
- Sampling of Back-EMF (only up to duty-cycle of 76%, due to discharge time of armature coil).
- Sampling of voltage, position and armature current.
- PID control (The integration part is untested).
- 500 Hz PWM frequency.
- 500 Hz internal sampling rate (once per PWM cycle).
- Up to 400 kHz I2C bus frequency (Max. supported by the ATmega168 MCU).
- NOTE: Default I2C address is 5 (0x05)
- NOTE: Requires the August 2015 version of the RHD plugin to work. Earlier versions of the firmware are not compatible with this version of the RHD plugin, due to the placement and usage of MCU registers. Using earlier versions of the firmware will cause the PWM frequency divider registers to be overwritten with the wrong values!
[edit] Bugs and Issues
The February 2015 version of the firmware contains a bug that causes the firmware to crash if the PWM duty cycle becomes larger than 76% (196/255). When the firmware crashes, communication and control are lost and the servomotor will spin out of control. It is highly recommended to test if a servomotor has this bug, before use, as the gear train might be damaged! This is tested by removing a gear from the gear-train of the servomotor and then demand a large PWM duty cycle (a P-gain of 150 and error of about 500 achieves this). If the error is present, update the servo firmware to the August 2015 version (version can be seen in main.c).
Another bug in the servo firmware, is the inclusion of register 0x2F in the register range that is saved to the onboard EEPROM. Because register 0x2F is used as a communication timeout register, the value of this register changes very frequently. This implicates that in the time between a checksum has been created and register values are written to EEPROM, the value of this register has most likely changed. Thus, when restoring register values from EEPROM, the checksum will rarely match the stored data resulting in default register values being loaded. This causes the servomotor's I2C address to be reset to the default value of 5 (0x05). This bug was fixed in the February 2015 version.
A minor issue is loss of data during communication. In the February 2015 version, when manually setting controller gains and operating with an RHD time period of less than 4.0 ms, loss of I2C communication data would occur. As of the August 2015 version of the firmware, where controller gains are set automatically from rhdconfig.xml this issue seems to have minimized itself. When operating with an RHD time period of 2.0 ms and 1.0 ms, loss of I2C communication data rarely occurs. However, this phenomenon needs further investigation.
[edit] Needs Investigation
Some features needs further testing/investigation to validate their functionality or finding errors:
- Effects of high D-gain.
- Integration part of build-in PID-controller.
- Anti-windup limits of integrator.
- On rare occasions, data loss when operating with an RHD time period of less than 4.0 ms.
- Write protection of registers 0x20 through 0x2E does not seem work. Perhaps not a necessary feature?
- Why servomotor moves to upper position limit (when narrower than full range, 0-1023), when being below the lower position limit on power up (when above the upper position limit on power up, it also moves to the upper position limit).
[edit] Programming of Servomotors
In order to update the firmware of the servomotors, the backside is opened by unscrewing the four philips-head screws and removing the back panel. The OpenServo PCB should be visible with side B facing out. The servomotor is then CAREFULLY placed in the created programming stand. There is only one way to orient the servomotor correctly in the programming stand! When oriented correctly, the pins of the programmer will connect to terminals: MISO1, MOSI1, SCK1, RESET-P and GND-P of the modified OpenServo PCB (see diagram).
The servomotor is then programmed by connecting the programming stand to the ISP port of an STK500/600 programmer and writing 'Make install' while in the directory where the servo firmware source files are located (requires AVRdude to be installed).
NOTE: The servomotor needs external power while being programmed.
NOTE: The file Makefile needs to be configured according to the programmer used (STK500/600), by commenting/uncommenting the apprpriate lines at the end of the Makefile.
[edit] Operation
[edit] Worth Knowing
- NOTE: Make sure that servomotor firmware is updated to the August 2015 version!
- NOTE: Even though the position range theoretically is 0-1023, in practice it is not! Due to the internal end-stops on the output axle, the full motion range is about 20-980 (one servo had a range of 20-973, another 15-988). Beware of overshooting when operating near the limits.
- The full range of the servo motor is approximately 207° ≈ 3.6128 rad.
- in rhdconfig.xml, PID gains are set, along with the servomotor I2C address, motion range limits, scaling factor, offset and name (the I2C address of the servomotor is not set to the value in the 'id' field, it only specifies which address to communicate with).
- A P-gain is needed to make the servo run. A value of about 50 is a good starting point.
- A D-gain does not affect the precision/overshooting. See the "Controller Characteristics" section for test results.
- For using milli-radians as the unit of angle in the RHD, a scaling factor αpos,mrad = 3.7909 should be applied as the scaling parameter in rhdconfig.xml. For milli-degrees, use αpos,mdeg = 217.2088. Using a scaling factor of unity means using A/D values in the RHD.
- The offset parameter in rhdconfig.xml is used for offsetting the zero position.
- Internal sampling of Back-EMF, voltage, position and current runs at 500 Hz (once per PWM cycle).
- Obtainable external sampling rates with 400 kHz I2C bus frequency are approx. 259-312 Hz with four servomotors (a single robot leg) and approx. 41-43 Hz with 24 servomotors (six legs of four servos).
- If the servomotor is powered up while outside of the defined position range (if the position range is narrower than full range), it will automatically move to the upper position limit. If the servomotor is powered up while being within the position range, it will remain stationary until a reference is given.
- Consider deleting generated logfiles, as they take up space and can prevent the Beaglebone Black from booting, if storage becomes full. Logging can be enabled / disabled in rhdconfig.xml under the <rhdlog> tag.
[edit] Commands
By writing a command number to the scmd variable in RHDtest, the command is sent to the servomotor which performs special functions. The commands are listed in the table below. When written to scmd the format should be decimal.
Cmd. No. | Action | |
---|---|---|
0x80 | 128 | Restart the microcontroller |
0x81 | 129 | Write simple checksum to read/write registers |
0x82 | 130 | Enable PWM to motor |
0x83 | 131 | Disable PWM to motor |
0x84 | 132 | Enable write of read/write protected registers |
0x85 | 133 | Disable write of read/write protected registers |
0x86 | 134 | Save read/write protected registers to EEPROM |
0x87 | 135 | Load read/write protected registers from EEPROM |
0x88 | 136 | Restore read/write protected registers from EEPROM |
0x89 | 137 | Change servo I2C address to saddr |
The registers saved to EEPROM are in the range 0x20 through 0x2E, which are the registers containing the I2C address, controller gains, PWM frequency divider and minimum/maximum position range of the servo (0-1023 is set by default). See registers.h for register assignments.
[edit] Changing Servomotor I2C Address
The I2C address of a servomotor can be changed by writing the desired address to the variable saddr in RHDtest and then writing command 0x89 to the variable scmd in RHDtest. When this is done, the servomotor MCU restarts and communication will be lost. To regain communication, the servomotor I2C address needs to be changed in rhdconfig.xml (the ID field) to the new value and the RHD restarted. If the I2C address is forgotten, the servomotor needs to be reprogrammed (the default address is 5 (0x05)).
[edit] Controller Characteristics
Tests have been conducted with regards to determining the precision of the servomotor depending on the PD-controller gains. The table below shows the results of different step responses. The servomotor is tested while standing on its own without any load. The steady-state position error ess, and error e (the step size), are in A/D units.
P = 100, e = 600 | |||
---|---|---|---|
D = 0 | D = 50 | D = 100 | D = 150 |
ess = 12 | ess = 15 | ess = 15 | ess = 15 |
From these results it is seen that the D-gain has a very limited impact on the controller performance.
In order to avoid overshooting too much, small steps have been performed with large P-gains. The idea is to obtain a precise movement for a large step by making multiple small ones. The results can be seen in the table below. The steady-state position error ess, and error e (the step size), are in A/D units (approximate).
P = 400, D = 0 | |||||
---|---|---|---|---|---|
e = 25 | e = 50 | e = 75 | e = 100 | e = 200 | e = 500 |
ess = 7 | ess = 2 | ess = 6 | ess = 5 | ess = 6 | ess = 9 |
P = 400, D = 50 | |||||
---|---|---|---|---|---|
e = 25 | e = 50 | e = 75 | e = 100 | e = 200 | e = 500 |
ess = 7 | ess = 2 | ess = 7 | ess = 8 | ess = 7 | ess = 8 |
P = 400, D = 100 | |||||
---|---|---|---|---|---|
e = 25 | e = 50 | e = 75 | e = 100 | e = 200 | e = 500 |
ess = 8 | ess = 1 | ess = 6 | ess = 3 | ess = 5 | ess = 9 |
These results are approximate as the steady state error varies a little between steps. From these results it can be seen that with a P-gain of 400 and a step-size of 50, good results are obtained (i.e. a small steady-state error). Again it is seen that the effect of the D-gain is negligible.
It is noticed that steady-state errors tend to be larger when stepping on the high end of the potentiometer (i.e. steps 750-775, 800-850, 700-900 etc.). It should also be noticed that steady-state errors tend to be larger when moving from a higher position to a lower one (i.e. a step from 900 to 700 yields a larger steady-state error than moving from 700 to 900).
[edit] Further Reading
For more information, the reader is encouraged to see:
- "Haptic Sensor Based Navigation for autonomous Legged Robot", Master's Thesis, DTU, Mark Paw Hansen, March 2015
- "Control of 6-legged Inspection Robot for Iron Constructions", Bachelor's Thesis, DTU, Jan L. Svendsen, December 2013
- "Control and Design of Magnetic Foot for Inspection Robot", Bachelor's Thesis, DTU, Jesper A. Jørgensen, August 2014
- The OpenServo website.
- The short guide to using the Beaglebone Black BBB_guide_101v3.pdf