webots::Robot::Robot() {
if (cInstance == NULL)
cInstance = this;
else {
cerr << "Only one instance of the Robot class should be created" << endl;
exit(-1);
}
initDarwinOP();
initDevices();
gettimeofday(&mStart, NULL);
mPreviousStepTime = 0.0;
mKeyboardEnable = false;
mKeyboard = new Keyboard();
// Load TimeStep from the file "config.ini"
minIni ini("config.ini");
LoadINISettings(&ini, "Robot Config");
if (mTimeStep < 16) {
cout << "The time step selected of " << mTimeStep << "ms is very small and will probably not be respected." << endl;
cout << "A time step of at least 16ms is recommended." << endl;
}
mCM730->MakeBulkReadPacketWb(); // Create the BulkReadPacket to read the actuators states in Robot::step
// Unactive all Joints in the Motion Manager //
std::map<const std::string, int>::iterator motorIt;
for (motorIt = Motor::mNamesToIDs.begin() ; motorIt != Motor::mNamesToIDs.end(); ++motorIt ) {
::Robot::MotionStatus::m_CurrentJoints.SetEnable((*motorIt).second, 0);
::Robot::MotionStatus::m_CurrentJoints.SetValue((*motorIt).second, ((Motor *) mDevices[(*motorIt).first])->getGoalPosition());
}
// Make each motors go to the start position slowly
const int msgLength = 5; // id + Goal Position (L + H) + Moving speed (L + H)
int value = 0, changed_motors = 0, n = 0;
int param[20 * msgLength];
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; // id
value = motor->getGoalPosition(); // Start position
param[n++] = ::Robot::CM730::GetLowByte(value);
param[n++] = ::Robot::CM730::GetHighByte(value);
value = 100; // small speed 100 => 11.4 rpm => 1.2 rad/s
param[n++] = ::Robot::CM730::GetLowByte(value);
param[n++] = ::Robot::CM730::GetHighByte(value);
changed_motors++;
}
}
mCM730->SyncWrite(::Robot::MX28::P_GOAL_POSITION_L, msgLength, changed_motors , param);
usleep(2000000); // wait a moment to reach start position
// Switch LED to GREEN
mCM730->WriteWord(::Robot::CM730::ID_CM, ::Robot::CM730::P_LED_HEAD_L, 1984, 0);
mCM730->WriteWord(::Robot::CM730::ID_CM, ::Robot::CM730::P_LED_EYE_L, 1984, 0);
}
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;
}
}
