Skip to content

Commit 6335531

Browse files
kubascikmichalkubascikmichal
committed
Automatic rotation
Edited automatic rotation
1 parent 0eaf567 commit 6335531

File tree

4 files changed

+261
-103
lines changed

4 files changed

+261
-103
lines changed

‎RoboHeart.cpp

Lines changed: 226 additions & 89 deletions
Original file line numberDiff line numberDiff line change
@@ -15,53 +15,11 @@
1515

1616
LSM6DS3 RoboHeart::imu(I2C_MODE, LSM6DS3_I2C_ADDR);
1717
float RoboHeart::_rotationX;
18-
float RoboHeart::_driftX;
1918
float RoboHeart::_rotationY;
20-
float RoboHeart::_driftY;
2119
float RoboHeart::_rotationZ;
22-
float RoboHeart::_driftZ;
23-
24-
void RoboHeart::rotationCallBack(void *pvParameter)
25-
{
26-
uint32_t start_time_us = (uint32_t)esp_timer_get_time();
27-
28-
float prev_angular_velocityX = 0;
29-
float prev_angular_velocityY = 0;
30-
float prev_angular_velocityZ = 0;
31-
while(true){
32-
uint32_t actual_time_us = (uint32_t)esp_timer_get_time();
33-
uint32_t diff_us = actual_time_us - start_time_us;
34-
if(diff_us > PERIOD_US){
35-
float angular_velocityX = imu.readFloatGyroX() - _driftX;
36-
float angular_velocityY = imu.readFloatGyroY() - _driftY;
37-
float angular_velocityZ = imu.readFloatGyroZ() - _driftZ;
38-
39-
_rotationX += ((prev_angular_velocityX+angular_velocityX)/2)*(0.000001*diff_us);
40-
_rotationY += ((prev_angular_velocityY+angular_velocityY)/2)*(0.000001*diff_us);
41-
_rotationZ += ((prev_angular_velocityZ+angular_velocityZ)/2)*(0.000001*diff_us);
42-
43-
prev_angular_velocityX = angular_velocityX;
44-
prev_angular_velocityY = angular_velocityY;
45-
prev_angular_velocityZ = angular_velocityZ;
46-
if (_rotationX > 360) {
47-
_rotationX -= 360;
48-
} else if (_rotationX < 0) {
49-
_rotationX += 360;
50-
}
51-
if (_rotationY > 360) {
52-
_rotationY -= 360;
53-
} else if (_rotationY < 0) {
54-
_rotationY += 360;
55-
}
56-
if (_rotationZ > 360) {
57-
_rotationZ -= 360;
58-
} else if (_rotationZ < 0) {
59-
_rotationZ += 360;
60-
}
61-
start_time_us = actual_time_us;
62-
}
63-
}
64-
}
20+
tIMUdata RoboHeart::imuData = {0};
21+
EventGroupHandle_t RoboHeart::xIMUeventGroup;
22+
SemaphoreHandle_t RoboHeart::xVarMutex;
6523

6624
RoboHeart::RoboHeart() {}
6725

