Skip to content

Commit

Permalink
corrected proto file
Browse files Browse the repository at this point in the history
  • Loading branch information
@jkerpe
jkerpe committed Oct 6, 2023
1 parent 3238c16 commit bbbb67f
Show file tree
Hide file tree
Showing 5 changed files with 43 additions and 45 deletions.
22 changes: 11 additions & 11 deletions lib/HALSim/IMU.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,7 @@
* @brief IMU implementation
* @author Juliane Kerpe <[email protected]>
*
* @addtogroup HALInterfaces
* @addtogroup HALSim
*
* @{
*/
Expand Down Expand Up @@ -66,20 +66,20 @@ void IMU::init()
{
for (uint8_t axisIndex = 0; axisIndex < NUMBER_OF_AXIS; ++axisIndex)
{

m_accelerationValues[axisIndex] = 0.0;
m_gyroValue = 0.0;

m_accelerationValues[axisIndex] = 0.0;
}
m_gyroValue = 0.0;
m_accelerometer->enable(m_samplingPeriod);
m_gyro->enable(m_samplingPeriod);
std::cout << "accel an"<< std::endl;
//std::cout << "IMU.cpp: Turned on IMU."<< std::endl;

}

void IMU::calibrate()
{
// to do: implement
m_sensorCalibStarted = true;
m_sensorCalibSuccessful = true;
}


Expand All @@ -93,18 +93,18 @@ const double* IMU::getAccelerometerValues()
m_accelerationValues[axisIndex] = accelerationValues[axisIndex];
}
}
//if (abs(m_accelerationValues[0])>0.1 || abs(m_accelerationValues[1])>0.1){
std::cout<<"Accelerometer.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "<<m_accelerationValues[1]<< "; z = "<<m_accelerationValues[2]<< std::endl;
//}
std::cout<<"IMU.cpp: ACCEL: x = " << m_accelerationValues[0]<< "; y = "<<m_accelerationValues[1]<< "; z = "<<accelerationValues[2]<< std::endl;

return m_accelerationValues;
}

const double IMU::getGyroValue()
{
const double * gyroValues = m_gyro->getValues();
if (gyroValues != nullptr) {
m_gyroValue= gyroValues[INDEX_OF_Z_AXIS];
m_gyroValue = gyroValues[INDEX_OF_Z_AXIS];
}
//std::cout<<"IMU.cpp: GYRO: z = " << m_gyroValue<< std::endl;
return m_gyroValue;
}

Expand All @@ -117,7 +117,7 @@ bool IMU::isCalibrationSuccessful()
{
// to do: implement

return true;
return m_sensorCalibSuccessful;
}

/******************************************************************************
Expand Down
6 changes: 3 additions & 3 deletions lib/HALSim/IMU.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ class IMU : public IIMU
m_accelerometer(accelerometer),
m_gyro(gyro),

m_sensorCalibSuccessfull(false),
m_sensorCalibSuccessful(false),
m_sensorCalibStarted(false),
m_samplingPeriod(10)
{
Expand Down Expand Up @@ -139,7 +139,7 @@ class IMU : public IIMU
/**
* Number of used axis of the accelerometer.
*/
static const uint8_t NUMBER_OF_AXIS = 3;
static const uint8_t NUMBER_OF_AXIS = 2;


static const uint8_t INDEX_OF_Z_AXIS = 2;
Expand All @@ -149,7 +149,7 @@ class IMU : public IIMU
webots::Accelerometer* m_accelerometer; /**< The accelerometer */
webots::Gyro* m_gyro; /**< The gyro */
uint8_t m_calibErrorInfo; /**< Indicates which sensor failed the calibration, if the calibration failed. */
bool m_sensorCalibSuccessfull; /**< Indicates weather the calibration was successfull or not. */
bool m_sensorCalibSuccessful; /**< Indicates weather the calibration was successfull or not. */
bool m_sensorCalibStarted; /**< Indicates weather the calibration has started or not. */
double m_accelerationValues[NUMBER_OF_AXIS]; /**< The last value of each sensor */
double m_gyroValue;
Expand Down
41 changes: 21 additions & 20 deletions lib/RemoteControl/App.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,8 +78,8 @@ const char* App::CH_NAME_MOTOR_SPEEDS = "MOT_SPEEDS";
/* Initialize channel name for sending line sensors data. */
const char* App::CH_NAME_LINE_SENSORS = "LINE_SENS";

/* Initialize channel name for sending accelerometer data. */
const char* App::CH_NAME_ACCELEROMETER = "ACCEL_SENS";
/* Initialize channel name for sending IMU data. */
const char* App::CH_NAME_IMU = "IMU_DATA";

