#include <driver.h>
Classes | |
struct | StatusField |
Public Member Functions | |
void | clearStatusCache () |
void | commandDutyCycle (float cmd) |
void | commandSpeed (double cmd) |
void | configureParams () |
std::string | deviceName () |
uint8_t | deviceNumber () |
Driver (Gateway &gateway, uint8_t device_number, std::string device_name) | |
uint16_t | encoderCounts () |
double | getD () |
double | getI () |
double | getP () |
uint8_t * | getRawD () |
uint8_t * | getRawI () |
uint8_t * | getRawP () |
bool | isConfigured () |
float | lastAnalogInput () |
float | lastBusVoltage () |
float | lastCurrent () |
float | lastDutyCycle () |
uint8_t | lastFault () |
uint8_t | lastMode () |
float | lastOutVoltage () |
double | lastPosition () |
uint8_t | lastPower () |
double | lastSetpoint () |
double | lastSpeed () |
float | lastTemperature () |
Driver | operator= (const Driver &rhs) |
uint8_t | posEncoderRef () |
void | processMessage (const Message &received_msg) |
void | requestFeedbackCurrent () |
void | requestFeedbackDutyCycle () |
void | requestFeedbackMessages () |
void | requestFeedbackPosition () |
void | requestFeedbackPowerState () |
void | requestFeedbackSetpoint () |
void | requestFeedbackSpeed () |
void | requestStatusMessages () |
void | resetConfiguration () |
void | setEncoderCPR (uint16_t encoder_cpr) |
void | setGains (double p, double i, double d) |
void | setGearRatio (float gear_ratio) |
void | setMode (uint8_t mode) |
void | setMode (uint8_t mode, double p, double i, double d) |
uint8_t | spdEncoderRef () |
float | statusCurrentGet () |
float | statusDutyCycleGet () |
double | statusPositionGet () |
double | statusSpeedGet () |
void | updateGains () |
void | verifyParams () |
Private Member Functions | |
void | sendFixed16x16 (uint32_t id, double value) |
void | sendFixed8x8 (uint32_t id, float value) |
void | sendUint16 (uint32_t id, uint16_t value) |
void | sendUint8 (uint32_t id, uint8_t value) |
StatusField * | statusFieldForMessage (const Message &msg) |
bool | verifyRaw16x16 (uint8_t *received, double expected) |
bool | verifyRaw8x8 (uint8_t *received, float expected) |
Private Attributes | |
bool | configured_ |
uint8_t | control_mode_ |
std::string | device_name_ |
uint8_t | device_number_ |
uint16_t | encoder_cpr_ |
double | gain_d_ |
double | gain_i_ |
double | gain_p_ |
Gateway & | gateway_ |
float | gear_ratio_ |
uint8_t | state_ |
StatusField | status_fields_ [12] |
puma_motor_driver::Driver::Driver | ( | Gateway & | gateway, |
uint8_t | device_number, | ||
std::string | device_name | ||
) |
Definition at line 39 of file driver.cpp.
Clear the received flags from the status cache, in preparation for the next request batch to go out.
Definition at line 451 of file driver.cpp.
void puma_motor_driver::Driver::commandDutyCycle | ( | float | cmd | ) |
Command the supplied value in open-loop voltage control.
[in] | cmd | Value to command, ranging from -1.0 to 1.0, where zero is neutral. |
Definition at line 144 of file driver.cpp.
void puma_motor_driver::Driver::commandSpeed | ( | double | cmd | ) |
Command the desired speed set-point in close-loop speed control.
[in] | cmd | Value to command in rad/s. |
Definition at line 149 of file driver.cpp.
The requesting part of the state machine that sends a message to the motor controller requesting a parameter be set.
Definition at line 308 of file driver.cpp.
std::string puma_motor_driver::Driver::deviceName | ( | ) | [inline] |
uint8_t puma_motor_driver::Driver::deviceNumber | ( | ) | [inline] |
uint16_t puma_motor_driver::Driver::encoderCounts | ( | ) |
Process the last received encoder counts response
Definition at line 649 of file driver.cpp.
double puma_motor_driver::Driver::getD | ( | ) |
Process the last received D gain for the current control mode.
Definition at line 691 of file driver.cpp.
double puma_motor_driver::Driver::getI | ( | ) |
Process the last received I gain for the current control mode.
Definition at line 673 of file driver.cpp.
double puma_motor_driver::Driver::getP | ( | ) |
Process the last received P gain for the current control mode.
Definition at line 655 of file driver.cpp.
uint8_t * puma_motor_driver::Driver::getRawD | ( | ) |
Process the last received I gain for the current control mode.
Definition at line 745 of file driver.cpp.
uint8_t * puma_motor_driver::Driver::getRawI | ( | ) |
Process the last received I gain for the current control mode.
Definition at line 727 of file driver.cpp.
uint8_t * puma_motor_driver::Driver::getRawP | ( | ) |
Process the last received P gain for the current control mode.
Definition at line 709 of file driver.cpp.
Gets if the driver has been configured.
Definition at line 393 of file driver.cpp.
Process the last received analog_input response.
Definition at line 587 of file driver.cpp.
Process the last received bus voltage response.
Definition at line 533 of file driver.cpp.
float puma_motor_driver::Driver::lastCurrent | ( | ) |
Process the last received current response.
Definition at line 539 of file driver.cpp.
float puma_motor_driver::Driver::lastDutyCycle | ( | ) |
Process the last received duty cycle response.
Definition at line 527 of file driver.cpp.
uint8_t puma_motor_driver::Driver::lastFault | ( | ) |
Process the last received fault response.
Definition at line 557 of file driver.cpp.
uint8_t puma_motor_driver::Driver::lastMode | ( | ) |
Process the last received mode response.
Definition at line 569 of file driver.cpp.
Process the last received out voltage response.
Definition at line 575 of file driver.cpp.
double puma_motor_driver::Driver::lastPosition | ( | ) |
Process the last received travel response.
Definition at line 545 of file driver.cpp.
uint8_t puma_motor_driver::Driver::lastPower | ( | ) |
Process the last received power response.
Definition at line 563 of file driver.cpp.
double puma_motor_driver::Driver::lastSetpoint | ( | ) |
Process the last received set-point response for the current control mode.
Definition at line 593 of file driver.cpp.
double puma_motor_driver::Driver::lastSpeed | ( | ) |
Process the last received speed response.
Definition at line 551 of file driver.cpp.
Process the last received temperature response.
Definition at line 581 of file driver.cpp.
uint8_t puma_motor_driver::Driver::posEncoderRef | ( | ) |
Updates the PID gains. Process the last received position encoder reference response
Definition at line 637 of file driver.cpp.
void puma_motor_driver::Driver::processMessage | ( | const Message & | received_msg | ) |
Definition at line 46 of file driver.cpp.
Sends a message to the motor controller requesting the instantaneous current consumption to populate the cache of feedback data.
Definition at line 476 of file driver.cpp.
Sends a message to the motor controller requesting the instantaneous duty cycle to populate the cache of feedback data.
Definition at line 471 of file driver.cpp.
Sends messages to the motor controller requesting all missing elements to populate the cache of feedback data. Returns true if any messages were sent, false if the cache is already complete.
Definition at line 463 of file driver.cpp.
Sends a message to the motor controller requesting the instantaneous angular distance to populate the cache of feedback data.
Definition at line 481 of file driver.cpp.
Sends a message to the motor controller requesting the state of the power flag.
Definition at line 491 of file driver.cpp.
Sends a message to the motor controller requesting the instantaneous set point of the current control mode to populate the cache of feedback data.
Definition at line 496 of file driver.cpp.
Sends a message to the motor controller requesting the instantaneous angular speed to populate the cache of feedback data.
Definition at line 486 of file driver.cpp.
Sends messages to the motor controller requesting all missing elements to populate the cache of status data. Returns true if any messages were sent, false if the cache is already complete.
Definition at line 458 of file driver.cpp.
Definition at line 515 of file driver.cpp.
void puma_motor_driver::Driver::sendFixed16x16 | ( | uint32_t | id, |
double | value | ||
) | [private] |
Definition at line 92 of file driver.cpp.
void puma_motor_driver::Driver::sendFixed8x8 | ( | uint32_t | id, |
float | value | ||
) | [private] |
Definition at line 82 of file driver.cpp.
void puma_motor_driver::Driver::sendUint16 | ( | uint32_t | id, |
uint16_t | value | ||
) | [private] |
Definition at line 73 of file driver.cpp.
void puma_motor_driver::Driver::sendUint8 | ( | uint32_t | id, |
uint8_t | value | ||
) | [private] |
Helpers to generate data for CAN messages.
Definition at line 64 of file driver.cpp.
void puma_motor_driver::Driver::setEncoderCPR | ( | uint16_t | encoder_cpr | ) |
Set the encoders resolution in counts per rev.
[in] | encoder_cpr | Value to set. |
Definition at line 134 of file driver.cpp.
void puma_motor_driver::Driver::setGains | ( | double | p, |
double | i, | ||
double | d | ||
) |
Set the control mode's PID gains for close loop control.
[in] | p | Value to set. |
[in] | i | Value to set. |
[in] | d | Value to set. |
Definition at line 398 of file driver.cpp.
void puma_motor_driver::Driver::setGearRatio | ( | float | gear_ratio | ) |
Set the gear ratio of the motors.
[in] | gear_ratio | Value to set. |
Definition at line 139 of file driver.cpp.
void puma_motor_driver::Driver::setMode | ( | uint8_t | mode | ) |
Set the control mode of the motor drivers.
[in] | mode | Value to set. |
Definition at line 410 of file driver.cpp.
void puma_motor_driver::Driver::setMode | ( | uint8_t | mode, |
double | p, | ||
double | i, | ||
double | d | ||
) |
Set the control mode of the motor drivers with PID gains for close loop control.
[in] | mode | Value to set. |
[in] | p | Value to set. |
[in] | i | Value to set. |
[in] | d | Value to set. |
Definition at line 427 of file driver.cpp.
uint8_t puma_motor_driver::Driver::spdEncoderRef | ( | ) |
Process the last received speed encoder reference response
Definition at line 643 of file driver.cpp.
Process the last received set-point response in currrent closed-loop control.
Definition at line 626 of file driver.cpp.
Process the last received set-point response in voltage open-loop control.
Definition at line 620 of file driver.cpp.
Driver::StatusField * puma_motor_driver::Driver::statusFieldForMessage | ( | const Message & | msg | ) | [private] |
Definition at line 762 of file driver.cpp.
double puma_motor_driver::Driver::statusPositionGet | ( | ) |
Process the last received set-point response in position closed-loop control.
Definition at line 631 of file driver.cpp.
double puma_motor_driver::Driver::statusSpeedGet | ( | ) |
Process the last received set-point response in speed closed-loop control.
Definition at line 614 of file driver.cpp.
Reset the configured flag to restart the verification process.
Definition at line 521 of file driver.cpp.
The verifying part of the state machine that checks the response of the motor controller to ensure the value was set.
Definition at line 155 of file driver.cpp.
bool puma_motor_driver::Driver::verifyRaw16x16 | ( | uint8_t * | received, |
double | expected | ||
) | [private] |
Comparing the raw bytes of the 16x16 fixed-point numbers to avoid comparing the floating point values.
Definition at line 102 of file driver.cpp.
bool puma_motor_driver::Driver::verifyRaw8x8 | ( | uint8_t * | received, |
float | expected | ||
) | [private] |
Comparing the raw bytes of the 8x8 fixed-point numbers to avoid comparing the floating point values.
Definition at line 118 of file driver.cpp.
bool puma_motor_driver::Driver::configured_ [private] |
uint8_t puma_motor_driver::Driver::control_mode_ [private] |
std::string puma_motor_driver::Driver::device_name_ [private] |
uint8_t puma_motor_driver::Driver::device_number_ [private] |
uint16_t puma_motor_driver::Driver::encoder_cpr_ [private] |
double puma_motor_driver::Driver::gain_d_ [private] |
double puma_motor_driver::Driver::gain_i_ [private] |
double puma_motor_driver::Driver::gain_p_ [private] |
Gateway& puma_motor_driver::Driver::gateway_ [private] |
float puma_motor_driver::Driver::gear_ratio_ [private] |
uint8_t puma_motor_driver::Driver::state_ [private] |
StatusField puma_motor_driver::Driver::status_fields_[12] [private] |