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; } }