Components and supplies
6 DOF Sensor - MPU6050
Arduino Nano R3
Project description
Code
MotionSensor Tutorial
c_cpp
Illustration of how to use the tutorial
1/******************************************************************************* 2 Tutorial.ino - This is a MotionSensor example sketch which shows how to use it 3 and how to use Quaternions for robust and efficient motion 4 processing 5 6 Copyright(C) 2022 Howard James May 7 8 This file is part of the SweetMaker SDK 9 10 The SweetMaker SDK is free software: you can redistribute it and / or 11 modify it under the terms of the GNU General Public License as published by 12 the Free Software Foundation, either version 3 of the License, or 13 (at your option) any later version. 14 15 The SweetMaker SDK is distributed in the hope that it will be useful, 16 but WITHOUT ANY WARRANTY; without even the implied warranty of 17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the 18 GNU General Public License for more details. 19 20 You should have received a copy of the GNU General Public License 21 along with this program.If not, see <http://www.gnu.org/licenses/>. 22 23 Contact me at sweet.maker@outlook.com 24 25******************************************************************************** 26Release Date Change Description 27--------|-------------|--------------------------------------------------------| 28 1 07-Mar-2022 Initial release 29*******************************************************************************/ 30 31#include <Wire.h> 32#include <SweetMaker.h> 33#include <MotionSensor.h> 34 35using namespace SweetMaker; 36 37/* 38 * Motion Sensor instance for our use. 39 */ 40MotionSensor motionSensor; 41 42/* 43 * Function prototypes used in this sketch 44 */ 45void myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value); 46void handleSerialInput(); 47 48/* 49 * RunModes used in this tutorial. These correspond to different outputs 50 */ 51enum RunMode { 52 IDLE = 0, 53 QUATERNION = 1, 54 YAW_PITCH_ROLL = 2, 55 GRAVITY = 3, 56 TILT_X_Y = 4, 57 TILT_X_Y_NON_TRIG = 5, 58 ROTATIONAL_SPEED = 6, 59 VECTOR_ANGLE = 7, 60} runMode = IDLE; 61 62/* 63 * Arduino framework setup function - called once at system start 64 */ 65void setup() 66{ 67 Serial.begin(115200); // Serial connection 68 Serial.println("Motion Sensor & Quaternion Tutorial"); 69 70 /* 71 * Register eventHandler with SweetMaker Framework 72 */ 73 EventMngr::getMngr()->configCallBack(myEventHandlerCallback); 74 75 /* 76 * Initialise motionSensor - this establishes comms over I2C with 77 * the MPU6050 and downloads code to the MPU for motion processing 78 * allowing generation of rotation quaternion. 79 */ 80 if(motionSensor.init() != 0) { 81 // try again 82 } 83 84 /* 85 * While this sketch doesn't use TimerTick events we give it a kick 86 * to start the generation of events. 87 */ 88 TimerTickMngt::getTimerMngt()->update(0); 89 90 Serial.println("Setup complete"); 91} 92 93/* 94 * Arduino framework main loop 95 * 96 * This function does little more than: 97 * 1) Calculate loop elapsed time, 98 * 2) Watch for input on the serial port 99 * 3) Call SweetMaker update function 100 * 101 * The Motion Sensor is part of the SweetMaker framework and inherits from 102 * AutoUpdate - this integrates it with the Auto Update process which 103 * triggers the checking for new sensor readings. 104 * 105 */ 106void loop() 107{ 108 static unsigned long lastUpdateTime_ms = millis(); 109 unsigned long thisTime_ms = millis(); 110 unsigned long elapsedTime_ms = thisTime_ms - lastUpdateTime_ms; 111 112 handleSerialInput(); 113 114 AutoUpdateMngr::getUpdater()->update(elapsedTime_ms); 115 116 lastUpdateTime_ms = thisTime_ms; 117} 118 119/* 120 * myEventHandlerCallback: This captures events from the SweetMaker Framework 121 * we are only interested in: 122 * - MOTION_SENSOR_NEW_SMPL_RDY 123 * - MOTION_SENSOR_INIT_ERROR 124 * 125 * This function switches on runmode state and demonstrates different motion processing 126 * accordingly. The output is sent to the Serial port for the Arduino Serial Plotter. 127 */ 128void myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value) 129{ 130 switch (event) 131 { 132 case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY: 133 switch (runMode) { 134 135 /* 136 * The MPU6050 returns a Rotational Quaternion which represents how the sensor has been rotated. 137 * See the following link for a full explanation. 138 * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 139 * As the sensor moves the Rotational Quaternion updates accordingly. 140 * 141 * Note: the rotational quaternion doesn't immediately say how a particular vector 142 * has changed - this must be calculated using the Rotational Quaternion 143 * 144 * Also: the MPU6050 doesn't have a magnetometer and thus no concept of true North, 145 * consequently it's hortizontal orientation is arbitrary and will drift 146 * 147 * The Rotational Quaternion indicates the amount of rotation (r) about an axis (x,y,z) 148 * 149 * The MPU6050 provides this as 14 bit signed integer values +/-16384 150 * 151 * These values have little use on their own without further processing. 152 */ 153 case QUATERNION: 154 { 155 Serial.print("r:"); Serial.print(motionSensor.rotQuat.r); Serial.print("\ "); 156 Serial.print("x:"); Serial.print(motionSensor.rotQuat.x); Serial.print("\ "); 157 Serial.print("y:"); Serial.print(motionSensor.rotQuat.y); Serial.print("\ "); 158 Serial.print("z:"); Serial.print(motionSensor.rotQuat.z); Serial.println(); 159 } 160 break; 161 162 /* 163 * One common representation of 3D orientation is Yaw Pitch Roll. 164 * - Yaw is horizontal orientation - rotation about Z axis 165 * - Pitch is forward backward tilt - rotation about Y axis 166 * - Roll is side to side tilt - rotation about X axis 167 * 168 * These values can be calculated by MotionSensor from the Rotational Quaternion. 169 * Note that defining pitch as rotation about Y is arbitrary etc. 170 * Also note that this uses trigonometric functions to calculate and so takes a 171 * little time to compute. This may be fine for many applications. 172 * 173 * Reminder that Yaw will drift as the MPU6050 has no magnetometer 174 */ 175 case YAW_PITCH_ROLL: 176 { 177 unsigned long start_us = micros(); 178 int16_t yaw = motionSensor.rotQuat.getRotZ(); 179 int16_t pitch = motionSensor.rotQuat.getRotY(); 180 int16_t roll = motionSensor.rotQuat.getRotX(); 181 unsigned long stop_us = micros(); 182 183 Serial.print(stop_us - start_us); Serial.print("\ "); 184 Serial.print(yaw); Serial.print("\ "); 185 Serial.print(pitch); Serial.print("\ "); 186 Serial.print(roll); Serial.println(); 187 } 188 break; 189 190 /* 191 * The MPU6050 is great for calculating Gravity and the lack of 192 * magnetometer is no limitation for this. MotionSensor supports 193 * returning Gravity and this has small computational expense 194 * using no trigonometric functions. 195 * 196 */ 197 case GRAVITY: 198 { 199 Quaternion_16384 gravity; 200 unsigned long start_us = micros(); 201 motionSensor.rotQuat.getGravity(&gravity); 202 unsigned long stop_us = micros(); 203 204 Serial.print(stop_us - start_us); Serial.print("\ "); 205 Serial.print(gravity.x); Serial.print("\ "); 206 Serial.print(gravity.y); Serial.print("\ "); 207 Serial.print(gravity.z); Serial.println(); 208 } 209 break; 210 211 /* 212 * As previously shown Tilt X and Tilt Y (Pitch and Roll) 213 * can be produced. 214 */ 215 case TILT_X_Y: 216 { 217 unsigned long start_us = micros(); 218 int16_t tilt_forward = motionSensor.rotQuat.getRotX(); 219 int16_t tilt_side = motionSensor.rotQuat.getRotY(); 220 unsigned long stop_us = micros(); 221 222 Serial.print(stop_us - start_us); Serial.print("\ "); 223 Serial.print(tilt_forward); Serial.print("\ "); 224 Serial.print(tilt_side); Serial.println("\ 0"); 225 } 226 break; 227 228 /* 229 * In order to avoid the use of trigonometric functions it may be 230 * adequate to use the Sin of RotX and Sin RotY. This can be calculated 231 * using only multiplcations and right shifts. 232 */ 233 case TILT_X_Y_NON_TRIG: 234 { 235 unsigned long start_us = micros(); 236 int16_t tilt_forward = motionSensor.rotQuat.getSinRotX(); 237 int16_t tilt_side = motionSensor.rotQuat.getSinRotY(); 238 unsigned long stop_us = micros(); 239 240 Serial.print(stop_us - start_us); Serial.print("\ "); 241 Serial.print(tilt_forward); Serial.print("\ "); 242 Serial.print(tilt_side); Serial.println("\ 0"); 243 } 244 break; 245 246 /* 247 * The rotational quaternion produced by the MPU6050 can also 248 * provide an indication of rotational speed by looking at the 249 * rotational quaternion delta and the angular part of that. 250 * 251 * The angular part is equal to cos(theta/2), theta being the 252 * angular rotation between the two last readings. As the sample period 253 * is a fraction of a second, theta is likely to be small and cos(theta/2) 254 * smaller still. The value should be handled accordingly. 255 * 256 * Note: the rotation speed here is not about any one particular axis rather 257 * it is about any arbitraty axis. 258 */ 259 case ROTATIONAL_SPEED: 260 { 261 int16_t rot_speed = 16384 - motionSensor.rotQuatDelta.r; 262 Serial.print(rot_speed); Serial.println("\ 0\ 0\ 0"); 263 } 264 break; 265 266 /* 267 * One of the most powerful uses of the quaternion is finding 268 * how close the sensor is to pointing in a particular direction. 269 * The dot product between two positional quaternions tells us the 270 * cosine of the angle between the two vectors in 3D space. This 271 * is calculated without the use of trigonometric functions. 272 * 273 * As we have seen the MPU6050 provides a robust calculation of where the 274 * Z axis (gravity) is now pointing. Thus we choose to compare gravity 275 * against vectors we are interested in. 276 * 277 */ 278 case VECTOR_ANGLE: 279 { 280 Quaternion_16384 gq; 281 motionSensor.rotQuat.getGravity(&gq); 282 283 Quaternion_16384 z_axis = { 0, 0, 0, 0x4000 }; 284 Quaternion_16384 forward_right = { 0,9459,9459,9459 }; 285 Quaternion_16384 forward_left = { 0,9459,-9459,9459 }; 286 287 int16_t up = z_axis.dotProduct(&gq); 288 int16_t fr = forward_right.dotProduct(&gq); 289 int16_t fl = forward_left.dotProduct(&gq); 290 291 Serial.print(up); Serial.print("\ "); 292 Serial.print(fr); Serial.print("\ "); 293 Serial.print(fl); Serial.println("\ 0"); 294 } 295 break; 296 297 default: 298 break; 299 } 300 break; 301 302 case MotionSensor::MOTION_SENSOR_INIT_ERROR: 303 Serial.print("Motion Sensor Init Error: "); Serial.println(value); 304 break; 305 306 default: 307 break; 308 } 309 310} 311 312 313void handleSerialInput() 314{ 315 if (!Serial.available()) 316 return; 317 318 char inChar = Serial.read(); 319 320 Serial.println("_:0\ _:0\ _:0\ _:0"); 321 322 switch (inChar) 323 { 324 325 case '0': 326 runMode = IDLE; 327 break; 328 329 case '1': 330 Serial.println("r x y z"); 331 runMode = QUATERNION; 332 break; 333 334 case '2': 335 Serial.println("time_us yaw pitch roll"); 336 runMode = YAW_PITCH_ROLL; 337 break; 338 339 case '3': 340 Serial.println("time_us x y z"); 341 runMode = GRAVITY; 342 break; 343 344 case '4': 345 Serial.println("time_us tilt_forward tilt_side"); 346 runMode = TILT_X_Y; 347 break; 348 349 case '5': 350 Serial.println("time_us tilt_forward tilt_side"); 351 runMode = TILT_X_Y_NON_TRIG; 352 break; 353 354 case '6': 355 Serial.println("rotational_speed"); 356 runMode = ROTATIONAL_SPEED; 357 break; 358 359 case '7': 360 Serial.println("z_axis forward_right forward_left"); 361 runMode = VECTOR_ANGLE; 362 break; 363 364 case 'c': 365 { 366 /* 367 * Each MPU6050 requires it's own set of calibration values 368 * These can be calculated by MotionSensor and these values 369 * submited in the motionSensor.init() function. 370 * 371 * You may choose to store calibration values in EEPROM so 372 * so that they can be retrieved at system start without 373 * hardcoding them into the sketch (which only works if you 374 * are using a single MPU6050). See the SweetMaker StrawberryString 375 * which does just this. 376 */ 377 Serial.println("Calibrating Motion Sensor"); 378 MotionSensor::CALIBRATION calibration; 379 motionSensor.runSelfCalibrate(&calibration); 380 } 381 break; 382 383 384 /* 385 * The MPU6050 may not be mounted 100% flat or may be mounted in 386 * some arbitrary orientation and an offset applied to make it 387 * work as if it were horizontal. This can be done using the 388 * MotionSensor auto level feature. This takes advantage of the 389 * RotationalQuaternions ability to easily combine rotations. 390 * 391 * This works by finding the rotation beteen gravity and the Z axis 392 * and using this as an offset. 393 * 394 * https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another 395 */ 396 case 'l': 397 Serial.println("Self Leveling"); 398 motionSensor.autoLevel(); 399 break; 400 401 /* 402 * AutoLeveling can compensate for non-horizontal mounting 403 * but a rotation about Z requires specific configuration. 404 * Here we autolevel and also apply a 45 degree rotation offset. 405 */ 406 case 'z': 407 { 408 Serial.println("Self Leveling + Z Rotation of 45 degrees"); 409 410 float rotation_z = 45; 411 412 Quaternion_16384 gravity; 413 RotationQuaternion_16384 offsetQ; 414 415 RotationQuaternion_16384 rotZ(rotation_z, 0, 0, 16384); 416 Quaternion_16384 zAxis(0, 0, 0, 16384); 417 418 motionSensor.clearOffsetRotation(); 419 motionSensor.rotQuat.getGravity(&gravity); 420 421 offsetQ.findOffsetRotation(&zAxis, &gravity); 422 offsetQ.crossProduct(&rotZ); 423 424 motionSensor.setOffsetRotation(&offsetQ); 425 } 426 break; 427 428 default: 429 break; 430 } 431} 432 433 434
SweetMaker MotionSensor
MotionSensor library and Example Tutorial
SweetMaker MotionSensor
MotionSensor library and Example Tutorial
MotionSensor Tutorial
c_cpp
Illustration of how to use the tutorial
1/******************************************************************************* 2 Tutorial.ino - This is a MotionSensor example sketch which shows how to use it 3 and how to use Quaternions for robust and efficient motion 4 processing 5 6 Copyright(C) 2022 Howard James May 7 8 This file is part of the SweetMaker SDK 9 10 The SweetMaker SDK is free software: you can redistribute it and / or 11 modify it under the terms of the GNU General Public License as published by 12 the Free Software Foundation, either version 3 of the License, or 13 (at your option) any later version. 14 15 The SweetMaker SDK is distributed in the hope that it will be useful, 16 but WITHOUT ANY WARRANTY; without even the implied warranty of 17 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.See the 18 GNU General Public License for more details. 19 20 You should have received a copy of the GNU General Public License 21 along with this program.If not, see <http://www.gnu.org/licenses/>. 22 23 Contact me at sweet.maker@outlook.com 24 25******************************************************************************** 26Release Date Change Description 27--------|-------------|--------------------------------------------------------| 28 1 07-Mar-2022 Initial release 29*******************************************************************************/ 30 31#include <Wire.h> 32#include <SweetMaker.h> 33#include <MotionSensor.h> 34 35using namespace SweetMaker; 36 37/* 38 * Motion Sensor instance for our use. 39 */ 40MotionSensor motionSensor; 41 42/* 43 * Function prototypes used in this sketch 44 */ 45void myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value); 46void handleSerialInput(); 47 48/* 49 * RunModes used in this tutorial. These correspond to different outputs 50 */ 51enum RunMode { 52 IDLE = 0, 53 QUATERNION = 1, 54 YAW_PITCH_ROLL = 2, 55 GRAVITY = 3, 56 TILT_X_Y = 4, 57 TILT_X_Y_NON_TRIG = 5, 58 ROTATIONAL_SPEED = 6, 59 VECTOR_ANGLE = 7, 60} runMode = IDLE; 61 62/* 63 * Arduino framework setup function - called once at system start 64 */ 65void setup() 66{ 67 Serial.begin(115200); // Serial connection 68 Serial.println("Motion Sensor & Quaternion Tutorial"); 69 70 /* 71 * Register eventHandler with SweetMaker Framework 72 */ 73 EventMngr::getMngr()->configCallBack(myEventHandlerCallback); 74 75 /* 76 * Initialise motionSensor - this establishes comms over I2C with 77 * the MPU6050 and downloads code to the MPU for motion processing 78 * allowing generation of rotation quaternion. 79 */ 80 if(motionSensor.init() != 0) { 81 // try again 82 } 83 84 /* 85 * While this sketch doesn't use TimerTick events we give it a kick 86 * to start the generation of events. 87 */ 88 TimerTickMngt::getTimerMngt()->update(0); 89 90 Serial.println("Setup complete"); 91} 92 93/* 94 * Arduino framework main loop 95 * 96 * This function does little more than: 97 * 1) Calculate loop elapsed time, 98 * 2) Watch for input on the serial port 99 * 3) Call SweetMaker update function 100 * 101 * The Motion Sensor is part of the SweetMaker framework and inherits from 102 * AutoUpdate - this integrates it with the Auto Update process which 103 * triggers the checking for new sensor readings. 104 * 105 */ 106void loop() 107{ 108 static unsigned long lastUpdateTime_ms = millis(); 109 unsigned long thisTime_ms = millis(); 110 unsigned long elapsedTime_ms = thisTime_ms - lastUpdateTime_ms; 111 112 handleSerialInput(); 113 114 AutoUpdateMngr::getUpdater()->update(elapsedTime_ms); 115 116 lastUpdateTime_ms = thisTime_ms; 117} 118 119/* 120 * myEventHandlerCallback: This captures events from the SweetMaker Framework 121 * we are only interested in: 122 * - MOTION_SENSOR_NEW_SMPL_RDY 123 * - MOTION_SENSOR_INIT_ERROR 124 * 125 * This function switches on runmode state and demonstrates different motion processing 126 * accordingly. The output is sent to the Serial port for the Arduino Serial Plotter. 127 */ 128void myEventHandlerCallback(uint16_t event, uint8_t source, uint16_t value) 129{ 130 switch (event) 131 { 132 case MotionSensor::MOTION_SENSOR_NEW_SMPL_RDY: 133 switch (runMode) { 134 135 /* 136 * The MPU6050 returns a Rotational Quaternion which represents how the sensor has been rotated. 137 * See the following link for a full explanation. 138 * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation 139 * As the sensor moves the Rotational Quaternion updates accordingly. 140 * 141 * Note: the rotational quaternion doesn't immediately say how a particular vector 142 * has changed - this must be calculated using the Rotational Quaternion 143 * 144 * Also: the MPU6050 doesn't have a magnetometer and thus no concept of true North, 145 * consequently it's hortizontal orientation is arbitrary and will drift 146 * 147 * The Rotational Quaternion indicates the amount of rotation (r) about an axis (x,y,z) 148 * 149 * The MPU6050 provides this as 14 bit signed integer values +/-16384 150 * 151 * These values have little use on their own without further processing. 152 */ 153 case QUATERNION: 154 { 155 Serial.print("r:"); Serial.print(motionSensor.rotQuat.r); Serial.print("\ "); 156 Serial.print("x:"); Serial.print(motionSensor.rotQuat.x); Serial.print("\ "); 157 Serial.print("y:"); Serial.print(motionSensor.rotQuat.y); Serial.print("\ "); 158 Serial.print("z:"); Serial.print(motionSensor.rotQuat.z); Serial.println(); 159 } 160 break; 161 162 /* 163 * One common representation of 3D orientation is Yaw Pitch Roll. 164 * - Yaw is horizontal orientation - rotation about Z axis 165 * - Pitch is forward backward tilt - rotation about Y axis 166 * - Roll is side to side tilt - rotation about X axis 167 * 168 * These values can be calculated by MotionSensor from the Rotational Quaternion. 169 * Note that defining pitch as rotation about Y is arbitrary etc. 170 * Also note that this uses trigonometric functions to calculate and so takes a 171 * little time to compute. This may be fine for many applications. 172 * 173 * Reminder that Yaw will drift as the MPU6050 has no magnetometer 174 */ 175 case YAW_PITCH_ROLL: 176 { 177 unsigned long start_us = micros(); 178 int16_t yaw = motionSensor.rotQuat.getRotZ(); 179 int16_t pitch = motionSensor.rotQuat.getRotY(); 180 int16_t roll = motionSensor.rotQuat.getRotX(); 181 unsigned long stop_us = micros(); 182 183 Serial.print(stop_us - start_us); Serial.print("\ "); 184 Serial.print(yaw); Serial.print("\ "); 185 Serial.print(pitch); Serial.print("\ "); 186 Serial.print(roll); Serial.println(); 187 } 188 break; 189 190 /* 191 * The MPU6050 is great for calculating Gravity and the lack of 192 * magnetometer is no limitation for this. MotionSensor supports 193 * returning Gravity and this has small computational expense 194 * using no trigonometric functions. 195 * 196 */ 197 case GRAVITY: 198 { 199 Quaternion_16384 gravity; 200 unsigned long start_us = micros(); 201 motionSensor.rotQuat.getGravity(&gravity); 202 unsigned long stop_us = micros(); 203 204 Serial.print(stop_us - start_us); Serial.print("\ "); 205 Serial.print(gravity.x); Serial.print("\ "); 206 Serial.print(gravity.y); Serial.print("\ "); 207 Serial.print(gravity.z); Serial.println(); 208 } 209 break; 210 211 /* 212 * As previously shown Tilt X and Tilt Y (Pitch and Roll) 213 * can be produced. 214 */ 215 case TILT_X_Y: 216 { 217 unsigned long start_us = micros(); 218 int16_t tilt_forward = motionSensor.rotQuat.getRotX(); 219 int16_t tilt_side = motionSensor.rotQuat.getRotY(); 220 unsigned long stop_us = micros(); 221 222 Serial.print(stop_us - start_us); Serial.print("\ "); 223 Serial.print(tilt_forward); Serial.print("\ "); 224 Serial.print(tilt_side); Serial.println("\ 0"); 225 } 226 break; 227 228 /* 229 * In order to avoid the use of trigonometric functions it may be 230 * adequate to use the Sin of RotX and Sin RotY. This can be calculated 231 * using only multiplcations and right shifts. 232 */ 233 case TILT_X_Y_NON_TRIG: 234 { 235 unsigned long start_us = micros(); 236 int16_t tilt_forward = motionSensor.rotQuat.getSinRotX(); 237 int16_t tilt_side = motionSensor.rotQuat.getSinRotY(); 238 unsigned long stop_us = micros(); 239 240 Serial.print(stop_us - start_us); Serial.print("\ "); 241 Serial.print(tilt_forward); Serial.print("\ "); 242 Serial.print(tilt_side); Serial.println("\ 0"); 243 } 244 break; 245 246 /* 247 * The rotational quaternion produced by the MPU6050 can also 248 * provide an indication of rotational speed by looking at the 249 * rotational quaternion delta and the angular part of that. 250 * 251 * The angular part is equal to cos(theta/2), theta being the 252 * angular rotation between the two last readings. As the sample period 253 * is a fraction of a second, theta is likely to be small and cos(theta/2) 254 * smaller still. The value should be handled accordingly. 255 * 256 * Note: the rotation speed here is not about any one particular axis rather 257 * it is about any arbitraty axis. 258 */ 259 case ROTATIONAL_SPEED: 260 { 261 int16_t rot_speed = 16384 - motionSensor.rotQuatDelta.r; 262 Serial.print(rot_speed); Serial.println("\ 0\ 0\ 0"); 263 } 264 break; 265 266 /* 267 * One of the most powerful uses of the quaternion is finding 268 * how close the sensor is to pointing in a particular direction. 269 * The dot product between two positional quaternions tells us the 270 * cosine of the angle between the two vectors in 3D space. This 271 * is calculated without the use of trigonometric functions. 272 * 273 * As we have seen the MPU6050 provides a robust calculation of where the 274 * Z axis (gravity) is now pointing. Thus we choose to compare gravity 275 * against vectors we are interested in. 276 * 277 */ 278 case VECTOR_ANGLE: 279 { 280 Quaternion_16384 gq; 281 motionSensor.rotQuat.getGravity(&gq); 282 283 Quaternion_16384 z_axis = { 0, 0, 0, 0x4000 }; 284 Quaternion_16384 forward_right = { 0,9459,9459,9459 }; 285 Quaternion_16384 forward_left = { 0,9459,-9459,9459 }; 286 287 int16_t up = z_axis.dotProduct(&gq); 288 int16_t fr = forward_right.dotProduct(&gq); 289 int16_t fl = forward_left.dotProduct(&gq); 290 291 Serial.print(up); Serial.print("\ "); 292 Serial.print(fr); Serial.print("\ "); 293 Serial.print(fl); Serial.println("\ 0"); 294 } 295 break; 296 297 default: 298 break; 299 } 300 break; 301 302 case MotionSensor::MOTION_SENSOR_INIT_ERROR: 303 Serial.print("Motion Sensor Init Error: "); Serial.println(value); 304 break; 305 306 default: 307 break; 308 } 309 310} 311 312 313void handleSerialInput() 314{ 315 if (!Serial.available()) 316 return; 317 318 char inChar = Serial.read(); 319 320 Serial.println("_:0\ _:0\ _:0\ _:0"); 321 322 switch (inChar) 323 { 324 325 case '0': 326 runMode = IDLE; 327 break; 328 329 case '1': 330 Serial.println("r x y z"); 331 runMode = QUATERNION; 332 break; 333 334 case '2': 335 Serial.println("time_us yaw pitch roll"); 336 runMode = YAW_PITCH_ROLL; 337 break; 338 339 case '3': 340 Serial.println("time_us x y z"); 341 runMode = GRAVITY; 342 break; 343 344 case '4': 345 Serial.println("time_us tilt_forward tilt_side"); 346 runMode = TILT_X_Y; 347 break; 348 349 case '5': 350 Serial.println("time_us tilt_forward tilt_side"); 351 runMode = TILT_X_Y_NON_TRIG; 352 break; 353 354 case '6': 355 Serial.println("rotational_speed"); 356 runMode = ROTATIONAL_SPEED; 357 break; 358 359 case '7': 360 Serial.println("z_axis forward_right forward_left"); 361 runMode = VECTOR_ANGLE; 362 break; 363 364 case 'c': 365 { 366 /* 367 * Each MPU6050 requires it's own set of calibration values 368 * These can be calculated by MotionSensor and these values 369 * submited in the motionSensor.init() function. 370 * 371 * You may choose to store calibration values in EEPROM so 372 * so that they can be retrieved at system start without 373 * hardcoding them into the sketch (which only works if you 374 * are using a single MPU6050). See the SweetMaker StrawberryString 375 * which does just this. 376 */ 377 Serial.println("Calibrating Motion Sensor"); 378 MotionSensor::CALIBRATION calibration; 379 motionSensor.runSelfCalibrate(&calibration); 380 } 381 break; 382 383 384 /* 385 * The MPU6050 may not be mounted 100% flat or may be mounted in 386 * some arbitrary orientation and an offset applied to make it 387 * work as if it were horizontal. This can be done using the 388 * MotionSensor auto level feature. This takes advantage of the 389 * RotationalQuaternions ability to easily combine rotations. 390 * 391 * This works by finding the rotation beteen gravity and the Z axis 392 * and using this as an offset. 393 * 394 * https://stackoverflow.com/questions/1171849/finding-quaternion-representing-the-rotation-from-one-vector-to-another 395 */ 396 case 'l': 397 Serial.println("Self Leveling"); 398 motionSensor.autoLevel(); 399 break; 400 401 /* 402 * AutoLeveling can compensate for non-horizontal mounting 403 * but a rotation about Z requires specific configuration. 404 * Here we autolevel and also apply a 45 degree rotation offset. 405 */ 406 case 'z': 407 { 408 Serial.println("Self Leveling + Z Rotation of 45 degrees"); 409 410 float rotation_z = 45; 411 412 Quaternion_16384 gravity; 413 RotationQuaternion_16384 offsetQ; 414 415 RotationQuaternion_16384 rotZ(rotation_z, 0, 0, 16384); 416 Quaternion_16384 zAxis(0, 0, 0, 16384); 417 418 motionSensor.clearOffsetRotation(); 419 motionSensor.rotQuat.getGravity(&gravity); 420 421 offsetQ.findOffsetRotation(&zAxis, &gravity); 422 offsetQ.crossProduct(&rotZ); 423 424 motionSensor.setOffsetRotation(&offsetQ); 425 } 426 break; 427 428 default: 429 break; 430 } 431} 432 433 434
SweetMaker core
Framework used by MotionSensor
Downloadable files
img_2854_YTldCI25bZ.jpg
The WS2812B LEDs are optional
img_2854_YTldCI25bZ.jpg
img_2854_YTldCI25bZ.jpg
The WS2812B LEDs are optional
img_2854_YTldCI25bZ.jpg
Comments
Only logged in users can leave comments
SweetMaker
0 Followers
•0 Projects
Table of contents
Intro
1
0