/** automatically do data exchange, if not otherwise specified */
int friRemote::doPositionControl(float newJntPosition[LBR_MNJ], bool flagDataExchange)
{
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
{
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
}
}
else
{
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntPos[i]=newJntPosition[i];
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
}
void Discharger::doIdle()
{
#ifdef ENABLE_T_INTERNAL
if(isPowerOn()) {
finalizeValueTintern(false);
}
#endif
}
void Discharger::powerOff(STATE reason)
{
if(!isPowerOn() || reason == DISCHARGING)
return;
setValue(0);
hardware::setDischargerOutput(false);
state_ = reason;
}
void Discharger::powerOn()
{
if(isPowerOn())
return;
setValue(0);
hardware::setDischargerOutput(true);
state_ = DISCHARGING;
}
void SystemPowerDown()
{
while(isPowerOn() == 0) {}
EALLOW;
SysCtrlRegs.WDCR = 0x0028; // Enable watchdog module
SysCtrlRegs.WDKEY = 0x00; // wrong key should restart
SysCtrlRegs.WDKEY = 0x00;
EDIS;
while(1){}
}
void SMPS::powerOn()
{
if(isPowerOn())
return;
//reset rising value
value_ = 0;
IoutSet_ = 0;
setValue(0);
hardware::setChargerOutput(true);
state_ = CHARGING;
}
void SMPS::powerOff(STATE reason)
{
if(!isPowerOn() || reason == CHARGING)
return;
setValue(0);
//reset rising value
value_ = 0;
IoutSet_ = 0;
hardware::setChargerOutput(false);
state_ = reason;
}
void SMPS::powerOff()
{
if(!isPowerOn())
return;
setValue(0);
//reset rising value
value_ = 0;
IoutSet_ = 0;
hardware::setChargerOutput(false);
on_ = false;
}
/** automatically do data exchange, if not otherwise specified
if flagDataExchange is set to false, call doDataExchange()
or doReceiveData()/doSendData() on your own
IN: newJntPosition - joint positions
newJntStiff - joint stiffness (Spring factor)
newJntDamp - joint damping (Damping factor)
newJntAddTorque - additional torque
Note: If any of the pointers (newJntPosition, newJntStiff, newJntDamp, newJntAddTorque) is NULL, the
value is ignored -> the respective cmd.cmd.cmdFlags field is set properly
Note: It is possible to change cmd.cmd.cmdFlags in monitor mode only !!
*/
int friRemote::doJntImpedanceControl(const float newJntPosition[LBR_MNJ],
const float newJntStiff[LBR_MNJ],
const float newJntDamp[LBR_MNJ],
const float newJntAddTorque[LBR_MNJ],
bool flagDataExchange)
{
// Helper, if not properly initialized or the like...
cmd.cmd.cmdFlags=0;
if (newJntPosition)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTPOS;
// Note:: If the interface is not in Command mode,
// The commands have to be "mirrored" to get in sync
if ((getState() != FRI_STATE_CMD) || (!isPowerOn()))
{
for (int i = 0; i < LBR_MNJ; i++)
{
cmd.cmd.jntPos[i]=msr.data.cmdJntPos[i]+msr.data.cmdJntPosFriOffset[i];
}
}
else
{
// compute new values here ...
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntPos[i]=newJntPosition[i];
}
}
if (newJntStiff)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTSTIFF;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntStiffness[i]=newJntStiff[i];
}
if (newJntDamp)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTDAMP;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.jntDamping[i]=newJntDamp[i];
}
if (newJntAddTorque)
{
cmd.cmd.cmdFlags|=FRI_CMD_JNTTRQ;
for (int i = 0; i < LBR_MNJ; i++)
cmd.cmd.addJntTrq[i]=newJntAddTorque[i];
}
if (flagDataExchange)
{
return doDataExchange();
}
return 1;
}
