Decorates the Teensy Encoder Library to read the angular wheel velocity from quadrature wheel encoders. More...
#include <encoder_diffbot.h>
Public Member Functions | |
| double | angularPosition () |
| double | angularVelocity () |
| Get the measure the angular joint velocity. More... | |
| Encoder (ros::NodeHandle &nh, uint8_t pin1, uint8_t pin2, int encoder_resolution) | |
| Construct a diffbot::Encoder providing access to quadrature encoder ticks and angular joint velocity. More... | |
| int | getRPM () |
| get revolutions per minute More... | |
| JointState | jointState () |
| int32_t | read () |
| Read the current encoder tick count. More... | |
| int | resolution () |
| Getter for encoder resolution. More... | |
| void | resolution (int resolution) |
| Setter for encoder resolution. More... | |
| double | ticksToAngle (const int &ticks) const |
| Convert number of encoder ticks to angle in radians. More... | |
| void | write (int32_t p) |
| Set the encoder tick count. More... | |
Public Attributes | |
| ::Encoder | encoder |
| JointState | joint_state_ |
Private Attributes | |
| int | encoder_resolution_ |
| ros::NodeHandle & | nh_ |
| long | prev_encoder_ticks_ |
| ros::Time | prev_update_time_ |
Decorates the Teensy Encoder Library to read the angular wheel velocity from quadrature wheel encoders.
This class is composed of ::Encoder from https://www.pjrc.com/teensy/td_libs_Encoder.html, which is capable of reading rising and falling edges of two Hall effect signals. This yields the highest possible tick count (encoder resolution) from the encoders. Take the DG01D-E motor/encoder, that has a 6 pole magnetic disk (observed with magnetic paper) and therefore 3 pulses per revolution (ppr). Reading both edges (rising and falling) of both channels in the code, we can observe roughly 542 ticks at the wheel output shaft. Because the DG01D-E motor/encoder has a 48:1 gear ratio (according to its datasheet) we round the tick count up to 576 counts per wheel revolution). So 576 counts / 48:1 gear ratio / 2 channels / 2 edges = 3 pulses per revolution at the motor shaft. Having an encoder with more ppr would increase the encoder resolution. For example let's say we we have an encoder with 7 ppr, then we get the following output resolution at the wheel: 7 ppr (at motor shaft) * 48:1 gear ratio * 2 channels * 2 edges = 1344 ppr (measured at wheel).
Definition at line 35 of file encoder_diffbot.h.
| diffbot::Encoder::Encoder | ( | ros::NodeHandle & | nh, |
| uint8_t | pin1, | ||
| uint8_t | pin2, | ||
| int | encoder_resolution | ||
| ) |
Construct a diffbot::Encoder providing access to quadrature encoder ticks and angular joint velocity.
| nh | reference to the main ros::NodeHandle to compute the velocity from time and ticks or angle (s = v * t) |
| pin1 | Pin of the first Hall effect sensor |
| pin2 | Pin of the second Hall effect sensor |
| encoder_resolution | number of tick counts for one full revolution of the wheel (not the motor shaft). Keep track of gear reduction ratio. |
Definition at line 4 of file encoder_diffbot.cpp.
| double diffbot::Encoder::angularPosition | ( | ) |
Definition at line 39 of file encoder_diffbot.cpp.
| double diffbot::Encoder::angularVelocity | ( | ) |
Get the measure the angular joint velocity.
Calculates the angular velocity of the wheel joint using the encoder ticks.
Definition at line 45 of file encoder_diffbot.cpp.
| int diffbot::Encoder::getRPM | ( | ) |
get revolutions per minute
Calculates the wheels revolution per minute using the encoder ticks.
Definition at line 58 of file encoder_diffbot.cpp.
| diffbot::JointState diffbot::Encoder::jointState | ( | ) |
Definition at line 14 of file encoder_diffbot.cpp.
|
inline |
Read the current encoder tick count.
Definition at line 89 of file encoder_diffbot.h.
|
inline |
Getter for encoder resolution.
Returns the currently set encder resolution.
Definition at line 112 of file encoder_diffbot.h.
|
inline |
Setter for encoder resolution.
Used to initialize the encoder with a new resolution, e.g. obtained from the ROS parameter server.
| resolution | value to which the encoder tick count shoudl be set |
Definition at line 106 of file encoder_diffbot.h.
| double diffbot::Encoder::ticksToAngle | ( | const int & | ticks | ) | const |
Convert number of encoder ticks to angle in radians.
Calculates the current tick count of the encoder to its absolute angle in radians using the encoder_resolution_.
| ticks | tick count from an encoder which is converted to a corresponding absolute angle. |
Definition at line 50 of file encoder_diffbot.cpp.
|
inline |
Set the encoder tick count.
Mainly used to reset the encoder back to zero.
| p | encoder ticks |
Definition at line 97 of file encoder_diffbot.h.
| ::Encoder diffbot::Encoder::encoder |
Definition at line 39 of file encoder_diffbot.h.
|
private |
Definition at line 121 of file encoder_diffbot.h.
| JointState diffbot::Encoder::joint_state_ |
Definition at line 112 of file encoder_diffbot.h.
|
private |
Definition at line 119 of file encoder_diffbot.h.
|
private |
Definition at line 125 of file encoder_diffbot.h.
|
private |
Definition at line 123 of file encoder_diffbot.h.