IMU 센서 추가
InertialUnit 은 webot의 피치, 롤 및 요 각도 만 제공합니다. 가속, 자이로 스코프 및 자력계 데이터가 필요한 경우 가속도계 , 자이로 및 나침반 구성 요소 를 추가해야합니다 . 이러한 센서에 엔티티를 추가하는 방법은 기본적으로 동일합니다. 데이터 읽기를위한 인터페이스 만 상당히 다릅니다. 둘째, InertialUnit (단위 : rad) 가속도계 출력 가속도 값 (단위 : m / s ^ 2), Gyro에서 출력되는 자세 각도에 주의하십시오. 출력은 차체의 회전 속도입니다 (단위는 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 각도 데이터 판독 코드 세그먼트 : ( 단위는 라디안 / rad )
std::cout<< "IMU roll: " <<imu->getRollPitchYaw()[0]<< " pitch: " <<imu->getRollPitchYaw()[1]
<< " yaw: " <<imu->getRollPitchYaw()[2] <<std::endl;
2 자이로 추가
2.1 자이로 엔티티 추가
Gyro Gyro 엔티티를 추가하는 단계는 InertialUnit 엔티티를 추가하는 단계와 동일합니다. step1 : 에서 Gyro 엔티티 를 선택하기 만하면되고 후속 작업은 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 가속도계 엔티티 추가
Gyro Gyro 엔티티를 추가하는 단계는 InertialUnit 엔티티를 추가하는 단계와 동일합니다. step1 : 에서 Gyro 엔티티 를 선택하기 만하면되고 후속 작업은 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 나침반 엔티티 추가
Gyro Gyro 엔티티를 추가하는 단계는 InertialUnit 엔티티를 추가하는 단계와 동일합니다. step1 : 에서 Gyro 엔티티 를 선택하기 만하면되고 후속 작업은 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
댓글 영역 ([email protected])에서 교환하고 토론 할 모든 사람을 환영합니다.