Implementation of a driver for EPOS2 Motor Controller. More...
#include <Epos2.h>
Public Types | |
Operation Modes - Names | |
enum | epos_states { FAULT, START, NOT_READY_TO_SWITCH_ON, SWITCH_ON_DISABLED, READY_TO_SWITCH_ON, SWITCH_ON, REFRESH, MEASURE_INIT, OPERATION_ENABLE, QUICK_STOP, QUICK_STOP_ACTIVE_DISABLE, QUICK_STOP_ACTIVE_ENABLE } |
enum | epos_opmodes { NO_OPERATION = 99, VELOCITY = -2, POSITION = -1, PROFILE_VELOCITY = 3, PROFILE_POSITION = 1, INTERPOLATED_PROFILE_POSITION = 7, HOMING = 6 } |
enum | epos_posmodes { HALT, ABSOLUTE, RELATIVE } |
Public Member Functions | |
CEpos2 (int8_t nodeId=0x00) | |
Constructor. More... | |
void | close () |
Disconnects hardware. More... | |
void | init () |
Connects hardware. More... | |
~CEpos2 () | |
Destructor. More... | |
State Management | |
long | getState () |
function to get current controller's state More... | |
void | shutdown () |
function to reach ready_to_switch_on state More... | |
void | switchOn () |
function to reach switch_on state More... | |
void | quickStop () |
function to reach switch_on state More... | |
void | enableOperation () |
function to reach operation_enable state and enables power on motor More... | |
void | disableOperation () |
function to reach switch_on and disables power on motor More... | |
void | disableVoltage () |
function to reach switch_on_disabled state More... | |
void | faultReset () |
function to reach switch_on_disabled state after a FAULT More... | |
Operation Modes Management | |
long | getOperationMode () |
function to know which operation mode is set More... | |
std::string | getOpModeDescription (long opmode) |
function to show the name of the operation mode More... | |
void | setOperationMode (long opmode) |
function to set the operation mode More... | |
void | enableController () |
function to facititate transitions from the start of the controller to switch it on More... | |
void | enableMotor (long opmode) |
function to facititate transitions from switched on to operation enabled More... | |
bool | isTargetReached () |
function to know if the motor has reached the target position or velocity More... | |
Operation Mode - velocity | |
long | getTargetVelocity () |
function to GET the target velocity This function gets the target velocity of velocity operation mode More... | |
void | setTargetVelocity (long velocity) |
function to SET the target velocity More... | |
void | startVelocity () |
function to move the motor in Velocity mode More... | |
void | stopVelocity () |
function to stop the motor in velocity mode More... | |
Operation Mode - profile_position | |
long | getTargetProfilePosition () |
function to GET the target position More... | |
void | setTargetProfilePosition (long position) |
function to SET the target position More... | |
void | startProfilePosition (epos_posmodes mode, bool blocking=true, bool wait=true, bool new_point=true) |
function to move the motor to a position in profile position mode More... | |
Operation Mode - profile_velocity | |
long | getTargetProfileVelocity () |
[OPMODE=profile_velocity] function to get the velocity More... | |
void | setTargetProfileVelocity (long velocity) |
[OPMODE=profile_velocity] function to set the velocity More... | |
void | startProfileVelocity () |
[OPMODE=profile_velocity] function to move the motor in a velocity More... | |
void | stopProfileVelocity () |
[OPMODE=profile_velocity] function to stop the motor More... | |
Operation Mode - current | |
long | getTargetCurrent () |
[OPMODE=current] function to GET the target current More... | |
void | setTargetCurrent (long current) |
[OPMODE=current] function to SET the target current More... | |
void | startCurrent () |
[OPMODE=current] function to move the motor in current mode More... | |
void | stopCurrent () |
[OPMODE=current] function to stop the motor in current mode More... | |
Operation Mode - home | |
void | setHome () |
function to set a Home position with user interaction More... | |
void | setHomePosition (long home_position_qc) |
function to set a Home position More... | |
long | getHomePosition () |
function to get the Home position More... | |
Control Tuning | |
Motion Control Configuration | |
long | getCurrentPGain () |
function to get P Current Gain More... | |
void | setCurrentPGain (long gain) |
function to set P Current Gain More... | |
long | getCurrentIGain () |
function to get I Current Gain More... | |
void | setCurrentIGain (long gain) |
function to set I Current Gain More... | |
long | getVelocityPGain () |
function to get P Velocity Gain More... | |
void | setVelocityPGain (long gain) |
function to set P Velocity Gain More... | |
long | getVelocityIGain () |
function to get I Velocity Gain More... | |
void | setVelocityIGain (long gain) |
function to set I Velocity Gain More... | |
long | getVelocitySetPointFactorPGain () |
function to GET Speed Regulator Set Point Factor P Velocity Gain More... | |
void | setVelocitySetPointFactorPGain (long gain) |
function to SET Speed Regulator Set Point Factor P Velocity Gain More... | |
long | getPositionPGain () |
function to get P Position Gain More... | |
void | setPositionPGain (long gain) |
function to set P Position Gain More... | |
long | getPositionIGain () |
function to get I Position Gain More... | |
void | setPositionIGain (long gain) |
function to set I Position Gain More... | |
long | getPositionDGain () |
function to get D Position Gain More... | |
void | setPositionDGain (long gain) |
function to set D Position Gain More... | |
long | getPositionVFFGain () |
function to get Velocity Feed Forward Position Gain More... | |
void | setPositionVFFGain (long gain) |
function to set Velocity Feed Forward Position Gain More... | |
long | getPositionAFFGain () |
function to get Acceleration Feed Forward Position Gain More... | |
void | setPositionAFFGain (long gain) |
function to set Acceleration Feed Forward Position Gain More... | |
void | getControlParameters (long &cp, long &ci, long &vp, long &vi, long &vspf, long &pp, long &pi, long &pd, long &pv, long &pa) |
function to get all control parameters More... | |
void | setControlParameters (long cp, long ci, long vp, long vi, long vspf, long pp, long pi, long pd, long pv, long pa) |
function to set all control parameters More... | |
void | printControlParameters (long cp, long ci, long vp, long vi, long vspf, long pp, long pi, long pd, long pv, long pa) |
function to show all control parameters More... | |
Profile | |
For Profile Position and Profile Velocity modes. | |
long | getProfileVelocity (void) |
function to GET the velocity of the Velocity Profile More... | |
void | setProfileVelocity (long velocity) |
function to SET the velocity of the Velocity Profile More... | |
long | getProfileMaxVelocity (void) |
function to GET the Max Velocity allowed in Profile More... | |
void | setProfileMaxVelocity (long velocity) |
function to SET the Max Velocity allowed in Profile More... | |
long | getProfileAcceleration (void) |
function to GET the Acceleration in profile More... | |
void | setProfileAcceleration (long acceleration) |
function to SET the Acceleration in profile More... | |
long | getProfileDeceleration (void) |
function to GET the Deceleration in profile More... | |
void | setProfileDeceleration (long deceleration) |
function to SET the Deceleration in profile More... | |
long | getProfileQuickStopDecel (void) |
function to GET the Quick Stop Deceleration in profile More... | |
void | setProfileQuickStopDecel (long deceleration) |
function to SET the Quick Stop Deceleration in profile More... | |
long | getProfileType (void) |
function to GET the type in profile More... | |
void | setProfileType (long type) |
function to SET the type in profile More... | |
long | getMaxAcceleration (void) |
function to GET Max Acceleration More... | |
void | setMaxAcceleration (long max_acceleration) |
function to SET Max Acceleration More... | |
void | getProfileData (long &vel, long &maxvel, long &acc, long &dec, long &qsdec, long &maxacc, long &type) |
function to GET all data of the velocity profile More... | |
void | setProfileData (long vel, long maxvel, long acc, long dec, long qsdec, long maxacc, long type) |
function to SET all data of the velocity profile More... | |
Sensor configuration | |
long | getEncoderPulseNumber () |
function to GET the Encoder Pulses More... | |
void | setEncoderPulseNumber (long pulses) |
function to SET the Encoder Pulses More... | |
long | getEncoderType () |
function to GET the Encoder Pulses More... | |
void | setEncoderType (long type) |
function to SET the Encoder type More... | |
long | getEncoderPolarity () |
function to GET the Encoder Pulses More... | |
void | setEncoderPolarity (long polarity) |
function to SET the Encoder polarity More... | |
void | getEncoderParameters (long &pulses, long &type, long &polarity) |
function to GET encoder parameters More... | |
void | setEncoderParameters (long pulses, long type, long polarity) |
function to SET encoder parameters More... | |
Motor | |
long | getMotorType () |
function to GET Motor type More... | |
void | setMotorType (long type) |
function to SET Motor type More... | |
long | getMotorContinuousCurrentLimit () |
function to GET Motor Continous Current Limit More... | |
void | setMotorContinuousCurrentLimit (long current_mA) |
function to SET Motor Continous Current Limit More... | |
long | getMotorOutputCurrentLimit () |
function to GET Motor Output Current Limit More... | |
void | setMotorOutputCurrentLimit (long current_mA) |
function to SET Motor Output Current Limit More... | |
long | getMotorPolePairNumber () |
function to GET Motor Pole Pair Number More... | |
void | setMotorPolePairNumber (char pole_pairs) |
function to SET Motor Pole Pair Number More... | |
long | getThermalTimeCtWinding () |
function to GET Motor Thermal Time Constant Winding More... | |
void | setThermalTimeCtWinding (long time_ds) |
function to SET Motor Thermal Time Constant Winding More... | |
void | getMotorParameters (long &type, long ¤t_c_mA, long ¤t_out_mA, char &pole_pairs, long &time_ds) |
function to GET encoder parameters More... | |
void | setMotorParameters (long type, long current_c_mA, long current_out_mA, char pole_pairs, long time_ds) |
function to SET motor parameters More... | |
Communication configuration | |
long | getRS232Baudrate () |
function to GET the RS232 Baudrate More... | |
void | setRS232Baudrate (long baudrate) |
function to SET the RS232 Baudrate More... | |
long | getRS232FrameTimeout () |
function to GET the RS232 Frame Timeout More... | |
void | setRS232FrameTimeout (long timeout) |
function to SET the RS232 Frame Timeout More... | |
long | getUSBFrameTimeout () |
function to GET the USB Frame Timeout More... | |
void | setUSBFrameTimeout (long timeout) |
function to SET the USB Frame Timeout More... | |
Position parameters configuration | |
long | getMaxFollowingError () |
function to GET the Max Following Error More... | |
void | setMaxFollowingError (long follerror) |
function to SET the Max Following Error More... | |
long | getMinPositionLimit () |
function to GET the Min Position Limit More... | |
void | setMinPositionLimit (long limit) |
function to SET the Min Position Limit More... | |
long | getMaxPositionLimit () |
function to GET the Max Position Limit More... | |
void | setMaxPositionLimit (long limit) |
function to SET the Max Position Limit More... | |
void | disablePositionLimits (void) |
function to DISABLE Position Limits More... | |
long | getPositionWindow () |
function to get Position Window More... | |
void | setPositionWindow (long window_qc) |
function to set Position Window More... | |
long | getPositionWindowTime () |
function to get Position Window Time More... | |
void | setPositionWindowTime (long time_ms) |
function to set Position Window Time More... | |
long | getVelocityWindow () |
function to get Velocity Window More... | |
void | setVelocityWindow (long window_rm) |
function to set Velocity Window More... | |
long | getVelocityWindowTime () |
function to get Velocity Window Time More... | |
void | setVelocityWindowTime (long time_ms) |
function to set Velocity Window Time More... | |
Dimension and notation configuration | |
long | getPositionNotationIndex () |
function to get Position Notation Index More... | |
void | setPositionNotationIndex (long notation) |
function to set Position Notation Index More... | |
long | getVelocityNotationIndex () |
function to get Velocity Notation Index More... | |
void | setVelocityNotationIndex (long notation) |
function to set Velocity Notation Index More... | |
long | getAccelerationNotationIndex () |
function to get Acceleration Notation Index More... | |
void | setAccelerationNotationIndex (long notation) |
function to set Acceleration Notation Index More... | |
long | getPositionDimensionIndex () |
function to get Position Dimension Index More... | |
void | setPositionDimensionIndex (long Dimension) |
function to set Position Dimension Index More... | |
long | getVelocityDimensionIndex () |
function to get Velocity Dimension Index More... | |
void | setVelocityDimensionIndex (long Dimension) |
function to set Velocity Dimension Index More... | |
long | getAccelerationDimensionIndex () |
function to get Acceleration Dimension Index More... | |
void | setAccelerationDimensionIndex (long Dimension) |
function to set Acceleration Dimension Index More... | |
Private Member Functions | |
long | getNegativeLong (long number) |
function to make a unsigned long signed More... | |
void | p (const char *text) |
function to make a unsigned long signed More... | |
void | p (const std::stringstream &text) |
Communication low level | |
void | openDevice () |
open EPOS2 device using CFTDI More... | |
int32_t | readObject (int16_t index, int8_t subindex) |
function to read an object from the EPOS2 More... | |
int | writeObject (int16_t index, int8_t subindex, int32_t data) |
function to write an object to the EPOS2 More... | |
void | sendFrame (int16_t *frame) |
function send a frame to EPOS2 More... | |
void | receiveFrame (uint16_t *ans_frame) |
function receive a frame from EPOS2 More... | |
int16_t | computeChecksum (int16_t *pDataArray, int16_t numberOfWords) |
function to compute EPOS2 checksum More... | |
Private Attributes | |
int8_t | node_id |
bool | verbose |
Static Private Attributes | |
static Ftdi::Context | ftdi |
a reference to the FTDI USB device More... | |
static bool | ftdi_initialized = false |
Info | |
static const std::string | error_names [] |
Strings of generic error descriptions. More... | |
static const int | error_codes [] |
error integer codes More... | |
static const std::string | error_descriptions [] |
error descriptions More... | |
long | readVelocity () |
function to read motor average velocity More... | |
long | readVelocitySensorActual () |
function to read velocity sensor actual More... | |
long | readVelocityDemand () |
function to read velocity demand More... | |
long | readVelocityActual () |
function to read velocity sensor actual More... | |
long | readCurrent () |
function to read motor current More... | |
long | readCurrentAveraged () |
function to read motor averaged current More... | |
long | readCurrentDemanded () |
function to read current demanded More... | |
int32_t | readPosition () |
function to read motor position More... | |
long | readStatusWord () |
function to read EPOS2 StatusWord More... | |
long | readEncoderCounter () |
function to read the Encoder Counter More... | |
long | readEncoderCounterAtIndexPulse () |
function to read the Encoder Counter at index pulse More... | |
long | readHallsensorPattern () |
function to read the Hall Sensor Pattern More... | |
long | readFollowingError () |
function to read the Following Error More... | |
void | getMovementInfo () |
prints information about movement More... | |
char | readError () |
function to read an Error information More... | |
void | readErrorHistory (long *error[5]) |
function to read last 5 errors More... | |
std::string | searchErrorDescription (long error_code) |
given an error code it returns its description More... | |
long | readVersionSoftware () |
function to read the software version More... | |
long | readVersionHardware () |
function to read the hardware version More... | |
Utilities | |
long | getAnalogOutput1 () |
function to get the value in the Analog Output1 More... | |
void | setAnalogOutput1 (long voltage_mV) |
function to set analog output 1 More... | |
void | saveParameters () |
function to save all parameters in EEPROM More... | |
void | restoreDefaultParameters () |
function to restore default parameters of EPOS2. More... | |
bool | getVerbose () |
void | setVerbose (bool verbose) |
long | getPositionMarker (int buffer=0) |
gets the position marked by the sensor More... | |
void | setPositionMarker (char polarity=0, char edge_type=0, char mode=0, char digitalIN=2) |
Sets the configuration of a marker position. More... | |
void | waitPositionMarker () |
wait for marker position reached More... | |
void | setHoming (int home_method=11, int speed_pos=100, int speed_zero=100, int acc=1000, int digitalIN=2) |
Sets the configuration of a homing operation. More... | |
void | doHoming (bool blocking=false) |
starts a homing operation More... | |
void | stopHoming () |
stops a homing operation More... | |
int | getDigInState (int digitalIN=2) |
Gives the state of a certain digital Input. More... | |
int | getDigInStateMask () |
Gives Digital Inputs State Mask. More... | |
int | getDigInFuncMask () |
Gives Digital Inputs Functionalities Mask. More... | |
int | getDigInPolarity () |
Gives Digital Inputs Polarity Mask. More... | |
int | getDigInExecutionMask () |
Gives Digital Inputs Execution Mask. More... | |
static void * | threadTargetReached (void *param) |
Thread listens to target reached. More... | |
Implementation of a driver for EPOS2 Motor Controller.
This class is a low level driver to manage Maxon Motor EPOS2 Controller. It is tested in EPOS2 50/5 using firmware Epos_2110h_6322h_0000h_0000h.bin. All functions objects in controller are in the document EPOS2 Firmware Specification of December 2008 Edition/ document number 821384-02.
It implements all functions to use one unique controller and adds other useful features to simplify some tasks.
enum CEpos2::epos_opmodes |
enum CEpos2::epos_states |
void CEpos2::close | ( | ) |
Disconnects hardware.
|
private |
void CEpos2::disableOperation | ( | ) |
function to reach switch_on and disables power on motor
Transition: 5 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
void CEpos2::disablePositionLimits | ( | void | ) |
void CEpos2::disableVoltage | ( | ) |
function to reach switch_on_disabled state
Transition: 7,9,10,12 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
void CEpos2::doHoming | ( | bool | blocking = false | ) |
void CEpos2::enableController | ( | ) |
function to facititate transitions from the start of the controller to switch it on
This function does all transitions from start to switched on to be able to work with the controller.
void CEpos2::enableMotor | ( | long | opmode | ) |
function to facititate transitions from switched on to operation enabled
This function does all transitions from switched on to operation enabled work with the motor.
\param opmode desired operation mode (velocity, profile_position,
profile_velocity,current,homing)
void CEpos2::enableOperation | ( | ) |
function to reach operation_enable state and enables power on motor
Transition: 4,16 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
void CEpos2::faultReset | ( | ) |
function to reach switch_on_disabled state after a FAULT
This function does the transition to reach switch_on_disabled state after a FAULT. It is necessary to clear Can passive mode error at the start of EPOS controller
Transition: 15 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
long CEpos2::getAccelerationDimensionIndex | ( | ) |
long CEpos2::getAccelerationNotationIndex | ( | ) |
long CEpos2::getAnalogOutput1 | ( | ) |
void CEpos2::getControlParameters | ( | long & | cp, |
long & | ci, | ||
long & | vp, | ||
long & | vi, | ||
long & | vspf, | ||
long & | pp, | ||
long & | pi, | ||
long & | pd, | ||
long & | pv, | ||
long & | pa | ||
) |
function to get all control parameters
gets all control parameters and save them into user variables
cp | Current Proportional |
ci | Current Integral |
vp | Velocity Proportional |
vi | Velocity Integral |
vspf | Velocity Set Point Factor P Gain |
pp | Postition Proportional |
pi | Position Integral |
pd | Position Derivative |
pv | Position Velocity Feed Forward Factor |
pa | Position Acceleration Feed Forward Factor |
long CEpos2::getCurrentIGain | ( | ) |
long CEpos2::getCurrentPGain | ( | ) |
int CEpos2::getDigInExecutionMask | ( | ) |
Gives Digital Inputs Execution Mask.
int CEpos2::getDigInFuncMask | ( | ) |
Gives Digital Inputs Functionalities Mask.
int CEpos2::getDigInPolarity | ( | ) |
Gives Digital Inputs Polarity Mask.
int CEpos2::getDigInState | ( | int | digitalIN = 2 | ) |
Gives the state of a certain digital Input.
int CEpos2::getDigInStateMask | ( | ) |
Gives Digital Inputs State Mask.
void CEpos2::getEncoderParameters | ( | long & | pulses, |
long & | type, | ||
long & | polarity | ||
) |
long CEpos2::getEncoderPolarity | ( | ) |
long CEpos2::getEncoderPulseNumber | ( | ) |
long CEpos2::getEncoderType | ( | ) |
long CEpos2::getHomePosition | ( | ) |
long CEpos2::getMaxAcceleration | ( | void | ) |
long CEpos2::getMaxFollowingError | ( | ) |
long CEpos2::getMaxPositionLimit | ( | ) |
long CEpos2::getMinPositionLimit | ( | ) |
long CEpos2::getMotorContinuousCurrentLimit | ( | ) |
long CEpos2::getMotorOutputCurrentLimit | ( | ) |
void CEpos2::getMotorParameters | ( | long & | type, |
long & | current_c_mA, | ||
long & | current_out_mA, | ||
char & | pole_pairs, | ||
long & | time_ds | ||
) |
function to GET encoder parameters
&type | |
¤t_c_mA | |
¤t_out_mA | |
&pole_pairs | |
&time_ds |
long CEpos2::getMotorPolePairNumber | ( | ) |
long CEpos2::getMotorType | ( | ) |
void CEpos2::getMovementInfo | ( | ) |
prints information about movement
Motor position, velocity, averaged velocity, demanded velocity, current, averaged current, demanded current.
|
private |
long CEpos2::getOperationMode | ( | ) |
std::string CEpos2::getOpModeDescription | ( | long | opmode | ) |
long CEpos2::getPositionAFFGain | ( | ) |
long CEpos2::getPositionDGain | ( | ) |
long CEpos2::getPositionDimensionIndex | ( | ) |
long CEpos2::getPositionIGain | ( | ) |
long CEpos2::getPositionMarker | ( | int | buffer = 0 | ) |
gets the position marked by the sensor
buffer | 0: captured position, 1: history 1, 2: history 2 |
long CEpos2::getPositionNotationIndex | ( | ) |
long CEpos2::getPositionPGain | ( | ) |
long CEpos2::getPositionVFFGain | ( | ) |
long CEpos2::getPositionWindow | ( | ) |
long CEpos2::getPositionWindowTime | ( | ) |
long CEpos2::getProfileAcceleration | ( | void | ) |
void CEpos2::getProfileData | ( | long & | vel, |
long & | maxvel, | ||
long & | acc, | ||
long & | dec, | ||
long & | qsdec, | ||
long & | maxacc, | ||
long & | type | ||
) |
function to GET all data of the velocity profile
This function gets all data and stores it in the output parameters defined.
&vel | Profile Velocity |
&maxvel | Profile Max Velocity |
&acc | Profile Acceleration |
&dec | Profile Deceleration |
&qsdec | Profile Quick Stop Deceleration |
&maxacc | Profile Max Acceleration |
&type | Profile Type |
long CEpos2::getProfileDeceleration | ( | void | ) |
long CEpos2::getProfileMaxVelocity | ( | void | ) |
long CEpos2::getProfileQuickStopDecel | ( | void | ) |
long CEpos2::getProfileType | ( | void | ) |
long CEpos2::getProfileVelocity | ( | void | ) |
long CEpos2::getRS232Baudrate | ( | ) |
long CEpos2::getRS232FrameTimeout | ( | ) |
long CEpos2::getState | ( | ) |
function to get current controller's state
EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
long CEpos2::getTargetCurrent | ( | ) |
long CEpos2::getTargetProfilePosition | ( | ) |
function to GET the target position
This function gets the target position of profile position operation mode
long CEpos2::getTargetProfileVelocity | ( | ) |
[OPMODE=profile_velocity] function to get the velocity
long CEpos2::getTargetVelocity | ( | ) |
function to GET the target velocity This function gets the target velocity of velocity operation mode
long CEpos2::getThermalTimeCtWinding | ( | ) |
long CEpos2::getUSBFrameTimeout | ( | ) |
long CEpos2::getVelocityDimensionIndex | ( | ) |
long CEpos2::getVelocityIGain | ( | ) |
long CEpos2::getVelocityNotationIndex | ( | ) |
long CEpos2::getVelocityPGain | ( | ) |
long CEpos2::getVelocitySetPointFactorPGain | ( | ) |
long CEpos2::getVelocityWindow | ( | ) |
long CEpos2::getVelocityWindowTime | ( | ) |
void CEpos2::init | ( | ) |
Connects hardware.
bool CEpos2::isTargetReached | ( | ) |
function to know if the motor has reached the target position or velocity
|
private |
|
private |
void CEpos2::printControlParameters | ( | long | cp, |
long | ci, | ||
long | vp, | ||
long | vi, | ||
long | vspf, | ||
long | pp, | ||
long | pi, | ||
long | pd, | ||
long | pv, | ||
long | pa | ||
) |
function to show all control parameters
Prints all control parameters got before with getControlParameters in cout
cp | Current Proportional |
ci | Current Integral |
vp | Velocity Proportional |
vi | Velocity Integral |
vspf | Velocity Set Point Factor P Gain |
pp | Postition Proportional |
pi | Position Integral |
pd | Position Derivative |
pv | Position Velocity Feed Forward Factor |
pa | Position Acceleration Feed Forward Factor |
void CEpos2::quickStop | ( | ) |
function to reach switch_on state
Transitions: 7,10,11 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
long CEpos2::readCurrent | ( | ) |
function to read motor current
This function reads the actual current of the motor [mA]
long CEpos2::readCurrentAveraged | ( | ) |
function to read motor averaged current
The current actual value averaged [mA] represents the current actual value filtered by 1st order digital lowpass filter with a cut-off frequency of 50 Hz.This function reads the current of the motor averaged [mA]
long CEpos2::readCurrentDemanded | ( | ) |
function to read current demanded
The «Current Demand Value» is the set value [mA] for the current controller
[mA]
long CEpos2::readEncoderCounter | ( | ) |
function to read the Encoder Counter
This function reads the internal counter register of the encoder multiplied by Polarity [qc]
long CEpos2::readEncoderCounterAtIndexPulse | ( | ) |
function to read the Encoder Counter at index pulse
This function reads the encoder counter reached at last detected encoder index pulse. [qc]
char CEpos2::readError | ( | ) |
function to read an Error information
This function reads the information of an error
void CEpos2::readErrorHistory | ( | long * | error[5] | ) |
function to read last 5 errors
This function reads the information of last five errors in EPOS
long CEpos2::readFollowingError | ( | ) |
function to read the Following Error
This function reads the actual Following Error [qc]
long CEpos2::readHallsensorPattern | ( | ) |
function to read the Hall Sensor Pattern
This function reads the actual state of the three hall sensors as a pattern. bit 0: hallsensor 1 bit 1: hallsensor 2 bit 2: hallsensor 3
|
private |
function to read an object from the EPOS2
This function sends a read object request using CFTDI.
index | the hexadecimal index of the object you want to read |
subindex | hexadecimal value of the object (usually 0x00) |
int32_t CEpos2::readPosition | ( | ) |
function to read motor position
This function reads the actual current of the motor. [qc] To get load position just do: load_pos[º] = motor_pos[qc] / ( 4 * encoder_pulses [lines/rev] * reduction )
long CEpos2::readStatusWord | ( | ) |
function to read EPOS2 StatusWord
This function reads the StatusWord. It doesn't give more information than just the returned StatusWord
long CEpos2::readVelocity | ( | ) |
function to read motor average velocity
This function reads the actual velocity of the motor [rev/min] -2147483648..2147483647
long CEpos2::readVelocityActual | ( | ) |
function to read velocity sensor actual
Description The velocity actual value is coupled to the velocity used as input to velocity controller [Velocity units]. Remarks The resolution of the short time velocity measurement (Velocity actual value, Velocity sensor actual value) is dependent on the encoder pulse number (Sensor Configuration) and the velocity measurement method (Miscellaneous Configuration bit 3). To improve the short time velocity measurement resolution set the Miscellaneous Configuration bit 3 to 1 or use an encoder with higher resolution. For example the short time velocity resolution with a 500-pulse encoder and Miscellaneous Configuration Bit 3 = 0 is: 1 quadcount / ms = 60’000 / (4 * 500) = 30 rpm.
long CEpos2::readVelocityDemand | ( | ) |
function to read velocity demand
Velocity demand value is generated by profile generator and is the set value for the velocity controller
long CEpos2::readVelocitySensorActual | ( | ) |
function to read velocity sensor actual
Description The velocity sensor actual value is given in quadcounts per second [inc/s]. Remarks The resolution of the short time velocity measurement (Velocity actual value, Velocity sensor actual value) is dependent on the encoder pulse number (Sensor Configuration) and the velocity measurement method (Miscellaneous Configuration bit 3). To improve the short time velocity measurement resolution set the Miscellaneous Configuration bit 3 to 1 or use an encoder with higher resolution. For example the short time velocity resolution with a 500-pulse encoder and Miscellaneous Configuration Bit 3 = 0 is: 1 quadcount / ms = 60’000 / (4 * 500) = 30 rpm.
[qc/s]
long CEpos2::readVersionHardware | ( | ) |
function to read the hardware version
This function reads the hardware version.
long CEpos2::readVersionSoftware | ( | ) |
function to read the software version
This function reads the software version, it can be useful to know if EPOS2 has the last firmware.
|
private |
function receive a frame from EPOS2
It uses CFTDI events in order to get complete frames of data from EPOS2. While frame is being received it calculates its length using the fourth piece of data and checks if data pieces have been stuffed for character 0x90, If there are, frame length is changed. Also It converts the 8 bit received frame to 16 bit frame.
frame | data frame which will be saved from EPOS2 |
void CEpos2::restoreDefaultParameters | ( | ) |
function to restore default parameters of EPOS2.
This function restores all default parameters of EPOS2.
void CEpos2::saveParameters | ( | ) |
function to save all parameters in EEPROM
This function save all actual parameters to EEPROM of EPOS2. Its necessary if you do changes you want to save such as new controller parameters or profiles.
std::string CEpos2::searchErrorDescription | ( | long | error_code | ) |
|
private |
void CEpos2::setAccelerationDimensionIndex | ( | long | Dimension | ) |
function to set Acceleration Dimension Index
This function sets Acceleration Dimension Index
Dimension | Dimension and Notation Factor Tables |
void CEpos2::setAccelerationNotationIndex | ( | long | notation | ) |
function to set Acceleration Notation Index
This function sets Acceleration Notation Index
notation | Dimension and Notation Factor Tables |
void CEpos2::setAnalogOutput1 | ( | long | voltage_mV | ) |
void CEpos2::setControlParameters | ( | long | cp, |
long | ci, | ||
long | vp, | ||
long | vi, | ||
long | vspf, | ||
long | pp, | ||
long | pi, | ||
long | pd, | ||
long | pv, | ||
long | pa | ||
) |
function to set all control parameters
Sets all control parameters
cp | Current Proportional |
ci | Current Integral |
vp | Velocity Proportional |
vi | Velocity Integral |
vspf | Velocity Set Point Factor P Gain |
pp | Postition Proportional |
pi | Position Integral |
pd | Position Derivative |
pv | Position Velocity Feed Forward Factor |
pa | Position Acceleration Feed Forward Factor |
void CEpos2::setCurrentIGain | ( | long | gain | ) |
void CEpos2::setCurrentPGain | ( | long | gain | ) |
void CEpos2::setEncoderParameters | ( | long | pulses, |
long | type, | ||
long | polarity | ||
) |
void CEpos2::setEncoderPolarity | ( | long | polarity | ) |
function to SET the Encoder polarity
This function sets the Encoder polarity bit 0: Incremental Encoder (0:CCW, 1:CW (encoder mounted on motor shaft) bit 1: Hall s (0:normal, 1:inverted) bit 2: SSI Encoder (0: CCW, 1:inverted)
DISABLE STATE only
polarity |
void CEpos2::setEncoderPulseNumber | ( | long | pulses | ) |
void CEpos2::setEncoderType | ( | long | type | ) |
void CEpos2::setHome | ( | ) |
function to set a Home position with user interaction
This function sets a new home position with user interaction. User has to follow instructions given when the function is called. It disables power, then user has to rotate the load and then the function reads the actual position and set there the new home
disable -> user rotate -> readPosition -> setHomePosition
void CEpos2::setHomePosition | ( | long | home_position_qc | ) |
void CEpos2::setHoming | ( | int | home_method = 11 , |
int | speed_pos = 100 , |
||
int | speed_zero = 100 , |
||
int | acc = 1000 , |
||
int | digitalIN = 2 |
||
) |
Sets the configuration of a homing operation.
It can use a sensor to reach the home, this sensor can be set as positive (18,2), negative(17,1) or home(27,23,11,7) switch. If the system doesn't have a switch, setting actual position as home (35) and Index methods (34,33) can be used.
home_method | # Description 35 Set Actual Position as home position 34 Index Negative / Positive Speed 33 Index Negative / Positive Speed 27 Home Switch Negative Speed 23 Home Switch Positive Speed 18 Positive Limit Switch 17 Negative Limit Switch 11 Home Switch Negative Speed & Index 7 Home Switch Positive Speed & Index 2 Positive Limit Switch & Index 1 Negative Limit Switch & Index 0 No homing operation required -1 Current Threshold Positive Speed & Index -2 Current Threshold Negative Speed & Index -3 Current Threshold Positive Speed -4 Current Threshold Negative Speed |
speed_pos | Speed for first positioning |
speed_zero | Speed for find home near 0 |
acc | Acceleration |
digitalIN | The pin which has the home switch property |
void CEpos2::setMaxAcceleration | ( | long | max_acceleration | ) |
void CEpos2::setMaxFollowingError | ( | long | follerror | ) |
void CEpos2::setMaxPositionLimit | ( | long | limit | ) |
void CEpos2::setMinPositionLimit | ( | long | limit | ) |
void CEpos2::setMotorContinuousCurrentLimit | ( | long | current_mA | ) |
void CEpos2::setMotorOutputCurrentLimit | ( | long | current_mA | ) |
void CEpos2::setMotorParameters | ( | long | type, |
long | current_c_mA, | ||
long | current_out_mA, | ||
char | pole_pairs, | ||
long | time_ds | ||
) |
function to SET motor parameters
type | |
current_c_mA | |
current_out_mA | |
pole_pairs | |
time_ds |
void CEpos2::setMotorPolePairNumber | ( | char | pole_pairs | ) |
void CEpos2::setMotorType | ( | long | type | ) |
void CEpos2::setOperationMode | ( | long | opmode | ) |
function to set the operation mode
opmode | desired operation mode (one of epos_opmodes) |
void CEpos2::setPositionAFFGain | ( | long | gain | ) |
void CEpos2::setPositionDGain | ( | long | gain | ) |
void CEpos2::setPositionDimensionIndex | ( | long | Dimension | ) |
function to set Position Dimension Index
This function sets Position Dimension Index
Dimension | Dimension and Notation Factor Tables |
void CEpos2::setPositionIGain | ( | long | gain | ) |
void CEpos2::setPositionMarker | ( | char | polarity = 0 , |
char | edge_type = 0 , |
||
char | mode = 0 , |
||
char | digitalIN = 2 |
||
) |
Sets the configuration of a marker position.
digitalIN | The pin which has the Marker position switch property |
polarity | 0: true on high, 1: true on low |
edge_type | 0 : rising, 1: falling, 2: both |
mode | 0: continuous 1: single 2: multiple |
void CEpos2::setPositionNotationIndex | ( | long | notation | ) |
function to set Position Notation Index
This function sets Position Notation Index
notation | Dimension and Notation Factor Tables |
void CEpos2::setPositionPGain | ( | long | gain | ) |
void CEpos2::setPositionVFFGain | ( | long | gain | ) |
void CEpos2::setPositionWindow | ( | long | window_qc | ) |
void CEpos2::setPositionWindowTime | ( | long | time_ms | ) |
void CEpos2::setProfileAcceleration | ( | long | acceleration | ) |
void CEpos2::setProfileData | ( | long | vel, |
long | maxvel, | ||
long | acc, | ||
long | dec, | ||
long | qsdec, | ||
long | maxacc, | ||
long | type | ||
) |
function to SET all data of the velocity profile
This function sets all data of the velocity profile.
vel | Profile Velocity |
maxvel | Profile Max Velocity |
acc | Profile Acceleration |
dec | Profile Deceleration |
qsdec | Profile Quick Stop Deceleration |
maxacc | Profile Mac Acceleration |
type | Profile Type |
void CEpos2::setProfileDeceleration | ( | long | deceleration | ) |
void CEpos2::setProfileMaxVelocity | ( | long | velocity | ) |
void CEpos2::setProfileQuickStopDecel | ( | long | deceleration | ) |
void CEpos2::setProfileType | ( | long | type | ) |
void CEpos2::setProfileVelocity | ( | long | velocity | ) |
void CEpos2::setRS232Baudrate | ( | long | baudrate | ) |
function to SET the RS232 Baudrate
This function sets the RS232 Baudrate (Firmware 14.36) 0: 9.6 kBaud 1: 14.4 kBaud 2: 19.2 kBaud 3: 38.4 kBaud 4: 57.6 kBaud 5: 115.2 kBaud
It needs to saveParameters and then restart EPOS2
baudrate | desired following the table |
void CEpos2::setRS232FrameTimeout | ( | long | timeout | ) |
void CEpos2::setTargetCurrent | ( | long | current | ) |
void CEpos2::setTargetProfilePosition | ( | long | position | ) |
function to SET the target position
This function sets the target position of profile position operation mode
position | Target position |
void CEpos2::setTargetProfileVelocity | ( | long | velocity | ) |
[OPMODE=profile_velocity] function to set the velocity
velocity | desired velocity |
void CEpos2::setTargetVelocity | ( | long | velocity | ) |
function to SET the target velocity
This function sets the target velocity of velocity operation mode
velocity | Target Velocity |
void CEpos2::setThermalTimeCtWinding | ( | long | time_ds | ) |
void CEpos2::setUSBFrameTimeout | ( | long | timeout | ) |
void CEpos2::setVelocityDimensionIndex | ( | long | Dimension | ) |
function to set Velocity Dimension Index
This function sets Velocity Dimension Index
Dimension | Dimension and Notation Factor Tables |
void CEpos2::setVelocityIGain | ( | long | gain | ) |
void CEpos2::setVelocityNotationIndex | ( | long | notation | ) |
function to set Velocity Notation Index
This function sets Velocity Notation Index
notation | Dimension and Notation Factor Tables |
void CEpos2::setVelocityPGain | ( | long | gain | ) |
void CEpos2::setVelocitySetPointFactorPGain | ( | long | gain | ) |
void CEpos2::setVelocityWindow | ( | long | window_rm | ) |
void CEpos2::setVelocityWindowTime | ( | long | time_ms | ) |
void CEpos2::setVerbose | ( | bool | verbose | ) |
void CEpos2::shutdown | ( | ) |
function to reach ready_to_switch_on state
Transitions: 2,6 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
void CEpos2::startCurrent | ( | ) |
void CEpos2::startProfilePosition | ( | epos_posmodes | mode, |
bool | blocking = true , |
||
bool | wait = true , |
||
bool | new_point = true |
||
) |
function to move the motor to a position in profile position mode
This function makes move the motor if profile position mode is the actual operation mode epos_posmodes: halt:0, absolut:1, relative:2
mode | epos_posmodes |
blocking | If it block the program or if it leave the program manage position reached |
wait | If it has to wait current movement to finish or just start the new one |
new_point | Assume Target position (seems that it doesn't do anything) |
void CEpos2::startProfileVelocity | ( | ) |
[OPMODE=profile_velocity] function to move the motor in a velocity
void CEpos2::startVelocity | ( | ) |
function to move the motor in Velocity mode
This function makes move the motor if velocity mode is the actual operation mode
void CEpos2::stopCurrent | ( | ) |
void CEpos2::stopProfileVelocity | ( | ) |
[OPMODE=profile_velocity] function to stop the motor
void CEpos2::stopVelocity | ( | ) |
function to stop the motor in velocity mode
This function stops the motor if velocity mode is the actual operation mode
void CEpos2::switchOn | ( | ) |
function to reach switch_on state
Transitions: 3 EPOS2 State Machine with libEPOS2 functions See EPOS2 state machine
|
static |
Thread listens to target reached.
param |
void CEpos2::waitPositionMarker | ( | ) |
|
private |
function to write an object to the EPOS2
This function sends a write object request using CFTDI.
index | the hexadecimal index of the object you want to read |
subindex | hexadecimal value of the object (usually 0x00) |
data | information to send |
|
static |
|
static |
|
staticprivate |
a reference to the FTDI USB device
This attribute points to the communication device used to send and receive data to and from the EPOS2. The communication device is cretaed inside the object at initialization time using the CFTDIServer and the description or serial number of the desired platform via the connect() function.
It is not possible to send or receive data to or from the epos2 until the driver is not connected to the hardware platform. Any attempt to do so will result in an exception being thrown.