Skip to content

Commit

Permalink
Clip CSP targets according to max velocity
Browse files Browse the repository at this point in the history
  • Loading branch information
PeterBowman committed Jan 24, 2020
1 parent e238464 commit 4499b78
Show file tree
Hide file tree
Showing 5 changed files with 39 additions and 8 deletions.
6 changes: 3 additions & 3 deletions libraries/YarpPlugins/TechnosoftIpos/DeviceDriverImpl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config)
yarp::os::Bottle & gearboxGroup = robotConfig->findGroup(iposGroup.find("gearbox").asString());
yarp::os::Bottle & encoderGroup = robotConfig->findGroup(iposGroup.find("encoder").asString());

int canId = config.check("canId", yarp::os::Value(0), "CAN node ID").asInt32(); // id-specific
vars.canId = config.check("canId", yarp::os::Value(0), "CAN node ID").asInt32(); // id-specific

vars.actualControlMode = VOCAB_CM_NOT_CONFIGURED;

Expand All @@ -61,7 +61,7 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config)
vars.syncPeriod = iposGroup.check("syncPeriod", yarp::os::Value(0.0), "SYNC message period (seconds)").asFloat64();
vars.initialMode = iposGroup.check("initialMode", yarp::os::Value(VOCAB_CM_IDLE), "initial YARP control mode vocab").asVocab();

if (!vars.validateInitialState(canId))
if (!vars.validateInitialState())
{
CD_ERROR("Invalid configuration parameters.\n");
return false;
Expand Down Expand Up @@ -109,7 +109,7 @@ bool TechnosoftIpos::open(yarp::os::Searchable & config)
double monitorPeriod = iposGroup.check("monitorPeriod", yarp::os::Value(DEFAULT_MONITOR_PERIOD),
"monitor thread period (seconds)").asFloat64();

can = new CanOpenNode(canId, sdoTimeout, driveStateTimeout);
can = new CanOpenNode(vars.canId, sdoTimeout, driveStateTimeout);

PdoConfiguration tpdo1Conf;

Expand Down
9 changes: 6 additions & 3 deletions libraries/YarpPlugins/TechnosoftIpos/ICanBusSharerImpl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -227,9 +227,12 @@ bool TechnosoftIpos::synchronize()
std::uint64_t data = linInterpBuffer->makeDataRecord(vars.synchronousCommandTarget);
return can->rpdo3()->write(data);
}

std::int32_t data = vars.degreesToInternalUnits(vars.synchronousCommandTarget);
return can->rpdo3()->write(data);
else
{
double value = vars.clipSyncPositionTarget();
std::int32_t data = vars.degreesToInternalUnits(value);
return can->rpdo3()->write(data);
}
}
default:
return true;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -147,6 +147,7 @@ bool TechnosoftIpos::setControlModeRaw(int j, int mode)
}

vars.synchronousCommandTarget = vars.internalUnitsToDegrees(vars.lastEncoderRead.queryPosition());
vars.prevSyncTarget = vars.synchronousCommandTarget;

return can->driveStatus()->requestState(DriveState::OPERATION_ENABLED)
&& can->rpdo3()->configure(PdoConfiguration().addMapping<std::int32_t>(0x607A))
Expand Down
23 changes: 22 additions & 1 deletion libraries/YarpPlugins/TechnosoftIpos/StateVariables.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -231,6 +231,27 @@ bool StateVariables::validateInitialState(unsigned int canId)

// -----------------------------------------------------------------------------

double StateVariables::clipSyncPositionTarget() const
{
double requested = synchronousCommandTarget;
double previous = prevSyncTarget;
double diff = requested - previous;

if (std::abs(diff) > maxVel * syncPeriod)
{
CD_WARNING("Maximum velocity exceeded, clipping target position (canId: %d).\n", canId);
double newTarget = previous + maxVel * syncPeriod * sgn(diff);
prevSyncTarget = newTarget;
return newTarget;
}
else
{
return requested;
}
}

// -----------------------------------------------------------------------------

void StateVariables::reset()
{
msr = mer = der = der2 = cer = ptStatus = 0;
Expand All @@ -240,7 +261,7 @@ void StateVariables::reset()
lastCurrentRead = 0.0;

requestedcontrolMode = 0;
synchronousCommandTarget = 0.0;
synchronousCommandTarget = prevSyncTarget = 0.0;
}

// -----------------------------------------------------------------------------
Expand Down
8 changes: 7 additions & 1 deletion libraries/YarpPlugins/TechnosoftIpos/StateVariables.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -67,7 +67,7 @@ class EncoderRead
struct StateVariables
{
//! Make sure stored variables actually make sense.
bool validateInitialState(unsigned int canId);
bool validateInitialState();

//! Wait with timeout for requested control mode change.
bool awaitControlMode(yarp::conf::vocab32_t mode);
Expand All @@ -93,6 +93,9 @@ struct StateVariables
//! Convert torque (Nm) to current (amperes).
double torqueToCurrent(double torque) const;

//! Clip travelled distance according to the maximum velocity allowed.
double clipSyncPositionTarget() const;

//! Reset internal state.
void reset();

Expand Down Expand Up @@ -132,6 +135,7 @@ struct StateVariables
std::atomic<std::uint8_t> lastNmtState {0};

std::atomic<double> synchronousCommandTarget {0.0};
std::atomic<double> prevSyncTarget {0.0};

std::atomic<bool> enableCsv {false};

Expand All @@ -153,6 +157,8 @@ struct StateVariables

double heartbeatPeriod {0.0};
double syncPeriod {0.0};

unsigned int canId = 0;
};

} // namespace roboticslab
Expand Down

0 comments on commit 4499b78

Please sign in to comment.