int webots::Robot::step(int ms) {
// play motions if any
Motion::playMotions();
double actualTime = getTime() * 1000;
int stepDuration = actualTime - mPreviousStepTime;
std::map<const std::string, int>::iterator motorIt;
std::map<const std::string, int>::iterator position_sensorIt;
// -------- Update speed of each motors, according to acceleration limit if set -------- //
for (motorIt = Motor::mNamesToIDs.begin() ; motorIt != Motor::mNamesToIDs.end(); ++motorIt ) {
Motor *motor = static_cast <Motor *> (mDevices[(*motorIt).first]);
motor->updateSpeed(stepDuration);
}
// -------- Bulk Read to read the actuators states (position, speed and load) and body sensors -------- //
if (!(::Robot::MotionManager::GetInstance()->GetEnable())) // If MotionManager is enable, no need to execute the BulkRead, the MotionManager has allready done it.
mCM730->BulkRead();
// Motors
for (motorIt = Motor::mNamesToIDs.begin() ; motorIt != Motor::mNamesToIDs.end(); ++motorIt) {
Motor *motor = static_cast <Motor *> (mDevices[(*motorIt).first]);
int motorId = (*motorIt).second;
motor->setPresentSpeed( mCM730->m_BulkReadData[motorId].ReadWord(::Robot::MX28::P_PRESENT_SPEED_L));
motor->setPresentLoad( mCM730->m_BulkReadData[motorId].ReadWord(::Robot::MX28::P_PRESENT_LOAD_L));
int limit = mCM730->m_BulkReadData[motorId].ReadWord(::Robot::MX28::P_TORQUE_LIMIT_L);
if (limit == 0) {
cerr << "Alarm detected on id = " << motorId << endl;
exit(EXIT_FAILURE);
}
}
// Position sensors
for (position_sensorIt = PositionSensor::mNamesToIDs.begin() ; position_sensorIt != PositionSensor::mNamesToIDs.end(); ++position_sensorIt) {
PositionSensor *position_sensor = static_cast <PositionSensor *> (mDevices[(*position_sensorIt).first]);
int position_sensorId = (*position_sensorIt).second;
position_sensor->setPresentPosition( mCM730->m_BulkReadData[position_sensorId].ReadWord(::Robot::MX28::P_PRESENT_POSITION_L));
}
int values[3];
// Gyro
values[0] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_GYRO_X_L);
values[1] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_GYRO_Y_L);
values[2] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_GYRO_Z_L);
((Gyro *)mDevices["Gyro"])->setValues(values);
// Accelerometer
values[0] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_ACCEL_X_L);
values[1] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_ACCEL_Y_L);
values[2] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_ACCEL_Z_L);
((Accelerometer *)mDevices["Accelerometer"])->setValues(values);
// Led states
values[0] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_LED_HEAD_L);
values[1] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_LED_EYE_L);
values[2] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadByte(::Robot::CM730::P_LED_PANNEL);
((LED *)mDevices["HeadLed"])->setColor(values[0]);
((LED *)mDevices["EyeLed"])->setColor(values[1]);
LED::setBackPanel(values[2]);
// push button state (TODO: check with real robot that the masks are correct)
//values[0] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_BUTTON) & 0x1;
//values[1] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_BUTTON) & 0x2;
//values[2] = mCM730->m_BulkReadData[::Robot::CM730::ID_CM].ReadWord(::Robot::CM730::P_BUTTON) & 0x4;
// -------- Sync Write to actuators -------- //
const int msgLength = 9; // id + P + Empty + Goal Position (L + H) + Moving speed (L + H) + Torque Limit (L + H)
int param[20 * msgLength];
int n = 0;
int changed_motors = 0;
int value;
for (motorIt = Motor::mNamesToIDs.begin() ; motorIt != Motor::mNamesToIDs.end(); ++motorIt ) {
Motor *motor = static_cast <Motor *> (mDevices[(*motorIt).first]);
int motorId = (*motorIt).second;
if (motor->getTorqueEnable() && !(::Robot::MotionStatus::m_CurrentJoints.GetEnable(motorId))) {
param[n++] = motorId;
param[n++] = motor->getPGain();
param[n++] = 0; // Empty
// TODO: controlPID should be implemented there
value = motor->getGoalPosition();
param[n++] = ::Robot::CM730::GetLowByte(value);
param[n++] = ::Robot::CM730::GetHighByte(value);
value = motor->getMovingSpeed();
param[n++] = ::Robot::CM730::GetLowByte(value);
param[n++] = ::Robot::CM730::GetHighByte(value);
value = motor->getTorqueLimit();
param[n++] = ::Robot::CM730::GetLowByte(value);
param[n++] = ::Robot::CM730::GetHighByte(value);
changed_motors++;
}
}
mCM730->SyncWrite(::Robot::MX28::P_P_GAIN, msgLength, changed_motors , param);
// -------- Keyboard Reset ----------- //
if (mKeyboardEnable == true)
mKeyboard->resetKeyPressed();
// -------- Timing management -------- //
if (stepDuration < ms) { // Step to short -> wait remaining time
usleep((ms - stepDuration) * 1000);
mPreviousStepTime = actualTime;
return 0;
}
else { // Step to long -> return step duration
mPreviousStepTime = actualTime;
return stepDuration;
}
}