/** Only in remote control state its possible to control the robot. */
static bool gIsRemoteCtrlActive = false;
Expand All @@ -96,9 +96,9 @@ void App::setup()
uint8_t lineSensorChannelDlc = maxLineSensors * sizeof(uint16_t);

IIMU& imu = Board::getInstance().getIMU();
uint8_t numberOfAxis = imu.getNumberOfAccelerometerAxis();
uint8_t numberOfIMUValues = imu.getNumberOfAccelerometerAxis()+1;
// to do: check
uint8_t accelChannelDlc = numberOfAxis * sizeof(double);
uint8_t imuChannelDlc = numberOfIMUValues * sizeof(double);


m_systemStateMachine.setState(&StartupState::getInstance());
Expand All @@ -115,8 +115,8 @@ void App::setup()
/* Providing line sensor data */
m_smpChannelIdLineSensors = m_smpServer.createChannel(CH_NAME_LINE_SENSORS, lineSensorChannelDlc);

/* Providing accelerometer data */
m_smpChannelIdAccelerometer = m_smpServer.createChannel(CH_NAME_ACCELEROMETER, accelChannelDlc);
/* Providing IMU data */
m_smpChannelIdAccelerometer = m_smpServer.createChannel(CH_NAME_IMU, imuChannelDlc);
}

void App::loop()
Expand Down Expand Up @@ -153,7 +153,7 @@ void App::loop()
if (true == m_sendSensorsDataInterval.isTimeout())
{
sendLineSensorsData();
sendAccelerometerData();
sendIMUData();

m_sendSensorsDataInterval.restart();
}
Expand Down Expand Up @@ -208,22 +208,23 @@ void App::sendLineSensorsData() const
}


void App::sendAccelerometerData() const
void App::sendIMUData() const
{
IIMU& imu = Board::getInstance().getIMU();
const double* accelerationValues = imu.getAccelerometerValues();
uint8_t numberOfAxis = imu.getNumberOfAccelerometerAxis();
IIMU& imu = Board::getInstance().getIMU();
uint8_t numberOfValues = imu.getNumberOfAccelerometerAxis()+1;

uint8_t payload[numberOfAxis * sizeof(double)];
uint8_t axisIdx = 0U;

while (numberOfAxis > axisIdx)
{
Util::doubleToByteArray(&payload[axisIdx * sizeof(double)], sizeof(double),
accelerationValues[axisIdx]);
uint8_t payload[numberOfValues * sizeof(double)];

++axisIdx;
}
const double* accelerationValues = imu.getAccelerometerValues();
const double gyrovalue = imu.getGyroValue();

/** First index: acceleration value in x-direction --> index 0
* Second index: acceleration value in y-direction --> index 1
* Third index: turning rate around z-axis --> index 2
*/
Util::doubleToByteArray(&payload[0 * sizeof(double)], sizeof(double), accelerationValues[0]);
Util::doubleToByteArray(&payload[1 * sizeof(double)], sizeof(double), accelerationValues[1]);
Util::doubleToByteArray(&payload[2 * sizeof(double)], sizeof(double), gyrovalue);

(void)m_smpServer.sendData(m_smpChannelIdAccelerometer, payload, sizeof(payload));
}
Expand Down
11 changes: 7 additions & 4 deletions lib/RemoteControl/App.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,8 +112,8 @@ class App
/** SerialMuxProt channel name for sending line sensors data. */
static const char* CH_NAME_LINE_SENSORS;

/** SerialMuxProt channel name for sending line sensors data. */
static const char* CH_NAME_ACCELEROMETER;
/** SerialMuxProt channel name for sending IMU data. */
static const char* CH_NAME_IMU;

/** The system state machine. */
StateMachine m_systemStateMachine;
Expand Down Expand Up @@ -158,9 +158,12 @@ class App
void sendLineSensorsData() const;

/**
* Send acceleration data via SerialMuxProt.
* Send IMU data via SerialMuxProt.
* First index: acceleration value in x-direction --> index 0
* Second index: acceleration value in y-direction --> index 1
* Third index: turning rate around z-axis --> index 2
*/
void sendAccelerometerData() const;
void sendIMUData() const;
};

/******************************************************************************
Expand Down
8 changes: 1 addition & 7 deletions webots/protos/Zumo32U4.proto
View file
Original file line number Diff line number Diff line change
Expand Up @@ -446,16 +446,10 @@ PROTO Zumo32U4 [
children [
Accelerometer {
name "accelerometer"
lookupTable [
0 0 0
0.1 0.1 0
1 1 0
2 2 0
]
xAxis TRUE
yAxis TRUE
zAxis TRUE
resolution -1
resolution 0.01
}
Gyro {
name "gyro"
Expand Down

0 comments on commit bbbb67f

Please sign in to comment.