@@ -80,6 +38,19 @@ bool RoboHeart::begin() {
8038
Wire.setPins(IMU_SDA, IMU_SCL);
8139
Wire.begin();
8240

41+
imu.settings.gyroEnabled = 1;
42+
imu.settings.gyroRange = 2000; //Max deg/s. Can be: 125, 245, 500, 1000, 2000
43+
imu.settings.gyroSampleRate = 104; //Hz. Can be: 13, 26, 52, 104, 208, 416, 833, 1666
44+
imu.settings.gyroBandWidth = 50; //Hz. Can be: 50, 100, 200, 400;
45+
46+
imu.settings.accelEnabled = 1;
47+
imu.settings.accelODROff = 0;
48+
imu.settings.accelRange = 2; //Max G force readable. Can be: 2, 4, 8, 16
49+
imu.settings.accelSampleRate = imu.settings.gyroSampleRate;
50+
imu.settings.accelBandWidth = 50; //Hz. Can be: 50, 100, 200, 400;
51+
52+
imu.settings.tempEnabled = 0;
53+
8354
byte status = imu.begin();
8455

8556
if (status != 0) {
@@ -118,20 +89,11 @@ void RoboHeart::setPWM(int motor, int freq, int pwm){
11889

11990
void RoboHeart::setAutomaticRotation(){
12091

121-
Serial.println("Automatic calibration started.");
122-
calculateDiff();
123-
int counter = 0;
124-
while (isCalibrated() == 0) {
125-
counter++;
126-
if(counter > 3){
127-
calculateDiff();
128-
counter = 0;
129-
Serial.println("Calibration failed, trying again...");
130-
Serial.println("Please let RoboHeart in stable position...");
131-
}
132-
}
133-
Serial.println("RoboHeart calibrated");
134-
xTaskCreate(&rotationCallBack, "RotationTask", 2048, NULL, 5, NULL);
92+
xVarMutex = xSemaphoreCreateMutex();
93+
xIMUeventGroup = xEventGroupCreate();
94+
95+
xTaskCreatePinnedToCore(IMUcalculateDataTask, "IMUcalculateDataTask", 8192, NULL, configMAX_PRIORITIES - 10, NULL, 0);
96+
xTaskCreatePinnedToCore(IMUgetDataTask, "IMUgetDataTask", 8192, NULL, configMAX_PRIORITIES - 10, NULL, 0);
13597
}
13698

13799
void RoboHeart::setDirectionTurnMotors(RoboHeartDRV8836& directionMotor,
@@ -195,36 +157,6 @@ char* RoboHeart::handleMotorMessage(MotorMSGType motorMSG) {
195157
return response;
196158
}
197159

198-
199-
void RoboHeart::calculateDiff(int timeout_ms){
200-
//Serial.println("Drift");
201-
_driftX = 0;
202-
_driftY = 0;
203-
_driftZ = 0;
204-
for (int i = 0; i < timeout_ms; i++) {
205-
_driftX += imu.readFloatGyroX() / timeout_ms;
206-
_driftY += imu.readFloatGyroY() / timeout_ms;
207-
_driftZ += imu.readFloatGyroZ() / timeout_ms;
208-
delay(1);
209-
}
210-
Serial.println(_driftX);
211-
Serial.println(_driftY);
212-
Serial.println(_driftZ);
213-
}
214-
215-
bool RoboHeart::isCalibrated(int timeout_ms) {
216-
unsigned long time = millis();
217-
long counter = 0;
218-
while ((millis() - time) < timeout_ms) {
219-
if (abs(imu.readFloatGyroX() - _driftX) > TRESHOLD || abs(imu.readFloatGyroY() - _driftY) > TRESHOLD || abs(imu.readFloatGyroZ() - _driftZ) > TRESHOLD) {
220-
counter++;
221-
}
222-
delay(1);
223-
}
224-
//Serial.println(counter);
225-
return (counter < (timeout_ms / 3));
226-
}
227-
228160
float RoboHeart::getRotationX(){
229161
return this->_rotationX;
230162
}
@@ -250,4 +182,209 @@ float RoboHeart::getTemperatureC(){
250182

251183
float RoboHeart::getTemperatureF(){
252184
return (getTemperatureC()* 1.8) + 32 ;
185+
}
186+
187+
void RoboHeart::computeEulerRates(float omega_x, float omega_y, float omega_z, float phi, float theta, float* dphi, float* dtheta, float* dpsi){
188+
// Transformation matrix elements
189+
float cos_phi = cosf(phi);
190+
float sin_phi = sinf(phi);
191+
float cos_theta = cosf(theta);
192+
193+
// Compute the rates of change of Euler angles
194+
*dphi = omega_x + (omega_y * sin_phi + omega_z * cos_phi) * tanf(theta);
195+
*dtheta = omega_y * cos_phi - omega_z * sin_phi;
196+
*dpsi = (omega_y * sin_phi + omega_z * cos_phi) / cos_theta;
197+
}
198+
199+
void RoboHeart::rungeKutta4(float* phi, float* theta, float* psi, float omega_x, float omega_y, float omega_z, float dt){
200+
// Intermediate variables for Runge-Kutta
201+
float k1_phi, k2_phi, k3_phi, k4_phi;
202+
float k1_theta, k2_theta, k3_theta, k4_theta;
203+
float k1_psi, k2_psi, k3_psi, k4_psi;
204+
205+
float phi_temp, theta_temp, psi_temp;
206+
float dphi, dtheta, dpsi;
207+
208+
// k1 values
209+
computeEulerRates(omega_x, omega_y, omega_z, *phi, *theta, &dphi, &dtheta, &dpsi);
210+
k1_phi = dphi * dt;
211+
k1_theta = dtheta * dt;
212+
k1_psi = dpsi * dt;
213+
214+
// k2 values
215+
phi_temp = *phi + k1_phi / 2;
216+
theta_temp = *theta + k1_theta / 2;
217+
psi_temp = *psi + k1_psi / 2;
218+
computeEulerRates(omega_x, omega_y, omega_z, phi_temp, theta_temp, &dphi, &dtheta, &dpsi);
219+
k2_phi = dphi * dt;
220+
k2_theta = dtheta * dt;
221+
k2_psi = dpsi * dt;
222+
223+
// k3 values
224+
phi_temp = *phi + k2_phi / 2;
225+
theta_temp = *theta + k2_theta / 2;
226+
psi_temp = *psi + k2_psi / 2;
227+
computeEulerRates(omega_x, omega_y, omega_z, phi_temp, theta_temp, &dphi, &dtheta, &dpsi);
228+
k3_phi = dphi * dt;
229+
k3_theta = dtheta * dt;
230+
k3_psi = dpsi * dt;
231+
232+
// k4 values
233+
phi_temp = *phi + k3_phi;
234+
theta_temp = *theta + k3_theta;
235+
psi_temp = *psi + k3_psi;
236+
computeEulerRates(omega_x, omega_y, omega_z, phi_temp, theta_temp, &dphi, &dtheta, &dpsi);
237+
k4_phi = dphi * dt;
238+
k4_theta = dtheta * dt;
239+
k4_psi = dpsi * dt;
240+
241+
// Update the Euler angles
242+
*phi += (k1_phi + 2 * k2_phi + 2 * k3_phi + k4_phi) / 6.0f;
243+
*theta += (k1_theta + 2 * k2_theta + 2 * k3_theta + k4_theta) / 6.0f;
244+
*psi += (k1_psi + 2 * k2_psi + 2 * k3_psi + k4_psi) / 6.0f;
245+
}
246+
247+
void RoboHeart::rungeKutta2(float* phi, float* theta, float* psi, float omega_x, float omega_y, float omega_z, float dt){
248+
// Intermediate variables for Runge-Kutta
249+
float k1_phi, k2_phi;
250+
float k1_theta, k2_theta;
251+
float k1_psi, k2_psi;
252+
253+
float phi_temp, theta_temp, psi_temp;
254+
float dphi, dtheta, dpsi;
255+
256+
// k1 values (slopes at the beginning of the interval)
257+
computeEulerRates(omega_x, omega_y, omega_z, *phi, *theta, &dphi, &dtheta, &dpsi);
258+
k1_phi = dphi * dt;
259+
k1_theta = dtheta * dt;
260+
k1_psi = dpsi * dt;
261+
262+
// Intermediate angles using k1 (midpoint approximation)
263+
phi_temp = *phi + k1_phi / 2;
264+
theta_temp = *theta + k1_theta / 2;
265+
psi_temp = *psi + k1_psi / 2;
266+
267+
// k2 values (slopes at the midpoint)
268+
computeEulerRates(omega_x, omega_y, omega_z, phi_temp, theta_temp, &dphi, &dtheta, &dpsi);
269+
k2_phi = dphi * dt;
270+
k2_theta = dtheta * dt;
271+
k2_psi = dpsi * dt;
272+
273+
// Update the Euler angles using the second-order RK method
274+
*phi += k2_phi;
275+
*theta += k2_theta;
276+
*psi += k2_psi;
277+
}
278+
279+
void RoboHeart::IMUgetDataTask(void *pvParameter){
280+
uint8_t status = 0;
281+
tIMUdata data = {0};
282+
283+
while(true) {
284+
imu.readRegister(&status, LSM6DS3_ACC_GYRO_STATUS_REG);
285+
if (status & LSM6DS3_ACC_GYRO_GDA_DATA_AVAIL) {
286+
imu.readRegisterRegion((uint8_t *) &data, LSM6DS3_ACC_GYRO_OUTX_L_G, 12);
287+
if (xSemaphoreTake(xVarMutex, portMAX_DELAY)) {
288+
imuData = data;
289+
xSemaphoreGive(xVarMutex);
290+
}
291+
xEventGroupSetBits(xIMUeventGroup, NEW_IMU_DATA_BIT);
292+
}
293+
vTaskDelay(((1000/imu.settings.gyroSampleRate)-1)/portTICK_PERIOD_MS);
294+
}
295+
}
296+
void RoboHeart::IMUcalculateDataTask(void *pvParameter){
297+
int64_t offsetRX = 0.0;
298+
int64_t offsetRY = 0.0;
299+
int64_t offsetRZ = 0.0;
300+
301+
int16_t previousAX = 0;
302+
int16_t previousAY = 0;
303+
int16_t previousAZ = 0;
304+
305+
int64_t sumRX = 0;
306+
int64_t sumRY = 0;
307+
int64_t sumRZ = 0;
308+
309+
float dphi, dtheta, dpsi;
310+
311+
bool calibration = false;
312+
int64_t calibrationSteps = 0;
313+
uint8_t calibrationCountdown = CALIBRATION_COUNTDOWN;
314+
315+
tIMUdata inputData = {0};
316+
float phi = 0.0, theta = 0.0, psi = 0.0;
317+
318+
while(true) {
319+
xEventGroupWaitBits(xIMUeventGroup, NEW_IMU_DATA_BIT, pdTRUE, pdFALSE, portMAX_DELAY);
320+
321+
if (xSemaphoreTake(xVarMutex, portMAX_DELAY)) {
322+
inputData = imuData;
323+
xSemaphoreGive(xVarMutex);
324+
}
325+
326+
if ((abs(previousAX - inputData.aX) < ACCELEROMETER_CUTOFF) && (abs(previousAY - inputData.aY) < ACCELEROMETER_CUTOFF) && (abs(previousAZ - inputData.aZ) < ACCELEROMETER_CUTOFF)) {
327+
previousAX = inputData.aX;
328+
previousAY = inputData.aY;
329+
previousAZ = inputData.aZ;
330+
331+
if (calibration) {
332+
sumRX += inputData.rX;
333+
sumRY += inputData.rY;
334+
sumRZ += inputData.rZ;
335+
calibrationSteps++;
336+
} else {
337+
if (calibrationCountdown) {
338+
calibrationCountdown--;
339+
continue;
340+
}
341+
342+
calibrationCountdown = CALIBRATION_COUNTDOWN;
343+
calibration = true;
344+
sumRX = inputData.rX;
345+
sumRY = inputData.rY;
346+
sumRZ = inputData.rZ;
347+
calibrationSteps = 1;
348+
}
349+
continue;
350+
} else {
351+
calibrationCountdown = CALIBRATION_COUNTDOWN;
352+
previousAX = inputData.aX;
353+
previousAY = inputData.aY;
354+
previousAZ = inputData.aZ;
355+
356+
if (calibration) {
357+
calibration = false;
358+
offsetRX = sumRX / calibrationSteps;
359+
offsetRY = sumRY / calibrationSteps;
360+
offsetRZ = sumRZ / calibrationSteps;
361+
}
362+
}
363+
364+
float omega_x = imu.calcGyro(inputData.rX - offsetRX) * DEG_TO_RAD;
365+
float omega_y = imu.calcGyro(inputData.rY - offsetRY) * DEG_TO_RAD;
366+
float omega_z = imu.calcGyro(inputData.rZ - offsetRZ) * DEG_TO_RAD;
367+
368+
//rungeKutta2(&phi, &theta, &psi, omega_x, omega_y, omega_z, 1.0 / imu.settings.gyroSampleRate);
369+
rungeKutta4(&phi, &theta, &psi, omega_x, omega_y, omega_z, 1.0 / imu.settings.gyroSampleRate);
370+
371+
if (phi >= TWO_PI) {
372+
phi -= TWO_PI;
373+
} else if (phi < 0.0) {
374+
phi += TWO_PI;
375+
}
376+
_rotationX = phi * RAD_TO_DEG;
377+
if (theta >= TWO_PI) {
378+
theta -= TWO_PI;
379+
} else if (theta < 0.0) {
380+
theta += TWO_PI;
381+
}
382+
_rotationY = theta * RAD_TO_DEG;
383+
if (psi >= TWO_PI) {
384+
psi -= TWO_PI;
385+
} else if (psi < 0.0) {
386+
psi += TWO_PI;
387+
}
388+
_rotationZ = psi * RAD_TO_DEG;
389+
}
253390
}

0 commit comments

Comments
 (0)