#include <CanCtrlPltfCOb3.h>
Represents all CAN components on an arbitrary canbus.
Definition at line 82 of file CanCtrlPltfCOb3.h.
Specify Cannodes (Identifiers to send velocities to one specific motor) This has to be adapted to the hardware-setup!
CANNODE_WHEEL1DRIVEMOTOR | |
CANNODE_WHEEL1STEERMOTOR | |
CANNODE_WHEEL2DRIVEMOTOR | |
CANNODE_WHEEL2STEERMOTOR | |
CANNODE_WHEEL3DRIVEMOTOR | |
CANNODE_WHEEL3STEERMOTOR | |
CANNODE_WHEEL4DRIVEMOTOR | |
CANNODE_WHEEL4STEERMOTOR |
Definition at line 94 of file CanCtrlPltfCOb3.h.
CanCtrlPltfCOb3::CanCtrlPltfCOb3 | ( | std::string | iniDirectory | ) |
Default constructor.
Definition at line 69 of file CanCtrlPltfCOb3.cpp.
CanCtrlPltfCOb3::~CanCtrlPltfCOb3 | ( | ) |
Default destructor.
Definition at line 225 of file CanCtrlPltfCOb3.cpp.
int CanCtrlPltfCOb3::ElmoRecordings | ( | int | iFlag, | |
int | iParam, | |||
std::string | sString | |||
) |
Provides several functions for drive information recording purposes using the built in ElmoRecorder, which allows to record drive information at a high frequency.
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 99: Abort and clear current SDO readout process 100: Request status of readout. Gives back 0 if all transmissions have finished and no CAN polling is needed anymore. |
Definition at line 1492 of file CanCtrlPltfCOb3.cpp.
int CanCtrlPltfCOb3::evalCanBuffer | ( | ) |
Triggers evaluation of the can-buffer.
Definition at line 897 of file CanCtrlPltfCOb3.cpp.
int CanCtrlPltfCOb3::getGearDeltaPosVelRadS | ( | int | iCanIdent, | |
double * | pdDeltaAngleGearRad, | |||
double * | pdVelGearRadS | |||
) |
Gets the delta joint-angle since the last call and the velocity.
iCanIdent | choose a can node | |
pdDeltaAngleGearRad | delta joint-position since the last call in radian | |
pdVelGearRadS | joint-velocity in radian per second |
Definition at line 1404 of file CanCtrlPltfCOb3.cpp.
int CanCtrlPltfCOb3::getGearPosVelRadS | ( | int | iCanIdent, | |
double * | pdAngleGearRad, | |||
double * | pdVelGearRadS | |||
) |
Gets the position and velocity.
iCanIdent | choose a can node | |
pdAngleGearRad | joint-position in radian | |
pdVelGearRadS | joint-velocity in radian per second |
Definition at line 1384 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::getMotorTorque | ( | int | iCanIdent, | |
double * | pdTorqueNm | |||
) |
Gets the motor torque (calculated from motor active current).
iCanIdent | choose a can node | |
pdTorqueNm | motor-torque in Newtonmeter |
Definition at line 1455 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::getStatus | ( | int | iCanIdent, | |
int * | piStatus, | |||
int * | piTempCel | |||
) |
Gets the status and temperature in degree celcius. (Not implemented for CanDriveHarmonica)
iCanIdent | choose a CANNode enumatraion |
Definition at line 1425 of file CanCtrlPltfCOb3.cpp.
bool CanCtrlPltfCOb3::initPltf | ( | ) |
Initializes all CAN nodes of the platfrom and performs homing procedure. !! The homing routine is hardware-dependent (steering and driving is coupled) !! !! If you use this code on other hardware -> make sure te remove or adapt homing sequence !!
Definition at line 928 of file CanCtrlPltfCOb3.cpp.
bool CanCtrlPltfCOb3::isPltfError | ( | ) |
Signs an error of the platform.
Definition at line 1240 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::readConfiguration | ( | ) | [protected] |
Definition at line 244 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::requestDriveStatus | ( | ) |
Requests the status of the drive. Status-Msg includes whether motor is in error-state (with source, e.g. undercurrent, overheated) or operational (enabled/disabled, at its limits, ...)
Definition at line 1371 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::requestMotorTorque | ( | ) |
Requests motor-torque (in fact active current) of the drive.
iCanIdent | choose a can node |
Definition at line 1442 of file CanCtrlPltfCOb3.cpp.
int CanCtrlPltfCOb3::requestMotPosVel | ( | int | iCanIdent | ) |
Requests position and velocity of the drive. (This is not implemented for CanDriveHarmonica. sending an Velocity command to an ELMO Harmonica-Ctrl triggers a aswer transmitting the current velocity)
iCanIdent | choose a can node |
Definition at line 1352 of file CanCtrlPltfCOb3.cpp.
bool CanCtrlPltfCOb3::resetPltf | ( | ) |
Reinitializes the nodes on the bus. The function might be neccessary after an emergency stop or an hardware failure to reinit drives.
Definition at line 1205 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::sendNetStartCanOpen | ( | ) | [protected] |
Starts up can node
Definition at line 1306 of file CanCtrlPltfCOb3.cpp.
void CanCtrlPltfCOb3::setMotorTorque | ( | int | iCanIdent, | |
double | dTorqueNm | |||
) |
Sends torques to the can node. Status is requested, too.
iCanIdent | choose a can node | |
dTorqueNM | motor-torque in Newtonmeter |
Definition at line 1471 of file CanCtrlPltfCOb3.cpp.
int CanCtrlPltfCOb3::setVelGearRadS | ( | int | iCanIdent, | |
double | dVelGearRadS | |||
) |
Sends veolocities to the can node. Status is requested, too.
iCanIdent | choose a can node | |
dVelGearRadS | joint-velocity in radian per second |
Definition at line 1325 of file CanCtrlPltfCOb3.cpp.
bool CanCtrlPltfCOb3::shutdownPltf | ( | ) |
Shutdown of the platform. Disables motors, enables brake and disconnects.
Definition at line 1228 of file CanCtrlPltfCOb3.cpp.
bool CanCtrlPltfCOb3::startWatchdog | ( | bool | bStarted | ) |
Starts the watchdog of the CAN components.
Definition at line 1292 of file CanCtrlPltfCOb3.cpp.
bool CanCtrlPltfCOb3::m_bWatchdogErr [protected] |
Definition at line 429 of file CanCtrlPltfCOb3.h.
CanMsg CanCtrlPltfCOb3::m_CanMsgRec [protected] |
Definition at line 427 of file CanCtrlPltfCOb3.h.
CanOpenIDType CanCtrlPltfCOb3::m_CanOpenIDParam [protected] |
Definition at line 414 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotDrive1 [protected] |
Definition at line 417 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotDrive2 [protected] |
Definition at line 418 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotDrive3 [protected] |
Definition at line 419 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotDrive4 [protected] |
Definition at line 420 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotSteer1 [protected] |
Definition at line 421 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotSteer2 [protected] |
Definition at line 422 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotSteer3 [protected] |
Definition at line 423 of file CanCtrlPltfCOb3.h.
GearMotorParamType CanCtrlPltfCOb3::m_GearMotSteer4 [protected] |
Definition at line 424 of file CanCtrlPltfCOb3.h.
IniFile CanCtrlPltfCOb3::m_IniFile [protected] |
Definition at line 434 of file CanCtrlPltfCOb3.h.
int CanCtrlPltfCOb3::m_iNumDrives [protected] |
Definition at line 437 of file CanCtrlPltfCOb3.h.
int CanCtrlPltfCOb3::m_iNumMotors [protected] |
Definition at line 436 of file CanCtrlPltfCOb3.h.
Mutex CanCtrlPltfCOb3::m_Mutex [protected] |
Definition at line 428 of file CanCtrlPltfCOb3.h.
ParamType CanCtrlPltfCOb3::m_Param [protected] |
Definition at line 412 of file CanCtrlPltfCOb3.h.
CanItf* CanCtrlPltfCOb3::m_pCanCtrl [protected] |
Definition at line 433 of file CanCtrlPltfCOb3.h.
std::vector<int> CanCtrlPltfCOb3::m_viMotorID [protected] |
Definition at line 452 of file CanCtrlPltfCOb3.h.
std::vector<CanDriveItf*> CanCtrlPltfCOb3::m_vpMotor [protected] |
Definition at line 449 of file CanCtrlPltfCOb3.h.
std::string CanCtrlPltfCOb3::sComposed [protected] |
Definition at line 247 of file CanCtrlPltfCOb3.h.
std::string CanCtrlPltfCOb3::sIniDirectory [protected] |
Reads configuration of can node and components from Inifile (should be adapted to use ROS-Parameter file)
Definition at line 246 of file CanCtrlPltfCOb3.h.