IMUセンサーを追加
InertialUnitが提供するのは、ロボットのピッチ、ロール、ヨーの角度のみです。加速度、ジャイロスコープ、磁力計のデータが必要な場合は、加速度計、ジャイロ、コンパスのコンポーネントを追加する必要があります。これらのセンサーにエンティティを追加する方法は基本的に同じです。データ読み取り用のインターフェイスのみがかなり異なります。次に、InertialUnit（単位はrad）から出力される姿勢角に注意してください。加速度計の出力加速度値（単位はm / s ^ 2）、ジャイロ出力は車体の回転速度です（単位はrad / s）。
1.姿勢測定センサーInertialUnitを追加します
1.1 InertialUnitエンティティを追加する
ステップ1： InertialUnit（IMUセンサー）をロボットに追加するステップ
2：シェイプノードをこのInertialUnitセンサーの子に設定し、外観とシェイプを設定します。半径0.015と高さ0.01を設定します。オフセットは（x = 0、y = 0.02、z = 0）です
。step3： IMUセンサーの名前を設定します（これはコントローラーで使用されます）
注：IMUの追加プロセス中にboundingObjectを設定しないでくださいプロパティと物理プロパティ。
1.2 InertialUnit読み取りコードを追加する
初期化コードスニペット：
#include <webots/InertialUnit.hpp> //头文件
InertialUnit *imu;
imu== robot->getInertialUnit("imu");
imu->enable(TIME_STEP);
IMU角度データ読み取りコードセグメント：（単位はラジアン/ラジアン）
std::cout<< "IMU roll: " <<imu->getRollPitchYaw()[0]<< " pitch: " <<imu->getRollPitchYaw()[1]
<< " yaw: " <<imu->getRollPitchYaw()[2] <<std::endl;
2ジャイロを追加
2.1ジャイロエンティティを追加
ジャイロジャイロエンティティを追加する手順は、InertialUnitエンティティを追加することと同じです。あなたが唯一する必要がでジャイロエンティティを選択STEP1 :,およびその後の操作があると同じSTEP2とSTEP3。
2.2ジャイロ読み取りコードを追加する
初期化コードスニペット：
Gyro *gyro;
gyro=robot->getGyro("gyro");
gyro->enable(TIME_STEP);
ジャイロデータ読み取りコードセグメント：（単位はrad / s）
std::cout<< "Gyro X: " <<gyro->getValues()[0]
<< " Y: " <<gyro->getValues()[1]<< " Z: " <<gyro->getValues()[2] <<std::endl;
3加速度計を追加する加速度計
3.1加速度計エンティティを追加する
ジャイロジャイロエンティティを追加する手順は、InertialUnitエンティティを追加することと同じです。あなたが唯一する必要がでジャイロエンティティを選択STEP1 :,およびその後の操作があると同じSTEP2とSTEP3。
3.2加速度計の読み取りコードを追加する
初期化コードスニペット：
Gyro *gyro;
gyro=robot->getGyro("gyro");
gyro->enable(TIME_STEP);
加速度データを読み取るためのコードセグメント：（単位はm / s ^ 2）
std::cout<< "Gyro X: " <<gyro->getValues()[0]
<< " Y: " <<gyro->getValues()[1]<< " Z: " <<gyro->getValues()[2] <<std::endl;
4磁力計コンパスを追加
4.1コンパスエンティティを追加する
ジャイロジャイロエンティティを追加する手順は、InertialUnitエンティティを追加することと同じです。あなたが唯一する必要がでジャイロエンティティを選択STEP1 :,およびその後の操作があると同じSTEP2とSTEP3。
4.2コンパスの読み取りコードを追加する
初期化コードスニペット：
Compass *compass;
compass=robot->getCompass("compass");
compass->enable(TIME_STEP);
加速度データを読み取るためのコードスニペット：
std::cout<< "Compass X: " <<compass->getValues()[0]
<< " Y: " <<compass->getValues()[1]<< " Z: " <<compass->getValues()[2] <<std::endl;
5.テスト結果
完全なコントローラーコードは次のとおりです。
#include <webots/Robot.hpp>
#include <webots/GPS.hpp>
#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>
#include <webots/Keyboard.hpp>
#include <webots/InertialUnit.hpp>
#include <webots/Gyro.hpp>
#include <webots/Accelerometer.hpp>
#include <webots/Compass.hpp>
#include <stdio.h>
#define TIME_STEP 64
// All the webots classes are defined in the "webots" namespace
using namespace webots;
int main(int argc, char **argv) {
// create the Robot instance.
Robot *robot = new Robot();
Keyboard kb;
kb.enable(TIME_STEP);
DistanceSensor *ds[2];
char dsNames[2][10] = {
"ds_right","ds_left"};
for (int i = 0; i < 2; i++) {
ds[i] = robot->getDistanceSensor(dsNames[i]);
ds[i]->enable(TIME_STEP);
}
GPS *gps;
gps = robot->getGPS("global_gps");
gps->enable(TIME_STEP);
InertialUnit *imu;
imu=robot->getInertialUnit("imu");
imu->enable(TIME_STEP);
Gyro *gyro;
gyro=robot->getGyro("gyro");
gyro->enable(TIME_STEP);
Accelerometer *accelerometer;
accelerometer=robot->getAccelerometer("accelerometer");
accelerometer->enable(TIME_STEP);
Compass *compass;
compass=robot->getCompass("compass");
compass->enable(TIME_STEP);
// initialise motors
Motor *wheels[4];
char wheels_names[4][8] = {
"wheel1", "wheel2", "wheel3", "wheel4"};
for (int i = 0; i < 4; i++) {
wheels[i] = robot->getMotor(wheels_names[i]);
wheels[i]->setPosition(INFINITY);
wheels[i]->setVelocity(0);
}
printf("init successd ...\n");
double leftSpeed = 0.0;
double rightSpeed = 0.0;
// Main loop:
// - perform simulation steps until Webots is stopping the controller
while (robot->step(TIME_STEP) != -1)
{
int key = kb.getKey();
if(key== 315)
{
leftSpeed = 3.0;
rightSpeed = 3.0;
}
else if(key== 317)
{
leftSpeed = -3.0;
rightSpeed = -3.0;
}
else if(key== 314)
{
leftSpeed = -3.0;
rightSpeed = 3.0;
}
else if(key== 316)
{
leftSpeed = 3.0;
rightSpeed = -3.0;
}
else
{
leftSpeed = 0.0;
rightSpeed = 0.0;
}
std::cout<< " Right Sensor Value:" <<ds[0]->getValue() << " Left Sensor Value:" <<ds[1]->getValue() <<std::endl;
std::cout<< "GPS Value X: " <<gps->getValues()[0]
<< " Y: " <<gps->getValues()[1]<< " Z: " <<gps->getValues()[2] <<std::endl;
std::cout<< "IMU roll: " <<imu->getRollPitchYaw()[0]<< " pitch: " <<imu->getRollPitchYaw()[1]
<< " yaw: " <<imu->getRollPitchYaw()[2] <<std::endl;
std::cout<< "Gyro X: " <<gyro->getValues()[0]
<< " Y: " <<gyro->getValues()[1]<< " Z: " <<gyro->getValues()[2] <<std::endl;
std::cout<< "Accel X: " <<accelerometer->getValues()[0]
<< " Y: " <<accelerometer->getValues()[1]<< " Z: " <<accelerometer->getValues()[2] <<std::endl;
std::cout<< "Compass X: " <<compass->getValues()[0]
<< " Y: " <<compass->getValues()[1]<< " Z: " <<compass->getValues()[2] <<std::endl;
wheels[0]->setVelocity(leftSpeed);
wheels[1]->setVelocity(rightSpeed);
wheels[2]->setVelocity(leftSpeed);
wheels[3]->setVelocity(rightSpeed);
};
// Enter here exit cleanup code.
delete robot;
return 0;
}
参照
[1] 1https：//cyberbotics.com/doc/reference/index？version = R2020a-rev1
[2] https://www.cyberbotics.com/doc/reference/motion
記事があなたに役立つと思う場合は、それを助けて好きにしてください。O（∩_∩）O
コメントエリア（[email protected]）で交流し、議論する皆を歓迎します。