#include <CanDriveHarmonica.h>
|
| CanDriveHarmonica () |
|
bool | disableBrake (bool bDisabled) |
|
bool | endHoming () |
|
bool | evalReceivedMsg (CanMsg &msg) |
|
bool | evalReceivedMsg () |
|
void | evalSDO (CanMsg &CMsg, int *pIndex, int *pSubindex) |
|
bool | execHoming () |
|
void | getData (double *pdPosGearRad, double *pdVelGearRadS, int *piTorqueCtrl, int *piStatusCtrl) |
|
unsigned int | getError () |
|
void | getGearDeltaPosVelRadS (double *pdDeltaAngleGearRad, double *pdVelGearRadS) |
|
void | getGearPosRad (double *pdPosGearRad) |
|
void | getGearPosVelRadS (double *pdAngleGearRad, double *pdVelGearRadS) |
|
void | getMotorTorque (double *dTorqueNm) |
|
int | getSDODataInt32 (CanMsg &CMsg) |
|
void | getStatus (int *piStatus, int *piTempCel) |
|
bool | getStatusLimitSwitch () |
|
double | getTimeToLastMsg () |
|
bool | init () |
|
bool | initHoming () |
|
void | IntprtSetFloat (int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, float fData) |
|
void | IntprtSetInt (int iDataLen, char cCmdChar1, char cCmdChar2, int iIndex, int iData) |
|
bool | isError () |
|
bool | isInitialized () |
|
void | requestMotorTorque () |
|
void | requestPosVel () |
|
void | requestStatus () |
|
bool | reset () |
|
bool | resetEMStop () |
|
void | sendHeartbeat () |
|
void | sendSDOAbort (int iObjIndex, int iObjSubIndex, unsigned int iErrorCode) |
|
void | sendSDODownload (int iObjIndex, int iObjSub, int iData) |
|
void | sendSDOUpload (int iObjIndex, int iObjSub) |
|
void | setCanItf (CanItf *pCanItf) |
|
void | setCanOpenParam (int iTxPDO1, int iTxPDO2, int iRxPDO2, int iTxSDO, int iRxSDO) |
|
void | setDriveParam (DriveParam driveParam) |
|
bool | setEMStop () |
|
void | setGearPosVelRadS (double dPosRad, double dVelRadS) |
|
void | setGearVelRadS (double dVelEncRadS) |
|
void | setMotorTorque (double dTorqueNm) |
|
int | setRecorder (int iFlag, int iParam=0, std::string sParam="/home/MyLog_") |
|
bool | setTypeMotion (int iType) |
|
bool | shutdown () |
|
bool | start () |
|
bool | startWatchdog (bool bStarted) |
|
bool | stop () |
|
Driver class for the motor drive of type Harmonica.
Definition at line 33 of file CanDriveHarmonica.h.
◆ StateHarmonica
States of the drive.
Enumerator |
---|
ST_PRE_INITIALIZED | |
ST_OPERATION_ENABLED | |
ST_OPERATION_DISABLED | |
ST_MOTOR_FAILURE | |
Definition at line 50 of file CanDriveHarmonica.h.
◆ CanDriveHarmonica()
CanDriveHarmonica::CanDriveHarmonica |
( |
| ) |
|
◆ disableBrake()
bool CanDriveHarmonica::disableBrake |
( |
bool |
bDisabled | ) |
|
|
virtual |
Disables the brake. This function is not implemented for Harmonica, because brakes are released upon power on and shut only at emergency stop.
Implements CanDriveItf.
Definition at line 474 of file CanDriveHarmonica.cpp.
◆ endHoming()
bool CanDriveHarmonica::endHoming |
( |
| ) |
|
Performs homing procedure Drives wheel in neutral Position for Startup.
◆ estimVel()
double CanDriveHarmonica::estimVel |
( |
double |
dPos | ) |
|
|
protected |
◆ evalMotorFailure()
void CanDriveHarmonica::evalMotorFailure |
( |
int |
iFailure | ) |
|
|
protected |
◆ evalReceivedMsg() [1/2]
bool CanDriveHarmonica::evalReceivedMsg |
( |
CanMsg & |
msg | ) |
|
|
virtual |
Evals a received message. Only messages with fitting identifiers are evaluated.
- Parameters
-
msg | message to be evaluated. |
Implements CanDriveItf.
Definition at line 75 of file CanDriveHarmonica.cpp.
◆ evalReceivedMsg() [2/2]
bool CanDriveHarmonica::evalReceivedMsg |
( |
| ) |
|
|
inlinevirtual |
◆ evalSDO()
void CanDriveHarmonica::evalSDO |
( |
CanMsg & |
CMsg, |
|
|
int * |
pIndex, |
|
|
int * |
pSubindex |
|
) |
| |
CANopen: Evaluates a service data object and gives back object and sub-object ID
Definition at line 1015 of file CanDriveHarmonica.cpp.
◆ evalStatusRegister()
bool CanDriveHarmonica::evalStatusRegister |
( |
int |
iStatus | ) |
|
|
protected |
◆ execHoming()
bool CanDriveHarmonica::execHoming |
( |
| ) |
|
|
virtual |
◆ finishedSDOSegmentedTransfer()
void CanDriveHarmonica::finishedSDOSegmentedTransfer |
( |
| ) |
|
|
protected |
CANopen: Give the collected data of a finished segmented SDO transfer to an appropriate (depending on the current object ID) processing function. Function is called by receivedSDODataSegment, when the transfer is finished. Currently, only Elmo Recorder data is uploaded segmented and is processed here.
- See also
- receivedSDODataSegment()
Definition at line 1117 of file CanDriveHarmonica.cpp.
◆ getData()
void CanDriveHarmonica::getData |
( |
double * |
pdPosGearRad, |
|
|
double * |
pdVelGearRadS, |
|
|
int * |
piTorqueCtrl, |
|
|
int * |
piStatusCtrl |
|
) |
| |
Returns some received values from the drive.
- Deprecated:
- use the other functions instead.
- Parameters
-
pdPosGearRad | position of the drive |
pdVelGearRadS | velocity of the drive |
piTorqueCtrl | torque |
piStatusCtrl | |
Definition at line 745 of file CanDriveHarmonica.cpp.
◆ getError()
unsigned int CanDriveHarmonica::getError |
( |
| ) |
|
|
inlinevirtual |
◆ getGearDeltaPosVelRadS()
void CanDriveHarmonica::getGearDeltaPosVelRadS |
( |
double * |
pdDeltaAngleGearRad, |
|
|
double * |
pdVelGearRadS |
|
) |
| |
|
virtual |
Returns the change of the position and the velocity. The given delta position is given since the last call of this function.
Implements CanDriveItf.
Definition at line 737 of file CanDriveHarmonica.cpp.
◆ getGearPosRad()
void CanDriveHarmonica::getGearPosRad |
( |
double * |
pdPosGearRad | ) |
|
|
virtual |
◆ getGearPosVelRadS()
void CanDriveHarmonica::getGearPosVelRadS |
( |
double * |
pdAngleGearRad, |
|
|
double * |
pdVelGearRadS |
|
) |
| |
|
virtual |
◆ getMotorTorque()
void CanDriveHarmonica::getMotorTorque |
( |
double * |
dTorqueNm | ) |
|
|
virtual |
◆ getSDODataInt32()
int CanDriveHarmonica::getSDODataInt32 |
( |
CanMsg & |
CMsg | ) |
|
◆ getStatus()
void CanDriveHarmonica::getStatus |
( |
int * |
piStatus, |
|
|
int * |
piTempCel |
|
) |
| |
|
inlinevirtual |
◆ getStatusLimitSwitch()
bool CanDriveHarmonica::getStatusLimitSwitch |
( |
| ) |
|
|
virtual |
Returns the status of the limit switch needed for homing. true = limit switch is reached; false = not reached
Implements CanDriveItf.
Definition at line 487 of file CanDriveHarmonica.cpp.
◆ getTimeToLastMsg()
double CanDriveHarmonica::getTimeToLastMsg |
( |
| ) |
|
|
virtual |
◆ init()
bool CanDriveHarmonica::init |
( |
| ) |
|
|
virtual |
◆ initHoming()
bool CanDriveHarmonica::initHoming |
( |
| ) |
|
|
virtual |
◆ IntprtSetFloat()
void CanDriveHarmonica::IntprtSetFloat |
( |
int |
iDataLen, |
|
|
char |
cCmdChar1, |
|
|
char |
cCmdChar2, |
|
|
int |
iIndex, |
|
|
float |
fData |
|
) |
| |
Sends a float value to the Harmonica using the built in interpreter.
Definition at line 905 of file CanDriveHarmonica.cpp.
◆ IntprtSetInt()
void CanDriveHarmonica::IntprtSetInt |
( |
int |
iDataLen, |
|
|
char |
cCmdChar1, |
|
|
char |
cCmdChar2, |
|
|
int |
iIndex, |
|
|
int |
iData |
|
) |
| |
|
virtual |
◆ isBitSet()
bool CanDriveHarmonica::isBitSet |
( |
int |
iVal, |
|
|
int |
iNrBit |
|
) |
| |
|
inlineprotected |
◆ isError()
bool CanDriveHarmonica::isError |
( |
| ) |
|
|
virtual |
◆ isInitialized()
bool CanDriveHarmonica::isInitialized |
( |
| ) |
|
|
inlinevirtual |
Check if the driver is already initialized. This is necessary if a drive gets switched off during runtime.
- Returns
- true if initialization occured already, false if not.
Implements CanDriveItf.
Definition at line 87 of file CanDriveHarmonica.h.
◆ receivedSDODataSegment()
int CanDriveHarmonica::receivedSDODataSegment |
( |
CanMsg & |
msg | ) |
|
|
protected |
CANopen: Segment data is stored to the SDOSegmented container. Function is called, when a segment during a segmented SDO transfer is received (by evalReceivedMsg). It analyzes the SDO transfer header to see, if the transfer is finished. If it's not finished, sendSDOUploadSegmentConfirmation is called to confirm the receive of the current segment and request the next one. -Currently only used for Elmo Recorder read-out
- See also
- evalReceivedMsg()
-
sendSDOUploadSegmentConfirmation()
-
finishedSDOSegmentedTransfer()
Definition at line 1053 of file CanDriveHarmonica.cpp.
◆ receivedSDOSegmentedInitiation()
int CanDriveHarmonica::receivedSDOSegmentedInitiation |
( |
CanMsg & |
msg | ) |
|
|
protected |
CANopen: Evaluates a service data object and gives back object and sub-object ID. Function is called, when a SDO initialize transfer segment is received (by evalReceivedMsg)
- See also
- evalReceivedMsg()
Definition at line 1031 of file CanDriveHarmonica.cpp.
◆ receivedSDOTransferAbort()
void CanDriveHarmonica::receivedSDOTransferAbort |
( |
unsigned int |
iErrorCode | ) |
|
|
protected |
◆ requestMotorTorque()
void CanDriveHarmonica::requestMotorTorque |
( |
| ) |
|
|
virtual |
Sends Requests for "active current" to motor via CAN To read Motor current perform the following: 1.) request motor current m_pW1DriveMotor->requestMotorTorque(); 2.) evaluate Can buffer to read motor current and decode it evalCanBuffer();
Implements CanDriveItf.
Definition at line 783 of file CanDriveHarmonica.cpp.
◆ requestPosVel()
void CanDriveHarmonica::requestPosVel |
( |
| ) |
|
|
virtual |
◆ requestStatus()
void CanDriveHarmonica::requestStatus |
( |
| ) |
|
|
virtual |
Requests status :) checks whether motor is operational, switched off or in error state. If motor is in which error state it checks which error occured
Implements CanDriveItf.
Definition at line 777 of file CanDriveHarmonica.cpp.
◆ reset()
bool CanDriveHarmonica::reset |
( |
| ) |
|
|
virtual |
◆ resetEMStop()
bool CanDriveHarmonica::resetEMStop |
( |
| ) |
|
|
inlinevirtual |
◆ sendHeartbeat()
void CanDriveHarmonica::sendHeartbeat |
( |
| ) |
|
Sends a heartbeat to the CAN-network to keep all listening watchdogs sleeping
Definition at line 767 of file CanDriveHarmonica.cpp.
◆ sendSDOAbort()
void CanDriveHarmonica::sendSDOAbort |
( |
int |
iObjIndex, |
|
|
int |
iObjSubIndex, |
|
|
unsigned int |
iErrorCode |
|
) |
| |
CANopen: This protocol cancels an active segmented transmission due to the given Error Code
Definition at line 933 of file CanDriveHarmonica.cpp.
◆ sendSDODownload()
void CanDriveHarmonica::sendSDODownload |
( |
int |
iObjIndex, |
|
|
int |
iObjSub, |
|
|
int |
iData |
|
) |
| |
CANopen: Downloads a service data object (master to device). (in expedited transfer mode, means in only one message)
Definition at line 987 of file CanDriveHarmonica.cpp.
◆ sendSDOUpload()
void CanDriveHarmonica::sendSDOUpload |
( |
int |
iObjIndex, |
|
|
int |
iObjSub |
|
) |
| |
CANopen: Uploads a service data object (device to master). (in expedited transfer mode, means in only one message)
Definition at line 963 of file CanDriveHarmonica.cpp.
◆ sendSDOUploadSegmentConfirmation()
void CanDriveHarmonica::sendSDOUploadSegmentConfirmation |
( |
bool |
toggleBit | ) |
|
|
protected |
CANopen: Answer a data mesage during a segmented SDO transfer including a toggling bit. Current toggle bit is stored in the SDOSegmented container. Function is called, when any SDO transfer segment is received (by receivedSDODataSegment)
Definition at line 1092 of file CanDriveHarmonica.cpp.
◆ setCanItf()
void CanDriveHarmonica::setCanItf |
( |
CanItf * |
pCanItf | ) |
|
|
inlinevirtual |
◆ setCanOpenParam()
void CanDriveHarmonica::setCanOpenParam |
( |
int |
iTxPDO1, |
|
|
int |
iTxPDO2, |
|
|
int |
iRxPDO2, |
|
|
int |
iTxSDO, |
|
|
int |
iRxSDO |
|
) |
| |
Sets the CAN identifiers of the drive node.
- Parameters
-
iTxPDO1 | first transmit process data object |
iTxPDO2 | second transmit process data object |
iRxPDO2 | second receive process data object |
iTxSDO | transmit service data object |
iRxSDO | receive service data object |
Definition at line 64 of file CanDriveHarmonica.cpp.
◆ setDriveParam()
void CanDriveHarmonica::setDriveParam |
( |
DriveParam |
driveParam | ) |
|
|
inlinevirtual |
◆ setEMStop()
bool CanDriveHarmonica::setEMStop |
( |
| ) |
|
|
inlinevirtual |
◆ setGearPosVelRadS()
void CanDriveHarmonica::setGearPosVelRadS |
( |
double |
dPosRad, |
|
|
double |
dVelRadS |
|
) |
| |
|
virtual |
◆ setGearVelRadS()
void CanDriveHarmonica::setGearVelRadS |
( |
double |
dVelEncRadS | ) |
|
|
virtual |
◆ setMotorTorque()
void CanDriveHarmonica::setMotorTorque |
( |
double |
dTorqueNm | ) |
|
|
virtual |
◆ setRecorder()
int CanDriveHarmonica::setRecorder |
( |
int |
iFlag, |
|
|
int |
iParam = 0 , |
|
|
std::string |
sParam = "/home/MyLog_" |
|
) |
| |
|
virtual |
Provides several functions for drive information recording purposes using the built in ElmoRecorder, which allows to record drive information at a high frequency.
- Parameters
-
iFlag | To keep the interface slight, use iParam to command the recorder: 0: Configure the Recorder to record the sources Main Speed(1), Main position(2), Active current(10), Speed command(16). With iParam = iRecordingGap you specify every which time quantum (4*90usec) a new data point (of 1024 points in total) is recorded; 1: Query Upload of recorded source (1=Main Speed, 2=Main position, 10=Active Current, 16=Speed command) with iParam and log data to file sParam = file prefix. Filename is extended with _MotorNumber_RecordedSource.log 2: Request status of ongoing readout process 99: Abort and clear current SDO readout process |
- Returns
- 0: Success, 1: Recorder hasn't been configured yet, 2: data collection still in progress
overwrites previous collected data (even from other processes)
Implements CanDriveItf.
Definition at line 1353 of file CanDriveHarmonica.cpp.
◆ setTypeMotion()
bool CanDriveHarmonica::setTypeMotion |
( |
int |
iType | ) |
|
|
virtual |
◆ shutdown()
bool CanDriveHarmonica::shutdown |
( |
| ) |
|
|
virtual |
◆ start()
bool CanDriveHarmonica::start |
( |
| ) |
|
|
virtual |
◆ startWatchdog()
bool CanDriveHarmonica::startWatchdog |
( |
bool |
bStarted | ) |
|
|
virtual |
Starts the watchdog. The Harmonica provides watchdog functionality which means the drive stops if the watchdog becomes active. To keep the watchdog inactive a heartbeat message has to be sent periodically. The update rate is set to 1s. The update is done in function setGearVelRadS().
Implements CanDriveItf.
Definition at line 421 of file CanDriveHarmonica.cpp.
◆ stop()
bool CanDriveHarmonica::stop |
( |
| ) |
|
|
virtual |
◆ ElmoRec
◆ m_bCurrentLimitOn
bool CanDriveHarmonica::m_bCurrentLimitOn |
|
protected |
◆ m_bIsInitialized
bool CanDriveHarmonica::m_bIsInitialized |
|
protected |
◆ m_bLimitSwitchEnabled
bool CanDriveHarmonica::m_bLimitSwitchEnabled |
|
protected |
◆ m_bLimSwLeft
bool CanDriveHarmonica::m_bLimSwLeft |
|
protected |
◆ m_bLimSwRight
bool CanDriveHarmonica::m_bLimSwRight |
|
protected |
◆ m_bOutputOfFailure
bool CanDriveHarmonica::m_bOutputOfFailure |
|
protected |
◆ m_bWatchdogActive
bool CanDriveHarmonica::m_bWatchdogActive |
|
protected |
◆ m_CanMsgLast
CanMsg CanDriveHarmonica::m_CanMsgLast |
|
protected |
◆ m_CurrentTime
◆ m_dAngleGearRadMem
double CanDriveHarmonica::m_dAngleGearRadMem |
|
protected |
◆ m_dMotorCurr
double CanDriveHarmonica::m_dMotorCurr |
|
protected |
◆ m_dOldPos
double CanDriveHarmonica::m_dOldPos |
|
protected |
◆ m_dPosGearMeasRad
double CanDriveHarmonica::m_dPosGearMeasRad |
|
protected |
◆ m_DriveParam
◆ m_dVelGearMeasRadS
double CanDriveHarmonica::m_dVelGearMeasRadS |
|
protected |
◆ m_FailureStartTime
TimeStamp CanDriveHarmonica::m_FailureStartTime |
|
protected |
◆ m_iCountRequestDiv
int CanDriveHarmonica::m_iCountRequestDiv |
|
protected |
◆ m_iDistSteerAxisToDriveWheelMM
int CanDriveHarmonica::m_iDistSteerAxisToDriveWheelMM |
|
protected |
◆ m_iMotorState
int CanDriveHarmonica::m_iMotorState |
|
protected |
◆ m_iNewMotorState
int CanDriveHarmonica::m_iNewMotorState |
|
protected |
◆ m_iNumAttempsRecFail
int CanDriveHarmonica::m_iNumAttempsRecFail |
|
protected |
◆ m_iPartnerDriveRatio
int CanDriveHarmonica::m_iPartnerDriveRatio |
|
protected |
◆ m_iStatusCtrl
int CanDriveHarmonica::m_iStatusCtrl |
|
protected |
◆ m_iTorqueCtrl
int CanDriveHarmonica::m_iTorqueCtrl |
|
protected |
◆ m_iTypeMotion
int CanDriveHarmonica::m_iTypeMotion |
|
protected |
◆ m_Param
◆ m_ParamCanOpen
◆ m_pCanCtrl
CanItf* CanDriveHarmonica::m_pCanCtrl |
|
protected |
◆ m_SendTime
◆ m_sErrorMessage
std::string CanDriveHarmonica::m_sErrorMessage |
|
protected |
◆ m_StartTime
◆ m_VelCalcTime
◆ m_WatchdogTime
◆ seg_Data
segData CanDriveHarmonica::seg_Data |
|
protected |
The documentation for this class was generated from the following files: