C++ CANopenMotorDriver class for ROS. More...
#include <inc/CANopenMotorDriver.h>
Public Member Functions | |
CANopenMotorDriver (unsigned char axis_id, ros::NodeHandle *nh) | |
Constructor. | |
void | enableOperation () |
void | readyToSwitchOn () |
void | run () |
Run. | |
void | setQuickStopDeceleration (double deceleration) |
Set quick stop deceleration. | |
void | setupDCmotorWithEncoder (unsigned int num_of_encoder_lines, double Tr, double T) |
void | setupHomingMode (HomingMethod method, double fast_speed, double slow_speed, double acceleration, double offset=0.0) |
Setup homing. | |
void | setupPositionScurveProfileMode (double target, double speed, double acceleration, double jerk_time) |
Setup position S-curve profile mode. | |
void | setupPositionTrapezoidalProfileMode (double target, double speed, double acceleration) |
Setup position trapezoidal profile mode. | |
void | setupStepperMotor (unsigned int num_of_steps, unsigned int num_of_usteps, double Tr, double T) |
void | setupVelocityProfileMode (double target_speed, double acceleration) |
Setup velocity profile mode. | |
void | startNode () |
Start remote node. | |
void | switchOn () |
~CANopenMotorDriver () | |
Destructor. | |
Private Member Functions | |
void | receivedCANframe (const can_msgs::CANFrame::ConstPtr &msg) |
Received CANframe. | |
Private Attributes | |
ros::Publisher | can_pub_ |
CAN bus publisher. | |
ros::Subscriber | can_sub_ |
CAN bus subscriber. | |
unsigned char | id_ |
ID of the CAN device being used. | |
boost::function< void(char *, int) | limitSwitchesCallback_ ) |
Callback boost function for limit switches. | |
double | load_to_motor_acceleration_ |
Acceleration units. | |
double | load_to_motor_jerk_ |
Jerk units. | |
double | load_to_motor_position_ |
Position units. | |
double | load_to_motor_speed_ |
Speed units. | |
double | T_ |
Time convertion factor. |
C++ CANopenMotorDriver class for ROS.
This class was based on Technosoft CANopen programming guide.
Definition at line 129 of file CANopenMotorDriver.h.
canopen::CANopenMotorDriver::CANopenMotorDriver | ( | unsigned char | axis_id, |
ros::NodeHandle * | nh | ||
) |
Constructor.
Macro for throwing an exception with a message, passing args.
Definition at line 48 of file CANopenMotorDriver.cpp.
Destructor.
Definition at line 58 of file CANopenMotorDriver.cpp.
Enable operation.
Definition at line 154 of file CANopenMotorDriver.cpp.
Ready to switch on.
Definition at line 128 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::receivedCANframe | ( | const can_msgs::CANFrame::ConstPtr & | msg | ) | [private] |
Received CANframe.
This is the callback function for incoming CAN frames.
msg | ROS can_msgs/CANFrame message. |
Definition at line 63 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::run | ( | ) |
void canopen::CANopenMotorDriver::setQuickStopDeceleration | ( | double | deceleration | ) |
Set quick stop deceleration.
The quick stop deceleration is the deceleration used to stop the motor if the Quick Stop command is received.
deceleration | Quick stop deceleration. |
Definition at line 358 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::setupDCmotorWithEncoder | ( | unsigned int | num_of_encoder_lines, |
double | Tr, | ||
double | T | ||
) |
Definition at line 75 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::setupHomingMode | ( | canopen::HomingMethod | method, |
double | fast_speed, | ||
double | slow_speed, | ||
double | acceleration, | ||
double | offset = 0.0 |
||
) |
Setup homing.
Homing is the method by which a drive seeks the home position. There are various methods to achieve this position using the four available sources for the homing signal: limit switches (negative and positive), home switch and index pulse.
method | The homing method determines the method that will be used during homing (1 to 35). |
fast_speed | The slower speed is used to find the index pulse. |
slow_speed | The faster speed is used to find the home switch. |
acceleration | The acceleration to be used for all the accelerations and decelerations with the standard homing modes. |
offset | The difference between the zero position for the application and the machine home position. |
Definition at line 184 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::setupPositionScurveProfileMode | ( | double | target, |
double | speed, | ||
double | acceleration, | ||
double | jerk_time | ||
) |
Setup position S-curve profile mode.
S-curve profile The built-in reference generator computes a position profile with an S-curve shape of the speed. This shape is due to the jerk limitation, leading to a trapezoidal or triangular profile for the acceleration and an S-curve profile for the speed. The CANopen master specifies the absolute or relative Target Position, the Profile Velocity, the Profile Acceleration and the jerk rate. The jerk rate is set indirectly via the Jerk time, which represents the time needed to reach the maximum acceleration starting from zero.
target | The position that the drive should move to. |
speed | The maximum speed to reach at the end of the acceleration ramp. |
acceleration | The acceleration and deceleration rates used to change the speed between 2 levels. |
jerk_time | The acceleration and deceleration rates used to change the speed between 2 levels. |
void canopen::CANopenMotorDriver::setupPositionTrapezoidalProfileMode | ( | double | target, |
double | speed, | ||
double | acceleration | ||
) |
Setup position trapezoidal profile mode.
Trapezoidal profile. The built-in reference generator computes the position profile with a trapezoidal shape of the speed, due to a limited acceleration. The CANopen master specifies the absolute or relative Target Position, the Profile Velocity and the Profile Acceleration.
target | The position that the drive should move to. |
speed | The maximum speed to reach at the end of the acceleration ramp. |
acceleration | The acceleration and deceleration rates used to change the speed between 2 levels. |
Definition at line 277 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::setupStepperMotor | ( | unsigned int | num_of_steps, |
unsigned int | num_of_usteps, | ||
double | Tr, | ||
double | T | ||
) |
Definition at line 93 of file CANopenMotorDriver.cpp.
void canopen::CANopenMotorDriver::setupVelocityProfileMode | ( | double | target_speed, |
double | acceleration | ||
) |
Setup velocity profile mode.
In the Velocity Profile Mode the drive performs speed control. The built-in reference generator computes a speed profile with a trapezoidal shape, due to a limited acceleration. The Target Velocity object specifies the jog speed (speed sign specifies the direction) and the Profile Acceleration object the acceleration/deceleration rate.
target_speed | The target speed. |
acceleration | The acceleration and deceleration rates used to change the speed between 2 levels. |
Definition at line 383 of file CANopenMotorDriver.cpp.
Start remote node.
This starts up the remote node.
Definition at line 115 of file CANopenMotorDriver.cpp.
Switch on.
Definition at line 141 of file CANopenMotorDriver.cpp.
CAN bus publisher.
Definition at line 312 of file CANopenMotorDriver.h.
CAN bus subscriber.
Definition at line 314 of file CANopenMotorDriver.h.
unsigned char canopen::CANopenMotorDriver::id_ [private] |
ID of the CAN device being used.
Definition at line 291 of file CANopenMotorDriver.h.
boost::function<void(char*, int) canopen::CANopenMotorDriver::limitSwitchesCallback_) [private] |
Callback boost function for limit switches.
Definition at line 330 of file CANopenMotorDriver.h.
double canopen::CANopenMotorDriver::load_to_motor_acceleration_ [private] |
Acceleration units.
Definition at line 303 of file CANopenMotorDriver.h.
double canopen::CANopenMotorDriver::load_to_motor_jerk_ [private] |
Jerk units.
Definition at line 305 of file CANopenMotorDriver.h.
double canopen::CANopenMotorDriver::load_to_motor_position_ [private] |
Position units.
Definition at line 299 of file CANopenMotorDriver.h.
double canopen::CANopenMotorDriver::load_to_motor_speed_ [private] |
Speed units.
Definition at line 301 of file CANopenMotorDriver.h.
double canopen::CANopenMotorDriver::T_ [private] |
Time convertion factor.
Definition at line 297 of file CANopenMotorDriver.h.