├── .gitattributes ├── .gitignore ├── 3D parts ├── EVO_version │ ├── Bumper.stl │ ├── Electronics SHELF.stl │ ├── Lateral.stl │ ├── Regular Wheel.stl │ ├── motor SHELF.stl │ ├── regular ARM.stl │ ├── servo holder.stl │ └── top SHELF.stl └── OriginalBROBOT_old │ └── B-robot STL 3D parts.stl ├── BROBOT └── BROBOT.ino ├── LICENSE ├── README.md ├── TouchOSC ├── BROBOT.touchosc └── install.txt ├── Utils ├── B_ROBOT_WIFI_CONFIG │ └── B_ROBOT_WIFI_CONFIG.ino └── libraries │ └── JJROBOTS_WIFI │ ├── JJROBOTS_WIFI.cpp │ ├── JJROBOTS_WIFI.h │ └── keywords.txt ├── hardware └── electronics │ ├── brobot.brd │ ├── brobot.pdf │ └── brobot.sch ├── install.txt └── libraries ├── I2Cdev ├── I2Cdev.cpp ├── I2Cdev.h └── keywords.txt ├── JJROBOTS_BROBOT ├── JJROBOTS_BROBOT.cpp ├── JJROBOTS_BROBOT.h └── keywords.txt ├── JJROBOTS_OSC ├── JJROBOTS_OSC.cpp ├── JJROBOTS_OSC.h ├── examples │ └── JJROBOTS_OSC_sample.ino └── keywords.txt └── MPU6050 ├── Examples ├── MPU6050_DMP6 │ ├── MPU6050_DMP6.ino │ └── Processing │ │ └── MPUTeapot.pde └── MPU6050_raw │ └── MPU6050_raw.ino ├── JJ_MPU6050_DMP_6Axis.h ├── MPU6050.cpp ├── MPU6050.h ├── MPU6050_6Axis_MotionApps20.h ├── MPU6050_9Axis_MotionApps41.h └── helper_3dmath.h /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear on external disk 35 | .Spotlight-V100 36 | .Trashes 37 | 38 | # Directories potentially created on remote AFP share 39 | .AppleDB 40 | .AppleDesktop 41 | Network Trash Folder 42 | Temporary Items 43 | .apdisk 44 | -------------------------------------------------------------------------------- /BROBOT/BROBOT.ino: -------------------------------------------------------------------------------- 1 | // B-ROBOT SELF BALANCE ARDUINO ROBOT WITH STEPPER MOTORS 2 | // JJROBOTS BROBOT KIT: (Arduino Leonardo + BROBOT ELECTRONIC BRAIN v2 + STEPPER MOTOR drivers) 3 | // This code is prepared for new BROBOT shield v2.0 with ESP8266 Wifi module 4 | // You need to install libraries JJROBOTS_OSC, JJROBOTS_BROBOT, I2Cdev and MPU6050 (from Github repository) 5 | // Author: JJROBOTS.COM (Jose Julio & Juan Pedro) 6 | // Date: 02/09/2014 7 | // Updated: 26/10/2015 8 | // Version: 2.21 9 | // License: GPL v2 10 | // Project URL: http://jjrobots.com/b-robot (Features,documentation,build instructions,how it works, SHOP,...) 11 | // New updates: 12 | // - New default parameters specially tuned for BROBOT EVO version 13 | // - Support for TouchMessages(/z). Main controls resets when user lift the fingers 14 | // - BROBOT sends the battery status to the interface 15 | // Remember to update the libraries and install the new TouchOSC layout 16 | // Thanks to our users on the forum for the new ideas. Specially sasa999, KomX, ... 17 | 18 | // The board needs at least 10-15 seconds with no motion (robot steady) at beginning to give good values... 19 | // MPU6050 IMU using internal DMP processor. Connected via I2C bus 20 | // Angle calculations and control part is running at 200Hz from DMP solution 21 | // DMP is using the gyro_bias_no_motion correction method. 22 | 23 | // The robot is OFF when the angle is high (robot is horizontal). When you start raising the robot it 24 | // automatically switch ON and start a RAISE UP procedure. 25 | // You could RAISE UP the robot also with the robot arm servo [OPTIONAL] (Push1 on the interface) 26 | // To switch OFF the robot you could manually put the robot down on the floor (horizontal) 27 | 28 | // We use a standard PID controllers (Proportional, Integral derivative controller) for robot stability 29 | // More info on the project page 30 | // We have a PI controller for speed control and a PD controller for stability (robot angle) 31 | // The output of the control (motors speed) is integrated so it´s really an acceleration not an speed. 32 | 33 | // We control the robot from a WIFI module using OSC standard UDP messages (JJROBOTS_OSC library) 34 | // You need an OSC app to control de robot (example: TouchOSC for IOS and Android) 35 | // Join the module Wifi Access Point (by default: JJROBOTS) with your Smartphone/Tablet... 36 | // Install the BROBOT layout into the OSC app (Touch OSC) and start play! (read the project page) 37 | // OSC controls: 38 | // fader1: Throttle (0.0-1.0) OSC message: /1/fader1 39 | // fader2: Steering (0.0-1.0) OSC message: /1/fader2 40 | // push1: Move servo arm (and robot raiseup) OSC message /1/push1 [OPTIONAL] 41 | // if you enable the touchMessage on TouchOSC options, controls return to center automatically when you lift your fingers 42 | // toggle1: Enable PRO mode. On PRO mode steering and throttle are more aggressive 43 | // PAGE2: PID adjustements [optional][don´t touch if you don´t know what you are doing...;-) ] 44 | 45 | #define WIFI_ESP // New WIFI ESP8266 Module. Comment this line if you are using the old Wifi module (RN-171) 46 | 47 | #include 48 | #include 49 | #include 50 | #include // I2Cdev lib from www.i2cdevlib.com 51 | #include // Modified version of the MPU6050 library to work with DMP (see comments inside) 52 | // This version optimize the FIFO (only contains quaternion) and minimize code size 53 | 54 | // NORMAL MODE PARAMETERS (MAXIMUN SETTINGS) 55 | #define MAX_THROTTLE 580 56 | #define MAX_STEERING 150 57 | #define MAX_TARGET_ANGLE 12 58 | 59 | // PRO MODE = MORE AGGRESSIVE (MAXIMUN SETTINGS) 60 | #define MAX_THROTTLE_PRO 980 //680 61 | #define MAX_STEERING_PRO 250 62 | #define MAX_TARGET_ANGLE_PRO 40 //20 63 | 64 | // Default control terms 65 | #define KP 0.19 66 | #define KD 28 67 | #define KP_THROTTLE 0.07 68 | #define KI_THROTTLE 0.04 69 | 70 | // Control gains for raiseup (the raiseup movement requiere special control parameters) 71 | #define KP_RAISEUP 0.16 72 | #define KD_RAISEUP 36 73 | #define KP_THROTTLE_RAISEUP 0 // No speed control on raiseup 74 | #define KI_THROTTLE_RAISEUP 0.0 75 | 76 | #define MAX_CONTROL_OUTPUT 500 77 | 78 | // Servo definitions 79 | #define SERVO_AUX_NEUTRO 1550 // Servo neutral position 80 | #define SERVO_MIN_PULSEWIDTH 650 81 | #define SERVO_MAX_PULSEWIDTH 2600 82 | 83 | // Battery management [optional]. This is not needed for alkaline or Ni-Mh batteries but usefull for if you use lipo batteries 84 | #define BATTERY_CHECK 1 // 0: No check, 1: check (send message to interface) 85 | #define LIPOBATT 0 // Default 0: No Lipo batt 1: Lipo batt (to adjust battery monitor range) 86 | #define BATTERY_WARNING 105 // (10.5 volts) aprox [for lipo batts, disable by default] 87 | #define BATTERY_SHUTDOWN 95 // (9.5 volts) [for lipo batts] 88 | #define SHUTDOWN_WHEN_BATTERY_OFF 0 // 0: Not used, 1: Robot will shutdown when battery is off (BATTERY_CHECK SHOULD BE 1) 89 | 90 | #define DEBUG 0 // 0 = No debug info (default) 91 | 92 | #define CLR(x,y) (x&=(~(1< Out of 109 | 110 | String MAC; // MAC address of Wifi module 111 | 112 | // MPU control/status vars 113 | bool dmpReady = false; // set true if DMP init was successful 114 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 115 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 116 | uint16_t packetSize; // expected DMP packet size (for us 18 bytes) 117 | uint16_t fifoCount; // count of all bytes currently in FIFO 118 | uint8_t fifoBuffer[18]; // FIFO storage buffer 119 | Quaternion q; 120 | 121 | uint8_t loop_counter; // To generate a medium loop 40Hz 122 | uint8_t slow_loop_counter; // slow loop 2Hz 123 | uint8_t sendBattery_counter; // To send battery status 124 | 125 | long timer_old; 126 | long timer_value; 127 | int debug_counter; 128 | float debugVariable; 129 | float dt; 130 | 131 | // class default I2C address is 0x68 for MPU6050 132 | MPU6050 mpu; 133 | 134 | // Angle of the robot (used for stability control) 135 | float angle_adjusted; 136 | float angle_adjusted_Old; 137 | 138 | // Default control values from constant definitions 139 | float Kp = KP; 140 | float Kd = KD; 141 | float Kp_thr = KP_THROTTLE; 142 | float Ki_thr = KI_THROTTLE; 143 | float Kp_user = KP; 144 | float Kd_user = KD; 145 | float Kp_thr_user = KP_THROTTLE; 146 | float Ki_thr_user = KI_THROTTLE; 147 | bool newControlParameters = false; 148 | bool modifing_control_parameters = false; 149 | float PID_errorSum; 150 | float PID_errorOld = 0; 151 | float PID_errorOld2 = 0; 152 | float setPointOld = 0; 153 | float target_angle; 154 | float throttle; 155 | float steering; 156 | float max_throttle = MAX_THROTTLE; 157 | float max_steering = MAX_STEERING; 158 | float max_target_angle = MAX_TARGET_ANGLE; 159 | float control_output; 160 | 161 | uint8_t mode; // mode = 0 Normal mode, mode = 1 Pro mode (More agressive) 162 | 163 | int16_t motor1; 164 | int16_t motor2; 165 | 166 | int16_t speed_M1, speed_M2; // Actual speed of motors 167 | int8_t dir_M1, dir_M2; // Actual direction of steppers motors 168 | int16_t actual_robot_speed; // overall robot speed (measured from steppers speed) 169 | int16_t actual_robot_speed_Old; 170 | float estimated_speed_filtered; // Estimated robot speed 171 | 172 | // DMP FUNCTIONS 173 | // This function defines the weight of the accel on the sensor fusion 174 | // default value is 0x80 175 | // The official invensense name is inv_key_0_96 (??) 176 | void dmpSetSensorFusionAccelGain(uint8_t gain) 177 | { 178 | // INV_KEY_0_96 179 | mpu.setMemoryBank(0); 180 | mpu.setMemoryStartAddress(0x60); 181 | mpu.writeMemoryByte(0); 182 | mpu.writeMemoryByte(gain); 183 | mpu.writeMemoryByte(0); 184 | mpu.writeMemoryByte(0); 185 | } 186 | 187 | // Quick calculation to obtein Phi angle from quaternion solution (from DMP internal quaternion solution) 188 | float dmpGetPhi() { 189 | mpu.getFIFOBytes(fifoBuffer, 16); // We only read the quaternion 190 | mpu.dmpGetQuaternion(&q, fifoBuffer); 191 | mpu.resetFIFO(); // We always reset FIFO 192 | 193 | //return( asin(-2*(q.x * q.z - q.w * q.y)) * 180/M_PI); //roll 194 | //return Phi angle (robot orientation) from quaternion DMP output 195 | return (atan2(2 * (q.y * q.z + q.w * q.x), q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z) * RAD2GRAD); 196 | } 197 | 198 | // PD controller implementation(Proportional, derivative). DT is in miliseconds 199 | float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd) 200 | { 201 | float error; 202 | float output; 203 | 204 | error = setPoint - input; 205 | 206 | // Kd is implemented in two parts 207 | // The biggest one using only the input (sensor) part not the SetPoint input-input(t-2) 208 | // And the second using the setpoint to make it a bit more agressive setPoint-setPoint(t-1) 209 | output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; 210 | //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t"); 211 | PID_errorOld2 = PID_errorOld; 212 | PID_errorOld = input; // error for Kd is only the input component 213 | setPointOld = setPoint; 214 | return (output); 215 | } 216 | 217 | 218 | // PI controller implementation (Proportional, integral). DT is in miliseconds 219 | float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki) 220 | { 221 | float error; 222 | float output; 223 | 224 | error = setPoint - input; 225 | PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR); 226 | PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX); 227 | 228 | //Serial.println(PID_errorSum); 229 | 230 | output = Kp * error + Ki * PID_errorSum * DT * 0.001; // DT is in miliseconds... 231 | return (output); 232 | } 233 | 234 | // 16 single cycle instructions = 1us at 16Mhz 235 | void delay_1us() 236 | { 237 | __asm__ __volatile__ ( 238 | "nop" "\n\t" 239 | "nop" "\n\t" 240 | "nop" "\n\t" 241 | "nop" "\n\t" 242 | "nop" "\n\t" 243 | "nop" "\n\t" 244 | "nop" "\n\t" 245 | "nop" "\n\t" 246 | "nop" "\n\t" 247 | "nop" "\n\t" 248 | "nop" "\n\t" 249 | "nop" "\n\t" 250 | "nop" "\n\t" 251 | "nop" "\n\t" 252 | "nop" "\n\t" 253 | "nop"); 254 | } 255 | 256 | // TIMER 1 : STEPPER MOTOR1 SPEED CONTROL 257 | ISR(TIMER1_COMPA_vect) 258 | { 259 | if (dir_M1 == 0) // If we are not moving we dont generate a pulse 260 | return; 261 | // We generate 1us STEP pulse 262 | SET(PORTE, 6); // STEP MOTOR 1 263 | delay_1us(); 264 | CLR(PORTE, 6); 265 | } 266 | // TIMER 3 : STEPPER MOTOR2 SPEED CONTROL 267 | ISR(TIMER3_COMPA_vect) 268 | { 269 | if (dir_M2 == 0) // If we are not moving we dont generate a pulse 270 | return; 271 | // We generate 1us STEP pulse 272 | SET(PORTD, 6); // STEP MOTOR 2 273 | delay_1us(); 274 | CLR(PORTD, 6); 275 | } 276 | 277 | 278 | // Set speed of Stepper Motor1 279 | // tspeed could be positive or negative (reverse) 280 | void setMotorSpeedM1(int16_t tspeed) 281 | { 282 | long timer_period; 283 | int16_t speed; 284 | 285 | // Limit max speed? 286 | 287 | // WE LIMIT MAX ACCELERATION of the motors 288 | if ((speed_M1 - tspeed) > MAX_ACCEL) 289 | speed_M1 -= MAX_ACCEL; 290 | else if ((speed_M1 - tspeed) < -MAX_ACCEL) 291 | speed_M1 += MAX_ACCEL; 292 | else 293 | speed_M1 = tspeed; 294 | 295 | #if MICROSTEPPING==16 296 | speed = speed_M1 * 46; // Adjust factor from control output speed to real motor speed in steps/second 297 | #else 298 | speed = speed_M1 * 23; // 1/8 Microstepping 299 | #endif 300 | 301 | if (speed == 0) 302 | { 303 | timer_period = ZERO_SPEED; 304 | dir_M1 = 0; 305 | } 306 | else if (speed > 0) 307 | { 308 | timer_period = 2000000 / speed; // 2Mhz timer 309 | dir_M1 = 1; 310 | SET(PORTB, 4); // DIR Motor 1 (Forward) 311 | } 312 | else 313 | { 314 | timer_period = 2000000 / -speed; 315 | dir_M1 = -1; 316 | CLR(PORTB, 4); // Dir Motor 1 317 | } 318 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 319 | timer_period = ZERO_SPEED; 320 | 321 | OCR1A = timer_period; 322 | // Check if we need to reset the timer... 323 | if (TCNT1 > OCR1A) 324 | TCNT1 = 0; 325 | } 326 | 327 | // Set speed of Stepper Motor2 328 | // tspeed could be positive or negative (reverse) 329 | void setMotorSpeedM2(int16_t tspeed) 330 | { 331 | long timer_period; 332 | int16_t speed; 333 | 334 | // Limit max speed? 335 | 336 | // WE LIMIT MAX ACCELERATION of the motors 337 | if ((speed_M2 - tspeed) > MAX_ACCEL) 338 | speed_M2 -= MAX_ACCEL; 339 | else if ((speed_M2 - tspeed) < -MAX_ACCEL) 340 | speed_M2 += MAX_ACCEL; 341 | else 342 | speed_M2 = tspeed; 343 | 344 | #if MICROSTEPPING==16 345 | speed = speed_M2 * 46; // Adjust factor from control output speed to real motor speed in steps/second 346 | #else 347 | speed = speed_M2 * 23; // 1/8 Microstepping 348 | #endif 349 | 350 | if (speed == 0) 351 | { 352 | timer_period = ZERO_SPEED; 353 | dir_M2 = 0; 354 | } 355 | else if (speed > 0) 356 | { 357 | timer_period = 2000000 / speed; // 2Mhz timer 358 | dir_M2 = 1; 359 | CLR(PORTC, 6); // Dir Motor2 (Forward) 360 | } 361 | else 362 | { 363 | timer_period = 2000000 / -speed; 364 | dir_M2 = -1; 365 | SET(PORTC, 6); // DIR Motor 2 366 | } 367 | if (timer_period > 65535) // Check for minimun speed (maximun period without overflow) 368 | timer_period = ZERO_SPEED; 369 | 370 | OCR3A = timer_period; 371 | // Check if we need to reset the timer... 372 | if (TCNT3 > OCR3A) 373 | TCNT3 = 0; 374 | } 375 | 376 | // Read control PID parameters from user. This is only for advanced users that want to "play" with the controllers... 377 | void readControlParameters() 378 | { 379 | // Parameter initialization (first time we enter page2) 380 | if ((OSC.page == 2) && (!modifing_control_parameters)) 381 | { 382 | OSC.fadder1 = 0.5; 383 | OSC.fadder2 = 0.5; 384 | OSC.fadder3 = 0.5; 385 | OSC.fadder4 = 0.0; 386 | //OSC.toggle1 = 0; 387 | modifing_control_parameters = true; 388 | } 389 | // Parameters Mode (page2 controls) 390 | // User could adjust KP, KD, KP_THROTTLE and KI_THROTTLE (fadder1,2,3,4) 391 | // Now we need to adjust all the parameters all the times because we don´t know what parameter has been moved 392 | if (OSC.page == 2) 393 | { 394 | Kp_user = KP * 2 * OSC.fadder1; 395 | Kd_user = KD * 2 * OSC.fadder2; 396 | Kp_thr_user = KP_THROTTLE * 2 * OSC.fadder3; 397 | Ki_thr_user = (KI_THROTTLE + 0.1) * 2 * OSC.fadder4; 398 | #if DEBUG>0 399 | Serial.print("Par: "); 400 | Serial.print(Kp_user); 401 | Serial.print(" "); 402 | Serial.print(Kd_user); 403 | Serial.print(" "); 404 | Serial.print(Kp_thr_user); 405 | Serial.print(" "); 406 | Serial.println(Ki_thr_user); 407 | #endif 408 | // Kill robot => Sleep 409 | while (OSC.toggle2) 410 | { 411 | //Reset external parameters 412 | mpu.resetFIFO(); 413 | PID_errorSum = 0; 414 | timer_old = millis(); 415 | setMotorSpeedM1(0); 416 | setMotorSpeedM2(0); 417 | OSC.MsgRead(); 418 | } 419 | newControlParameters = true; 420 | } 421 | if ((newControlParameters) && (!modifing_control_parameters)) 422 | { 423 | // Reset parameters 424 | OSC.fadder1 = 0.5; 425 | OSC.fadder2 = 0.5; 426 | //OSC.toggle1 = 0; 427 | newControlParameters = false; 428 | } 429 | } 430 | 431 | #ifdef WIFI_ESP 432 | 433 | int ESPwait(String stopstr, int timeout_secs) 434 | { 435 | String response; 436 | bool found = false; 437 | char c; 438 | long timer_init; 439 | long timer; 440 | 441 | timer_init = millis(); 442 | while (!found) { 443 | timer = millis(); 444 | if (((timer - timer_init) / 1000) > timeout_secs) { // Timeout? 445 | Serial.println("!Timeout!"); 446 | return 0; // timeout 447 | } 448 | if (Serial1_available()) { 449 | c = Serial1_read(); 450 | Serial.print(c); 451 | response += c; 452 | if (response.endsWith(stopstr)) { 453 | found = true; 454 | delay(10); 455 | Serial1_flush(); 456 | Serial.println(); 457 | } 458 | } // end Serial1_available() 459 | } // end while (!found) 460 | return 1; 461 | } 462 | 463 | // getMacAddress from ESP wifi module 464 | int ESPgetMac() 465 | { 466 | char c1, c2; 467 | bool timeout = false; 468 | long timer_init; 469 | long timer; 470 | uint8_t state = 0; 471 | uint8_t index = 0; 472 | 473 | MAC = ""; 474 | timer_init = millis(); 475 | while (!timeout) { 476 | timer = millis(); 477 | if (((timer - timer_init) / 1000) > 5) // Timeout? 478 | timeout = true; 479 | if (Serial1_available()) { 480 | c2 = c1; 481 | c1 = Serial1_read(); 482 | Serial.print(c1); 483 | switch (state) { 484 | case 0: 485 | if (c1 == ':') 486 | state = 1; 487 | break; 488 | case 1: 489 | if (c1 == '\r') { 490 | MAC.toUpperCase(); 491 | state = 2; 492 | } 493 | else { 494 | if ((c1 != '"') && (c1 != ':')) 495 | MAC += c1; // Uppercase 496 | } 497 | break; 498 | case 2: 499 | if ((c2 == 'O') && (c1 == 'K')) { 500 | Serial.println(); 501 | Serial1_flush(); 502 | return 1; // Ok 503 | } 504 | break; 505 | } // end switch 506 | } // Serial_available 507 | } // while (!timeout) 508 | Serial.println("!Timeout!"); 509 | Serial1_flush(); 510 | return -1; // timeout 511 | } 512 | 513 | int ESPsendCommand(char *command, String stopstr, int timeout_secs) 514 | { 515 | Serial1_println(command); 516 | ESPwait(stopstr, timeout_secs); 517 | delay(250); 518 | } 519 | #endif 520 | 521 | // INITIALIZATION 522 | void setup() 523 | { 524 | // STEPPER PINS ON JJROBOTS BROBOT BRAIN BOARD 525 | pinMode(4, OUTPUT); // ENABLE MOTORS 526 | pinMode(7, OUTPUT); // STEP MOTOR 1 PORTE,6 527 | pinMode(8, OUTPUT); // DIR MOTOR 1 PORTB,4 528 | pinMode(12, OUTPUT); // STEP MOTOR 2 PORTD,6 529 | pinMode(5, OUTPUT); // DIR MOTOR 2 PORTC,6 530 | digitalWrite(4, HIGH); // Disbale motors 531 | 532 | pinMode(10, OUTPUT); // Servo1 (arm) 533 | pinMode(13, OUTPUT); // Servo2 534 | 535 | Serial.begin(115200); // Serial output to console 536 | 537 | // Wifi module initialization: Wifi module should be preconfigured (use another sketch to configure the module) 538 | 539 | // Initialize I2C bus (MPU6050 is connected via I2C) 540 | Wire.begin(); 541 | // I2C 400Khz fast mode 542 | TWSR = 0; 543 | TWBR = ((16000000L / I2C_SPEED) - 16) / 2; 544 | TWCR = 1 << TWEN; 545 | 546 | #if DEBUG > 0 547 | delay(9000); 548 | #else 549 | delay(2000); 550 | #endif 551 | Serial.println("BROBOT by JJROBOTS v2.2"); 552 | Serial.println("Initializing I2C devices..."); 553 | //mpu.initialize(); 554 | // Manual MPU initialization... accel=2G, gyro=2000º/s, filter=20Hz BW, output=200Hz 555 | mpu.setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 556 | mpu.setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 557 | mpu.setFullScaleAccelRange(MPU6050_ACCEL_FS_2); 558 | mpu.setDLPFMode(MPU6050_DLPF_BW_10); //10,20,42,98,188 // Default factor for BROBOT:10 559 | mpu.setRate(4); // 0=1khz 1=500hz, 2=333hz, 3=250hz 4=200hz 560 | mpu.setSleepEnabled(false); 561 | 562 | delay(500); 563 | Serial.println("Initializing DMP..."); 564 | devStatus = mpu.dmpInitialize(); 565 | if (devStatus == 0) { 566 | // turn on the DMP, now that it's ready 567 | Serial.println("Enabling DMP..."); 568 | mpu.setDMPEnabled(true); 569 | mpuIntStatus = mpu.getIntStatus(); 570 | dmpReady = true; 571 | } 572 | else { // ERROR! 573 | // 1 = initial memory load failed 574 | // 2 = DMP configuration updates failed 575 | // (if it's going to break, usually the code will be 1) 576 | Serial.print("DMP Initialization failed (code "); 577 | Serial.print(devStatus); 578 | Serial.println(")"); 579 | } 580 | 581 | // Gyro calibration 582 | // The robot must be steady during initialization 583 | delay(500); 584 | Serial.println("Gyro calibration!! Dont move the robot in 10 seconds... "); 585 | delay(500); 586 | 587 | #ifdef WIFI_ESP 588 | // With the new ESP8266 WIFI MODULE WE NEED TO MAKE AN INITIALIZATION PROCESS 589 | Serial.println("Initalizing ESP Wifi Module..."); 590 | Serial.println("WIFI RESET"); 591 | Serial1_flush(); 592 | Serial1_print("+++"); // To ensure we exit the transparent transmision mode 593 | delay(100); 594 | ESPsendCommand("AT","OK",1); 595 | ESPsendCommand("AT+RST", "OK", 2); // ESP Wifi module RESET 596 | ESPwait("ready", 6); 597 | ESPsendCommand("AT+GMR", "OK", 5); 598 | Serial1_println("AT+CIPSTAMAC?"); 599 | ESPgetMac(); 600 | Serial.print("MAC:"); 601 | Serial.println(MAC); 602 | delay(250); 603 | ESPsendCommand("AT+CWQAP", "OK", 3); 604 | ESPsendCommand("AT+CWMODE=2", "OK", 3); // Soft AP mode 605 | // Generate Soft AP. SSID=JJROBOTS, PASS=87654321 606 | char *cmd = "AT+CWSAP=\"JJROBOTS_XX\",\"87654321\",5,3"; 607 | // Update XX characters with MAC address (last 2 characters) 608 | cmd[19] = MAC[10]; 609 | cmd[20] = MAC[11]; 610 | //const char cmd[50] = "AT+CWSAP=\"JJROBOTS_"+MAC.substring(MAC.length()-2)+"\",\"87654321\",5,3"; 611 | ESPsendCommand(cmd, "OK", 6); 612 | // Start UDP SERVER on port 2222 613 | Serial.println("Start UDP server at port 2222"); 614 | ESPsendCommand("AT+CIPMUX=0", "OK", 3); // Single connection mode 615 | ESPsendCommand("AT+CIPMODE=1", "OK", 3); // Transparent mode 616 | //ESPsendCommand("AT+CIPSTART=\"UDP\",\"0\",2223,2222,0", "OK", 3); 617 | ESPsendCommand("AT+CIPSTART=\"UDP\",\"192.168.4.2\",2223,2222,0", "OK", 3); 618 | delay(250); 619 | ESPsendCommand("AT+CIPSEND",">",2); // Start transmission (transparent mode) 620 | delay(5000); // Time to settle things... the bias_from_no_motion algorithm needs some time to take effect and reset gyro bias. 621 | #else 622 | delay(10000); // Time to settle things... the bias_from_no_motion algorithm needs some time to take effect and reset gyro bias. 623 | #endif 624 | 625 | // Verify connection 626 | Serial.println("Testing device connections..."); 627 | Serial.println(mpu.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 628 | timer_old = millis(); 629 | 630 | // Init servos 631 | Serial.println("Servo initialization..."); 632 | BROBOT.initServo(); 633 | BROBOT.moveServo1(SERVO_AUX_NEUTRO); 634 | 635 | // STEPPER MOTORS INITIALIZATION 636 | Serial.println("Steper motors initialization..."); 637 | // MOTOR1 => TIMER1 638 | TCCR1A = 0; // Timer1 CTC mode 4, OCxA,B outputs disconnected 639 | TCCR1B = (1 << WGM12) | (1 << CS11); // Prescaler=8, => 2Mhz 640 | OCR1A = ZERO_SPEED; // Motor stopped 641 | dir_M1 = 0; 642 | TCNT1 = 0; 643 | 644 | // MOTOR2 => TIMER3 645 | TCCR3A = 0; // Timer3 CTC mode 4, OCxA,B outputs disconnected 646 | TCCR3B = (1 << WGM32) | (1 << CS31); // Prescaler=8, => 2Mhz 647 | OCR3A = ZERO_SPEED; // Motor stopped 648 | dir_M2 = 0; 649 | TCNT3 = 0; 650 | 651 | //Adjust sensor fusion gain 652 | Serial.println("Adjusting DMP sensor fusion gain..."); 653 | dmpSetSensorFusionAccelGain(0x20); 654 | 655 | delay(200); 656 | 657 | // Enable stepper drivers and TIMER interrupts 658 | digitalWrite(4, LOW); // Enable stepper drivers 659 | // Enable TIMERs interrupts 660 | TIMSK1 |= (1 << OCIE1A); // Enable Timer1 interrupt 661 | TIMSK3 |= (1 << OCIE1A); // Enable Timer1 interrupt 662 | 663 | // Little motor vibration and servo move to indicate that robot is ready 664 | for (uint8_t k = 0; k < 5; k++) 665 | { 666 | setMotorSpeedM1(5); 667 | setMotorSpeedM2(5); 668 | BROBOT.moveServo1(SERVO_AUX_NEUTRO + 100); 669 | delay(200); 670 | setMotorSpeedM1(-5); 671 | setMotorSpeedM2(-5); 672 | BROBOT.moveServo1(SERVO_AUX_NEUTRO - 100); 673 | delay(200); 674 | } 675 | BROBOT.moveServo1(SERVO_AUX_NEUTRO); 676 | 677 | Serial.print("BATTERY:"); 678 | Serial.println(BROBOT.readBattery()); 679 | 680 | // OSC initialization 681 | OSC.fadder1 = 0.5; 682 | OSC.fadder2 = 0.5; 683 | 684 | Serial.println("Let´s start..."); 685 | 686 | mpu.resetFIFO(); 687 | timer_old = millis(); 688 | Robot_shutdown = false; 689 | mode = 0; 690 | } 691 | 692 | 693 | // MAIN LOOP 694 | void loop() 695 | { 696 | // If we run out of battery, we do nothing... STOP 697 | #if SHUTDOWN_WHEN__OFF==1 698 | if (Robot_shutdown) 699 | return; 700 | #endif 701 | 702 | debug_counter++; 703 | OSC.MsgRead(); // Read UDP OSC messages 704 | if (OSC.newMessage) 705 | { 706 | OSC.newMessage = 0; 707 | if (OSC.page == 1) // Get commands from user (PAGE1 are user commands: throttle, steering...) 708 | { 709 | OSC.newMessage = 0; 710 | throttle = (OSC.fadder1 - 0.5) * max_throttle; 711 | // We add some exponential on steering to smooth the center band 712 | steering = OSC.fadder2 - 0.5; 713 | if (steering > 0) 714 | steering = (steering * steering + 0.5 * steering) * max_steering; 715 | else 716 | steering = (-steering * steering + 0.5 * steering) * max_steering; 717 | 718 | modifing_control_parameters = false; 719 | if ((mode == 0) && (OSC.toggle1)) 720 | { 721 | // Change to PRO mode 722 | max_throttle = MAX_THROTTLE_PRO; 723 | max_steering = MAX_STEERING_PRO; 724 | max_target_angle = MAX_TARGET_ANGLE_PRO; 725 | mode = 1; 726 | } 727 | if ((mode == 1) && (OSC.toggle1 == 0)) 728 | { 729 | // Change to NORMAL mode 730 | max_throttle = MAX_THROTTLE; 731 | max_steering = MAX_STEERING; 732 | max_target_angle = MAX_TARGET_ANGLE; 733 | mode = 0; 734 | } 735 | } 736 | #if DEBUG==1 737 | Serial.print(throttle); 738 | Serial.print(" "); 739 | Serial.println(steering); 740 | #endif 741 | } // End new OSC message 742 | 743 | timer_value = millis(); 744 | 745 | // New DMP Orientation solution? 746 | fifoCount = mpu.getFIFOCount(); 747 | if (fifoCount >= 18) 748 | { 749 | if (fifoCount > 18) // If we have more than one packet we take the easy path: discard the buffer and wait for the next one 750 | { 751 | Serial.println("FIFO RESET!!"); 752 | mpu.resetFIFO(); 753 | return; 754 | } 755 | loop_counter++; 756 | slow_loop_counter++; 757 | dt = (timer_value - timer_old); 758 | timer_old = timer_value; 759 | 760 | angle_adjusted_Old = angle_adjusted; 761 | // Get new orientation angle from IMU (MPU6050) 762 | angle_adjusted = dmpGetPhi(); 763 | 764 | #if DEBUG==8 765 | Serial.print(throttle); 766 | Serial.print(" "); 767 | Serial.print(steering); 768 | Serial.print(" "); 769 | Serial.println(mode); 770 | #endif 771 | 772 | #if DEBUG==1 773 | Serial.println(angle_adjusted); 774 | #endif 775 | //Serial.print("\t"); 776 | mpu.resetFIFO(); // We always reset FIFO 777 | 778 | // We calculate the estimated robot speed: 779 | // Estimated_Speed = angular_velocity_of_stepper_motors(combined) - angular_velocity_of_robot(angle measured by IMU) 780 | actual_robot_speed_Old = actual_robot_speed; 781 | actual_robot_speed = (speed_M1 + speed_M2) / 2; // Positive: forward 782 | 783 | int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0; // 90 is an empirical extracted factor to adjust for real units 784 | int16_t estimated_speed = -actual_robot_speed_Old - angular_velocity; // We use robot_speed(t-1) or (t-2) to compensate the delay 785 | estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05; // low pass filter on estimated speed 786 | 787 | #if DEBUG==2 788 | Serial.print(" "); 789 | Serial.println(estimated_speed_filtered); 790 | #endif 791 | // SPEED CONTROL: This is a PI controller. 792 | // input:user throttle, variable: estimated robot speed, output: target robot angle to get the desired speed 793 | target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr); 794 | target_angle = constrain(target_angle, -max_target_angle, max_target_angle); // limited output 795 | 796 | #if DEBUG==3 797 | Serial.print(" "); 798 | Serial.println(estimated_speed_filtered); 799 | Serial.print(" "); 800 | Serial.println(target_angle); 801 | #endif 802 | 803 | // Stability control: This is a PD controller. 804 | // input: robot target angle(from SPEED CONTROL), variable: robot angle, output: Motor speed 805 | // We integrate the output (sumatory), so the output is really the motor acceleration, not motor speed. 806 | control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd); 807 | control_output = constrain(control_output, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); // Limit max output from control 808 | 809 | // The steering part from the user is injected directly on the output 810 | motor1 = control_output + steering; 811 | motor2 = control_output - steering; 812 | 813 | // Limit max speed (control output) 814 | motor1 = constrain(motor1, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); 815 | motor2 = constrain(motor2, -MAX_CONTROL_OUTPUT, MAX_CONTROL_OUTPUT); 816 | 817 | // NOW we send the commands to the motors 818 | if ((angle_adjusted < 76) && (angle_adjusted > -76)) // Is robot ready (upright?) 819 | { 820 | // NORMAL MODE 821 | digitalWrite(4, LOW); // Motors enable 822 | setMotorSpeedM1(motor1); 823 | setMotorSpeedM2(motor2); 824 | 825 | // Push1 Move servo arm 826 | if (OSC.push1) // Move arm 827 | { 828 | // Update to correct bug when the robot is lying backward 829 | if (angle_adjusted > -40) 830 | BROBOT.moveServo1(SERVO_MIN_PULSEWIDTH + 100); 831 | else 832 | BROBOT.moveServo1(SERVO_MAX_PULSEWIDTH + 100); 833 | } 834 | else 835 | BROBOT.moveServo1(SERVO_AUX_NEUTRO); 836 | 837 | // Push2 reset controls to neutral position 838 | if (OSC.push2) 839 | { 840 | OSC.fadder1 = 0.5; 841 | OSC.fadder2 = 0.5; 842 | } 843 | 844 | // Normal condition? 845 | if ((angle_adjusted < 45) && (angle_adjusted > -45)) 846 | { 847 | Kp = Kp_user; // Default user control gains 848 | Kd = Kd_user; 849 | Kp_thr = Kp_thr_user; 850 | Ki_thr = Ki_thr_user; 851 | } 852 | else // We are in the raise up procedure => we use special control parameters 853 | { 854 | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP 855 | Kd = KD_RAISEUP; 856 | Kp_thr = KP_THROTTLE_RAISEUP; 857 | Ki_thr = KI_THROTTLE_RAISEUP; 858 | } 859 | } 860 | else // Robot not ready (flat), angle > 70º => ROBOT OFF 861 | { 862 | digitalWrite(4, HIGH); // Disable motors 863 | setMotorSpeedM1(0); 864 | setMotorSpeedM2(0); 865 | PID_errorSum = 0; // Reset PID I term 866 | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP 867 | Kd = KD_RAISEUP; 868 | Kp_thr = KP_THROTTLE_RAISEUP; 869 | Ki_thr = KI_THROTTLE_RAISEUP; 870 | 871 | // if we pulse push1 button we raise up the robot with the servo arm 872 | if (OSC.push1) 873 | { 874 | // Because we know the robot orientation (face down of face up), we move the servo in the appropiate direction for raise up 875 | if (angle_adjusted > 0) 876 | BROBOT.moveServo1(SERVO_MIN_PULSEWIDTH); 877 | else 878 | BROBOT.moveServo1(SERVO_MAX_PULSEWIDTH); 879 | } 880 | else 881 | BROBOT.moveServo1(SERVO_AUX_NEUTRO); 882 | 883 | } 884 | // Check for new user control parameters 885 | readControlParameters(); 886 | 887 | } // End of new IMU data 888 | 889 | // Medium loop 40Hz 890 | if (loop_counter >= 5) 891 | { 892 | loop_counter = 0; 893 | // We do nothing here now... 894 | #if DEBUG==10 895 | Serial.print(angle_adjusted); 896 | Serial.print(" "); 897 | Serial.println(debugVariable); 898 | #endif 899 | } // End of medium loop 900 | 901 | if (slow_loop_counter >= 99) // 2Hz 902 | { 903 | slow_loop_counter = 0; 904 | // Read status 905 | #if BATTERY_CHECK==1 906 | int BatteryValue = BROBOT.readBattery(); 907 | sendBattery_counter++; 908 | if (sendBattery_counter>=10){ //Every 5 seconds we send a message 909 | sendBattery_counter=0; 910 | #if LIPOBATT==0 911 | // From >10.6 volts (100%) to 9.2 volts (0%) (aprox) 912 | float value = constrain((BatteryValue-92)/14.0,0.0,1.0); 913 | #else 914 | // For Lipo battery use better this config: (From >11.5v (100%) to 9.5v (0%) 915 | float value = constrain((BatteryValue-95)/20.0,0.0,1.0); 916 | #endif 917 | //Serial.println(value); 918 | OSC.MsgSend("/1/rotary1\0\0,f\0\0\0\0\0\0",20,value); 919 | } 920 | //if (Battery_value < BATTERY_WARNING){ 921 | // Serial.print("LOW BAT!! "); 922 | // } 923 | #endif // BATTERY_CHECK 924 | #if DEBUG==6 925 | Serial.print("B:"); 926 | Serial.println(BatteryValue); 927 | #endif 928 | #if SHUTDOWN_WHEN_BATTERY_OFF==1 929 | 930 | if (BROBOT.readBattery(); < BATTERY_SHUTDOWN) 931 | { 932 | // Robot shutdown !!! 933 | Serial.println("LOW BAT!! SHUTDOWN"); 934 | Robot_shutdown = true; 935 | // Disable steppers 936 | digitalWrite(4, HIGH); // Disable motors 937 | } 938 | #endif 939 | 940 | } // End of slow loop 941 | } 942 | 943 | 944 | 945 | 946 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | Self Balance arduino robot. Control via Smartphone. Fully 3D printed project. 294 | Copyright (C) 2013 Jose Julio 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | JJROBOTS B-ROBOT 2 | ================= 3 | Self Balancing Arduino robot. Control via Smartphone/Tablet. Fully 3D printed parts. 4 | New: This new version include support for the new V2 board! (with integrated ESP wifi module) 5 | 6 | All the documentation, features, how it works, build manuals and SHOP here: 7 | URL: http://jjrobots.com/b-robot 8 | 9 | This is the new and improved version of this old project: 10 | Blog: http://cienciaycacharreo.blogspot.com.es/2013/10/b-robot-un-robot-equilibrista-impreso_28.html 11 | 12 | To install code read the install.txt instructions 13 | 14 | JJROBOTS 2015. Science & fun! 15 | 16 | 17 | -------------------------------------------------------------------------------- /TouchOSC/BROBOT.touchosc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jjrobots/B-ROBOT/e56ca543972773a3251de853f4e5d3f47d61529f/TouchOSC/BROBOT.touchosc -------------------------------------------------------------------------------- /TouchOSC/install.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jjrobots/B-ROBOT/e56ca543972773a3251de853f4e5d3f47d61529f/TouchOSC/install.txt -------------------------------------------------------------------------------- /Utils/B_ROBOT_WIFI_CONFIG/B_ROBOT_WIFI_CONFIG.ino: -------------------------------------------------------------------------------- 1 | // CONFIGURATION UTILITY FOR RNXV WIFI MODULE 2 | // Author: JJROBOTS.COM (Jose Julio & Juan Pedro) 3 | // Date: 02/09/2014 4 | // Updated: 17/02/2015 5 | // Version: 1.01 6 | // License: GPL v2 7 | 8 | #include 9 | 10 | void setup() 11 | { 12 | delay(5000); 13 | Serial.begin(115200); 14 | 15 | // Wifi module initialization 16 | // We dont know if the module is at default 9600 or at 115200 so we reset on both baud rates... 17 | Serial.println("Factory Reset..."); 18 | Serial1.begin(9600); 19 | JJWIFI.WifiInit(); 20 | JJWIFI.WifiFactoryReset(); 21 | Serial1.end(); 22 | Serial1.begin(115200); // Default Wifi module Serial speed 23 | JJWIFI.WifiInit(); 24 | JJWIFI.WifiFactoryReset(); 25 | Serial.println("Wifi configuration..."); // CONFIGURATION ONLY NEEDED FIRST TIME 26 | JJWIFI.WifiChangeBaudRateFast(); 27 | JJWIFI.WifiEnableTCPUDP("2222","2223","192.168.1.11"); // Port 2222 28 | JJWIFI.WifiAP("JJROBOTS","12345678"); // Soft AP mode SSID:"JJROBOTS" pass "12345678", you could change the name... 29 | // The password only works for modules with firmware 4.41 or above 30 | 31 | delay(2000); 32 | Serial.println(); 33 | Serial.println("CONFIG:"); 34 | JJWIFI.WifiViewConfig(); 35 | Serial.println(); 36 | Serial.println("END!!"); 37 | Serial.println("The module should be ready to use!"); 38 | Serial.println(); 39 | Serial.println("ECHO MODE (to test)"); 40 | } 41 | 42 | 43 | // MAIN LOOP 44 | void loop() 45 | { 46 | char incomingByte; 47 | char incomingByte2; 48 | 49 | // ECHO from module (to test) 50 | // Bidirectional (to check or configure module) 51 | while (Serial1.available() > 0) { 52 | incomingByte = Serial1.read(); 53 | Serial.print(incomingByte); 54 | } 55 | while (Serial.available() > 0) { 56 | incomingByte2 = Serial.read(); 57 | Serial1.print(incomingByte2); 58 | } 59 | } 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /Utils/libraries/JJROBOTS_WIFI/JJROBOTS_WIFI.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | JJROBOTS_WIFI.cpp - Library for JJROBOTS wifi module. 3 | Code by Jose Julio and Juan Pedro. JJROBOTS.COM 4 | 5 | This library is free software; you can redistribute it and/or 6 | modify it under the terms of the GNU Lesser General Public 7 | License as published by the Free Software Foundation; either 8 | version 2.1 of the License, or (at your option) any later version 9 | 10 | 11 | */ 12 | #include "Arduino.h" 13 | #include "JJROBOTS_WIFI.h" 14 | 15 | #include 16 | //#include "WProgram.h" 17 | 18 | 19 | // Constructors //////////////////////////////////////////////////////////////// 20 | 21 | JJWIFI_Class::JJWIFI_Class() 22 | { 23 | } 24 | 25 | // Private function 26 | void _SerialEcho() 27 | { 28 | char incomingByte; 29 | 30 | delay(400); 31 | //Serial.println("SerialEcho"); 32 | while (Serial1.available() > 0) { 33 | incomingByte = Serial1.read(); 34 | Serial.print(incomingByte); 35 | } 36 | } 37 | 38 | 39 | // Public Methods ////////////////////////////////////////////////////////////// 40 | void JJWIFI_Class::WifiInit() 41 | { 42 | // Serial and Serial1 should be initialized on the main code... 43 | //Serial1.begin(115200); // Default Wifi module Serial speed 44 | //if (!Serial) 45 | // Serial.begin(115200); // Default baud rate if not initialized 46 | //delay(1000); 47 | } 48 | 49 | void JJWIFI_Class::WifiSendCommand(char command[],boolean EndOfLine) 50 | { 51 | if (EndOfLine){ 52 | Serial1.println(command); 53 | _SerialEcho(); 54 | } 55 | else{ 56 | Serial1.print(command); 57 | } 58 | delay(10); 59 | //return true; 60 | } 61 | 62 | void JJWIFI_Class::WifiEnterCommandMode() 63 | { 64 | // First we try an exit (to exit an old command mode) 65 | Serial1.print("exit\r"); 66 | _SerialEcho(); 67 | // Entramos en modo Comando 68 | Serial1.print("$$$"); 69 | _SerialEcho(); 70 | delay(10); 71 | } 72 | 73 | void JJWIFI_Class::WifiExitCommandMode() 74 | { 75 | // First we try an exit (to exit an old command mode) 76 | WifiSendCommand("exit"); 77 | delay(10); 78 | } 79 | 80 | void JJWIFI_Class::WifiFactoryReset() 81 | { 82 | WifiEnterCommandMode(); 83 | WifiSendCommand("factory RESET"); 84 | WifiSendCommand("reboot"); 85 | delay(1000); 86 | } 87 | 88 | // Change baudrate to 115200 89 | void JJWIFI_Class::WifiChangeBaudRateFast() 90 | { 91 | Serial1.end(); 92 | Serial1.begin(9600); // Deafult baud rate of Wifi module 93 | WifiEnterCommandMode(); 94 | WifiSendCommand("set uart baudrate 115200"); 95 | WifiSendCommand("save"); 96 | WifiSendCommand("reboot"); 97 | delay(500); 98 | Serial1.end(); 99 | Serial1.begin(115200); 100 | delay(500); 101 | } 102 | 103 | // Change baudrate to 9600 (default baud rate) 104 | void JJWIFI_Class::WifiChangeBaudRateSlow() 105 | { 106 | Serial1.end(); 107 | Serial1.begin(115200); // Fast baud rate 108 | WifiEnterCommandMode(); 109 | WifiSendCommand("set uart baudrate 9600"); 110 | WifiSendCommand("save"); 111 | WifiSendCommand("reboot"); 112 | delay(500); 113 | Serial1.end(); 114 | Serial1.begin(9600); 115 | } 116 | 117 | void JJWIFI_Class::WifiJoin(char ssid[], char password[], boolean WPA, boolean DHCP) 118 | { 119 | WifiEnterCommandMode(); 120 | if (DHCP) 121 | WifiSendCommand("set ip dhcp 1"); // Enable DHCP 122 | else 123 | WifiSendCommand("set ip dhcp 0"); // Disable DHCP (use static IP) 124 | if (WPA){ 125 | WifiSendCommand("set wlan phrase ",false); 126 | WifiSendCommand(password); 127 | } 128 | else{ // WEP 128bits 129 | WifiSendCommand("set wlan key ",false); 130 | WifiSendCommand(password); 131 | } 132 | WifiSendCommand("join ",false); 133 | WifiSendCommand(ssid); 134 | WifiExitCommandMode(); 135 | } 136 | 137 | void JJWIFI_Class::WifiSetIP(char ip[], char netmask[], char gateway[]) 138 | { 139 | WifiEnterCommandMode(); 140 | WifiSendCommand("set ip address ",false); 141 | WifiSendCommand(ip); 142 | WifiSendCommand("set ip netmask ",false); 143 | WifiSendCommand(netmask); 144 | WifiSendCommand("set ip gateway ",false); 145 | WifiSendCommand(gateway); 146 | WifiExitCommandMode(); 147 | } 148 | 149 | void JJWIFI_Class::WifiEnableUDP(char local_port[], char remote_port[], char remote[]) 150 | { 151 | WifiEnterCommandMode(); 152 | WifiSendCommand("set ip proto 1"); 153 | WifiSendCommand("set ip localport ",false); 154 | WifiSendCommand(local_port); 155 | WifiSendCommand("set ip host ",false); 156 | WifiSendCommand(remote); 157 | WifiSendCommand("set ip remote ",false); 158 | WifiSendCommand(remote_port); 159 | WifiSendCommand("save"); 160 | WifiSendCommand("reboot"); // We need to reboot to take effect 161 | delay(500); 162 | //Wifi_ExitCommandMode(); 163 | } 164 | 165 | void JJWIFI_Class::WifiEnableTCPUDP(char local_port[], char remote_port[], char remote[]) 166 | { 167 | WifiEnterCommandMode(); 168 | WifiSendCommand("set ip proto 3"); 169 | WifiSendCommand("set ip localport ",false); 170 | WifiSendCommand(local_port); 171 | WifiSendCommand("set ip host ",false); 172 | WifiSendCommand(remote); 173 | WifiSendCommand("set ip remote ",false); 174 | WifiSendCommand(remote_port); 175 | WifiSendCommand("save"); 176 | WifiSendCommand("reboot"); // We need to reboot to take effect 177 | delay(500); 178 | //Wifi_ExitCommandMode(); 179 | } 180 | 181 | 182 | void JJWIFI_Class::WifiViewConfig() 183 | { 184 | WifiEnterCommandMode(); 185 | WifiSendCommand("ver"); 186 | WifiSendCommand("get apmode"); 187 | WifiSendCommand("get ip"); 188 | //Wifi_SendCommand("get dns"); 189 | WifiSendCommand("get wlan"); 190 | WifiExitCommandMode(); 191 | } 192 | 193 | void JJWIFI_Class::WifiAP(char ssidname[], char passphrase[]) 194 | { 195 | WifiEnterCommandMode(); 196 | WifiSendCommand("set wlan join 7"); 197 | // Old firmwares 198 | WifiSendCommand("set wlan ssid ",false); 199 | WifiSendCommand(ssidname); 200 | WifiSendCommand("set wlan chan 1"); 201 | // Compatibility with firmware 4.41 Soft AP with WPA2 security 202 | WifiSendCommand("set apmode ssid ",false); 203 | WifiSendCommand(ssidname); 204 | WifiSendCommand("set apmode passphrase ",false); 205 | WifiSendCommand(passphrase); 206 | WifiSendCommand("set ip address 192.168.1.1"); 207 | WifiSendCommand("set ip netmask 255.255.255.0"); 208 | WifiSendCommand("set ip dhcp 4"); // DHCP server 209 | WifiSendCommand("save"); 210 | WifiSendCommand("reboot"); // We need to reboot to take effect 211 | } 212 | 213 | // make one instance for the user to use 214 | JJWIFI_Class JJWIFI; -------------------------------------------------------------------------------- /Utils/libraries/JJROBOTS_WIFI/JJROBOTS_WIFI.h: -------------------------------------------------------------------------------- 1 | #ifndef JJROBOTS_WIFI_h 2 | #define JJROBOTS_WIFI_h 3 | 4 | #include "Arduino.h" 5 | 6 | class JJWIFI_Class 7 | { 8 | private: 9 | public: 10 | JJWIFI_Class(); 11 | void WifiInit(); 12 | void WifiSendCommand(char command[],boolean EndOfLine = true); 13 | void WifiEnterCommandMode(); 14 | void WifiExitCommandMode(); 15 | void WifiFactoryReset(); 16 | void WifiChangeBaudRateFast(); 17 | void WifiChangeBaudRateSlow(); 18 | void WifiJoin(char ssid[], char password[], boolean WPA, boolean DHCP = true); 19 | void WifiSetIP(char ip[], char netmask[], char gateway[]); 20 | void WifiEnableUDP(char local_port[], char remote_port[], char remote[]); 21 | void WifiEnableTCPUDP(char local_port[], char remote_port[], char remote[]); 22 | void WifiAP(char ssidname[],char passphrase[]); 23 | void WifiViewConfig(); 24 | }; 25 | 26 | extern JJWIFI_Class JJWIFI; 27 | 28 | #endif -------------------------------------------------------------------------------- /Utils/libraries/JJROBOTS_WIFI/keywords.txt: -------------------------------------------------------------------------------- 1 | JJWIFI KEYWORD1 2 | 3 | WifiInit KEYWORD2 4 | 5 | WifiSendCommand KEYWORD2 6 | 7 | WifiEnterCommandMode KEYWORD2 8 | 9 | WifiExitCommandMode KEYWORD2 10 | 11 | WifiFactoryReset KEYWORD2 12 | 13 | WifiChangeBaudRateFast KEYWORD2 14 | 15 | WifiChangeBaudRateSlow KEYWORD2 16 | 17 | WifiJoin KEYWORD2 18 | 19 | WifiSetIP KEYWORD2 20 | 21 | WifiEnableUDP KEYWORD2 22 | WifiEnableTCPUDP KEYWORD2 23 | 24 | WifiViewConfig KEYWORD2 25 | 26 | WifiAP KEYWORD2 27 | -------------------------------------------------------------------------------- /hardware/electronics/brobot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jjrobots/B-ROBOT/e56ca543972773a3251de853f4e5d3f47d61529f/hardware/electronics/brobot.pdf -------------------------------------------------------------------------------- /install.txt: -------------------------------------------------------------------------------- 1 | 2 | - Install the Arduino IDE on your PC from (skip this step if you have arduino already installed) 3 | This code has been tested and developed on IDE version 1.0.5 4 | 5 | - Install the libraries 6 | Copy the folders inside the /libraries into the Arduino/libraries folder on your hard drive 7 | JJROBOTS_BROBOT 8 | JJROBOTS_OSC 9 | I2Cdev 10 | MPU6050 11 | 12 | - Copy the main code. 13 | Copy the /BROBOT folder to your prefered sketch folder 14 | 15 | - Compile and send the code 16 | Open your Arduino IDE 17 | Open the main code in /BROBOT/BROBOT.ino 18 | Connect your Leonardo board with the USB to the PC 19 | Note: If this is the first time you connect a Leonardo board to your PC maybe you will need to install the driver. 20 | Please follow the instructions on Arduino help 21 | Select the board as Leonardo (tools->board) 22 | Select the serial port that appears on the tools->Serial port 23 | Send the code to the board 24 | 25 | Have fun! 26 | -------------------------------------------------------------------------------- /libraries/I2Cdev/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2013-05-06 - add Francesco Ferrara's Fastwire v0.24 implementation with small modifications 7 | // 2013-05-05 - fix issue with writing bit values to words (Sasquatch/Farzanegan) 8 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 9 | // - add compiler warnings when using outdated or IDE or limited I2Cdev implementation 10 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 11 | // 2011-10-03 - added automatic Arduino version detection for ease of use 12 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 13 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 14 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 15 | // 2011-08-02 - added support for 16-bit registers 16 | // - fixed incorrect Doxygen comments on some methods 17 | // - added timeout value for read operations (thanks mem @ Arduino forums) 18 | // 2011-07-30 - changed read/write function structures to return success or byte counts 19 | // - made all methods static for multi-device memory savings 20 | // 2011-07-28 - initial release 21 | 22 | /* ============================================ 23 | I2Cdev device library code is placed under the MIT license 24 | Copyright (c) 2013 Jeff Rowberg 25 | 26 | Permission is hereby granted, free of charge, to any person obtaining a copy 27 | of this software and associated documentation files (the "Software"), to deal 28 | in the Software without restriction, including without limitation the rights 29 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 30 | copies of the Software, and to permit persons to whom the Software is 31 | furnished to do so, subject to the following conditions: 32 | 33 | The above copyright notice and this permission notice shall be included in 34 | all copies or substantial portions of the Software. 35 | 36 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 37 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 38 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 39 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 40 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 41 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 42 | THE SOFTWARE. 43 | =============================================== 44 | */ 45 | 46 | #ifndef _I2CDEV_H_ 47 | #define _I2CDEV_H_ 48 | 49 | // ----------------------------------------------------------------------------- 50 | // I2C interface implementation setting 51 | // ----------------------------------------------------------------------------- 52 | #define I2CDEV_IMPLEMENTATION I2CDEV_ARDUINO_WIRE 53 | //#define I2CDEV_IMPLEMENTATION I2CDEV_BUILTIN_FASTWIRE 54 | 55 | // comment this out if you are using a non-optimal IDE/implementation setting 56 | // but want the compiler to shut up about it 57 | #define I2CDEV_IMPLEMENTATION_WARNINGS 58 | 59 | // ----------------------------------------------------------------------------- 60 | // I2C interface implementation options 61 | // ----------------------------------------------------------------------------- 62 | #define I2CDEV_ARDUINO_WIRE 1 // Wire object from Arduino 63 | #define I2CDEV_BUILTIN_NBWIRE 2 // Tweaked Wire object from Gene Knight's NBWire project 64 | // ^^^ NBWire implementation is still buggy w/some interrupts! 65 | #define I2CDEV_BUILTIN_FASTWIRE 3 // FastWire object from Francesco Ferrara's project 66 | #define I2CDEV_I2CMASTER_LIBRARY 4 // I2C object from DSSCircuits I2C-Master Library at https://github.com/DSSCircuits/I2C-Master-Library 67 | 68 | // ----------------------------------------------------------------------------- 69 | // Arduino-style "Serial.print" debug constant (uncomment to enable) 70 | // ----------------------------------------------------------------------------- 71 | //#define I2CDEV_SERIAL_DEBUG 72 | 73 | #ifdef ARDUINO 74 | #if ARDUINO < 100 75 | #include "WProgram.h" 76 | #else 77 | #include "Arduino.h" 78 | #endif 79 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 80 | #include 81 | #endif 82 | #if I2CDEV_IMPLEMENTATION == I2CDEV_I2CMASTER_LIBRARY 83 | #include 84 | #endif 85 | #endif 86 | 87 | // 1000ms default read timeout (modify with "I2Cdev::readTimeout = [ms];") 88 | #define I2CDEV_DEFAULT_READ_TIMEOUT 1000 89 | 90 | class I2Cdev { 91 | public: 92 | I2Cdev(); 93 | 94 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 95 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 96 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 97 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 98 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 99 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 100 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 101 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 102 | 103 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 104 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 105 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 106 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 107 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 108 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 109 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 110 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 111 | 112 | static uint16_t readTimeout; 113 | }; 114 | 115 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 116 | ////////////////////// 117 | // FastWire 0.24 118 | // This is a library to help faster programs to read I2C devices. 119 | // Copyright(C) 2012 120 | // Francesco Ferrara 121 | ////////////////////// 122 | 123 | /* Master */ 124 | #define TW_START 0x08 125 | #define TW_REP_START 0x10 126 | 127 | /* Master Transmitter */ 128 | #define TW_MT_SLA_ACK 0x18 129 | #define TW_MT_SLA_NACK 0x20 130 | #define TW_MT_DATA_ACK 0x28 131 | #define TW_MT_DATA_NACK 0x30 132 | #define TW_MT_ARB_LOST 0x38 133 | 134 | /* Master Receiver */ 135 | #define TW_MR_ARB_LOST 0x38 136 | #define TW_MR_SLA_ACK 0x40 137 | #define TW_MR_SLA_NACK 0x48 138 | #define TW_MR_DATA_ACK 0x50 139 | #define TW_MR_DATA_NACK 0x58 140 | 141 | #define TW_OK 0 142 | #define TW_ERROR 1 143 | 144 | class Fastwire { 145 | private: 146 | static boolean waitInt(); 147 | 148 | public: 149 | static void setup(int khz, boolean pullup); 150 | static byte beginTransmission(byte device); 151 | static byte write(byte value); 152 | static byte writeBuf(byte device, byte address, byte *data, byte num); 153 | static byte readBuf(byte device, byte address, byte *data, byte num); 154 | static void reset(); 155 | static byte stop(); 156 | }; 157 | #endif 158 | 159 | #if I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 160 | // NBWire implementation based heavily on code by Gene Knight 161 | // Originally posted on the Arduino forum at http://arduino.cc/forum/index.php/topic,70705.0.html 162 | // Originally offered to the i2cdevlib project at http://arduino.cc/forum/index.php/topic,68210.30.html 163 | 164 | #define NBWIRE_BUFFER_LENGTH 32 165 | 166 | class TwoWire { 167 | private: 168 | static uint8_t rxBuffer[]; 169 | static uint8_t rxBufferIndex; 170 | static uint8_t rxBufferLength; 171 | 172 | static uint8_t txAddress; 173 | static uint8_t txBuffer[]; 174 | static uint8_t txBufferIndex; 175 | static uint8_t txBufferLength; 176 | 177 | // static uint8_t transmitting; 178 | static void (*user_onRequest)(void); 179 | static void (*user_onReceive)(int); 180 | static void onRequestService(void); 181 | static void onReceiveService(uint8_t*, int); 182 | 183 | public: 184 | TwoWire(); 185 | void begin(); 186 | void begin(uint8_t); 187 | void begin(int); 188 | void beginTransmission(uint8_t); 189 | //void beginTransmission(int); 190 | uint8_t endTransmission(uint16_t timeout=0); 191 | void nbendTransmission(void (*function)(int)) ; 192 | uint8_t requestFrom(uint8_t, int, uint16_t timeout=0); 193 | //uint8_t requestFrom(int, int); 194 | void nbrequestFrom(uint8_t, int, void (*function)(int)); 195 | void send(uint8_t); 196 | void send(uint8_t*, uint8_t); 197 | //void send(int); 198 | void send(char*); 199 | uint8_t available(void); 200 | uint8_t receive(void); 201 | void onReceive(void (*)(int)); 202 | void onRequest(void (*)(void)); 203 | }; 204 | 205 | #define TWI_READY 0 206 | #define TWI_MRX 1 207 | #define TWI_MTX 2 208 | #define TWI_SRX 3 209 | #define TWI_STX 4 210 | 211 | #define TW_WRITE 0 212 | #define TW_READ 1 213 | 214 | #define TW_MT_SLA_NACK 0x20 215 | #define TW_MT_DATA_NACK 0x30 216 | 217 | #define CPU_FREQ 16000000L 218 | #define TWI_FREQ 100000L 219 | #define TWI_BUFFER_LENGTH 32 220 | 221 | /* TWI Status is in TWSR, in the top 5 bits: TWS7 - TWS3 */ 222 | 223 | #define TW_STATUS_MASK (_BV(TWS7)|_BV(TWS6)|_BV(TWS5)|_BV(TWS4)|_BV(TWS3)) 224 | #define TW_STATUS (TWSR & TW_STATUS_MASK) 225 | #define TW_START 0x08 226 | #define TW_REP_START 0x10 227 | #define TW_MT_SLA_ACK 0x18 228 | #define TW_MT_SLA_NACK 0x20 229 | #define TW_MT_DATA_ACK 0x28 230 | #define TW_MT_DATA_NACK 0x30 231 | #define TW_MT_ARB_LOST 0x38 232 | #define TW_MR_ARB_LOST 0x38 233 | #define TW_MR_SLA_ACK 0x40 234 | #define TW_MR_SLA_NACK 0x48 235 | #define TW_MR_DATA_ACK 0x50 236 | #define TW_MR_DATA_NACK 0x58 237 | #define TW_ST_SLA_ACK 0xA8 238 | #define TW_ST_ARB_LOST_SLA_ACK 0xB0 239 | #define TW_ST_DATA_ACK 0xB8 240 | #define TW_ST_DATA_NACK 0xC0 241 | #define TW_ST_LAST_DATA 0xC8 242 | #define TW_SR_SLA_ACK 0x60 243 | #define TW_SR_ARB_LOST_SLA_ACK 0x68 244 | #define TW_SR_GCALL_ACK 0x70 245 | #define TW_SR_ARB_LOST_GCALL_ACK 0x78 246 | #define TW_SR_DATA_ACK 0x80 247 | #define TW_SR_DATA_NACK 0x88 248 | #define TW_SR_GCALL_DATA_ACK 0x90 249 | #define TW_SR_GCALL_DATA_NACK 0x98 250 | #define TW_SR_STOP 0xA0 251 | #define TW_NO_INFO 0xF8 252 | #define TW_BUS_ERROR 0x00 253 | 254 | //#define _MMIO_BYTE(mem_addr) (*(volatile uint8_t *)(mem_addr)) 255 | //#define _SFR_BYTE(sfr) _MMIO_BYTE(_SFR_ADDR(sfr)) 256 | 257 | #ifndef sbi // set bit 258 | #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit)) 259 | #endif // sbi 260 | 261 | #ifndef cbi // clear bit 262 | #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit)) 263 | #endif // cbi 264 | 265 | extern TwoWire Wire; 266 | 267 | #endif // I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_NBWIRE 268 | 269 | #endif /* _I2CDEV_H_ */ 270 | -------------------------------------------------------------------------------- /libraries/I2Cdev/keywords.txt: -------------------------------------------------------------------------------- 1 | ####################################### 2 | # Syntax Coloring Map For I2Cdev 3 | ####################################### 4 | 5 | ####################################### 6 | # Datatypes (KEYWORD1) 7 | ####################################### 8 | I2Cdev KEYWORD1 9 | 10 | ####################################### 11 | # Methods and Functions (KEYWORD2) 12 | ####################################### 13 | 14 | readBit KEYWORD2 15 | readBitW KEYWORD2 16 | readBits KEYWORD2 17 | readBitsW KEYWORD2 18 | readByte KEYWORD2 19 | readBytes KEYWORD2 20 | readWord KEYWORD2 21 | readWords KEYWORD2 22 | writeBit KEYWORD2 23 | writeBitW KEYWORD2 24 | writeBits KEYWORD2 25 | writeBitsW KEYWORD2 26 | writeByte KEYWORD2 27 | writeBytes KEYWORD2 28 | writeWord KEYWORD2 29 | writeWords KEYWORD2 30 | 31 | ####################################### 32 | # Instances (KEYWORD2) 33 | ####################################### 34 | 35 | ####################################### 36 | # Constants (LITERAL1) 37 | ####################################### 38 | 39 | -------------------------------------------------------------------------------- /libraries/JJROBOTS_BROBOT/JJROBOTS_BROBOT.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | JJROBOTS_BROBOT.cpp - Library for JJROBOTS BROBOT board. 3 | Code by Jose Julio and Juan Pedro. JJROBOTS.COM 4 | This library lets you control the servos of the board 5 | 6 | Updated: 25/10/2015. Support for new v2.1 board (new servo2 pin) 7 | 8 | This library is free software; you can redistribute it and/or 9 | modify it under the terms of the GNU Lesser General Public 10 | License as published by the Free Software Foundation; either 11 | version 2.1 of the License, or (at your option) any later version 12 | */ 13 | #include "JJROBOTS_BROBOT.h" 14 | #include "Arduino.h" 15 | 16 | // Default servo definitions 17 | #define SERVO_AUX_NEUTRO 1500 // Servo neutral position 18 | #define SERVO_MIN_PULSEWIDTH 700 19 | #define SERVO_MAX_PULSEWIDTH 2300 20 | 21 | #define BATT_VOLT_FACTOR 8 22 | 23 | // Constructors //////////////////////////////////////////////////////////////// 24 | BROBOT_Class::BROBOT_Class() 25 | { 26 | servo_min_pwm = SERVO_MIN_PULSEWIDTH; 27 | servo_max_pwm = SERVO_MAX_PULSEWIDTH; 28 | // First read of battery voltage: 29 | first_time=true; 30 | } 31 | 32 | // Private function 33 | 34 | // Public Methods ////////////////////////////////////////////////////////////// 35 | 36 | // Init servo on T4 timer. Output OC4B (Leonardo Pin10) 37 | // We configure the Timer4 for 11 bits PWM (enhacend precision) and 16.3ms period (OK for most servos) 38 | // Resolution: 8us per step (this is OK for servos, around 175 steps for typical servo) 39 | void BROBOT_Class::initServo() 40 | { 41 | int temp; 42 | 43 | // Initialize Timer4 as Fast PWM 44 | TCCR4A = (1< 11 bits 49 | 50 | temp = 1500>>3; 51 | TC4H = temp >> 8; 52 | OCR4B = temp & 0xff; 53 | 54 | // Reset timer 55 | TC4H = 0; 56 | TCNT4 = 0; 57 | 58 | // Set TOP to 1023 (10 bit timer) 59 | TC4H = 3; 60 | OCR4C = 0xFF; 61 | 62 | // OC4A = PC7 (Pin13) OC4B = PB6 (Pin10) OC4D = PD7 (Pin6) 63 | // Set pins as outputs 64 | DDRB |= (1 << 6); // OC4B = PB6 (Pin10 on Leonardo board) 65 | DDRC |= (1 << 7); // OC4A = PC7 (Pin13 on Leonardo board) 66 | DDRD |= (1 << 7); // OC4D = PD7 (Pin6 on Leonardo board) 67 | 68 | //Enable OC4A and OC4B and OCR4D output 69 | TCCR4A |= (1<>3; // Check max values and Resolution: 8us 79 | // 11 bits => 3 MSB bits on TC4H, LSB bits on OCR4B 80 | TC4H = pwm>>8; 81 | OCR4B = pwm & 0xFF; 82 | } 83 | 84 | void BROBOT_Class::moveServo2(int pwm) 85 | { 86 | pwm = constrain(pwm,servo_min_pwm,servo_max_pwm)>>3; // Check max values and Resolution: 8us 87 | // 11 bits => 3 MSB bits on TC4H, LSB bits on OCR4B 88 | TC4H = pwm>>8; 89 | OCR4A = pwm & 0xFF; // Old 2.0 boards servo2 output 90 | OCR4D = pwm & 0xFF; // New 2.1 boards servo2 output 91 | } 92 | 93 | // output : Battery voltage*10 (aprox) and noise filtered 94 | int BROBOT_Class::readBattery() 95 | { 96 | if (first_time) 97 | battery = analogRead(5)/BATT_VOLT_FACTOR; 98 | else 99 | battery = (battery*9 + (analogRead(5)/BATT_VOLT_FACTOR))/10; 100 | return battery; 101 | } 102 | 103 | 104 | // make one instance for the user to use 105 | BROBOT_Class BROBOT; -------------------------------------------------------------------------------- /libraries/JJROBOTS_BROBOT/JJROBOTS_BROBOT.h: -------------------------------------------------------------------------------- 1 | #ifndef JJROBOTS_BROBOT_h 2 | #define JJROBOTS_BROBOT_h 3 | 4 | class BROBOT_Class 5 | { 6 | private: 7 | int servo_min_pwm; 8 | int servo_max_pwm; 9 | int battery; 10 | bool first_time; 11 | public: 12 | BROBOT_Class(); 13 | void initServo(); 14 | void moveServo1(int pwm); 15 | void moveServo2(int pwm); 16 | int readBattery(); 17 | }; 18 | 19 | extern BROBOT_Class BROBOT; 20 | 21 | #endif -------------------------------------------------------------------------------- /libraries/JJROBOTS_BROBOT/keywords.txt: -------------------------------------------------------------------------------- 1 | BROBOT KEYWORD1 2 | initServo KEYWORD2 3 | moveServo1 KEYWORD2 4 | moveServo2 KEYWORD2 5 | readBattery KEYWORD2 6 | 7 | -------------------------------------------------------------------------------- /libraries/JJROBOTS_OSC/JJROBOTS_OSC.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | JJROBOTS_OSC.cpp - Library for OSC control (designed originally for JJROBOTS boards). 3 | This library is for a Leonardo board (or an atmega32U4 board) with a Wifi module connected on Serial1 4 | Code by Jose Julio and Juan Pedro. JJROBOTS.COM 5 | 6 | IMPORTART: This library rewrite the original Arduino Serial1 code (Serial interrupts) so you could 7 | not use the Arduino object Serial1 on your code! or you will get compiler errors! 8 | This library internally initialize the Serial1 port at 115200 bauds 9 | We need to rewrite the serial RX interrupt because original Arduino code discard new Serial bytes when the RX buffer is full. 10 | This code, when the RX buffer is full, discard OLD bytes and store the new ones <= This is what we need!! 11 | 12 | This library is free software; you can redistribute it and/or 13 | modify it under the terms of the GNU Lesser General Public 14 | License as published by the Free Software Foundation; either 15 | version 2.1 of the License, or (at your option) any later version. 16 | 17 | OSC Messages read: OSC: /page/command parameters 18 | FADDER (1,2,3,4) Ex: /1/fader1 f, XXXX => lenght:20, Param: float (0.0-1.0) 19 | XY (1,2) Ex: /1/xy1 f,f, XXXXXXXX => length: 24 Params: float,float (0.0-1.0) 20 | PUSH (1,2,3,4) Ex: /1/push1 f, XXXX => length:20 Param: float 21 | TOGGLE (1,2,3,4) Ex: /1/toggle1 f, XXXX => length:20 Param: floar 22 | OSC Message send: 23 | string to send (NOT WORKING YET!) 24 | 25 | Methods: 26 | MsgSend() 27 | MsgRead() 28 | 29 | Properties: 30 | page 31 | fader1,fader2,fader3,fader4 32 | xy1_x, xy1_y, xy2_x, xy2_y 33 | push1,push2,push3,push4 34 | toggle1,toggle2,toggle3,toggle4 35 | 36 | */ 37 | #include "Arduino.h" 38 | #include "JJROBOTS_OSC.h" 39 | 40 | #include 41 | 42 | // for DEBUG uncomment this lines... 43 | //#define DEBUG 0 44 | //#define DEBUG2 0 45 | //#define DEBUG3 0 46 | 47 | // We reescribe the ISR for USART1 48 | // Same definitions as in HardwareSerial.cpp 49 | #define SERIAL_BUFFER_SIZE 128 50 | 51 | struct ring_buffer 52 | { 53 | unsigned char buffer[SERIAL_BUFFER_SIZE]; 54 | volatile unsigned int head; 55 | volatile unsigned int tail; 56 | }; 57 | ring_buffer rx_bufferS1 = { { 0 }, 0, 0 }; 58 | uint8_t rx_bufferS1_overflow=0; 59 | 60 | inline void store_char(unsigned char c, ring_buffer *buffer) 61 | { 62 | int i = (unsigned int)(buffer->head + 1) % SERIAL_BUFFER_SIZE; 63 | 64 | // Always store new byte on ring buffer. 65 | buffer->buffer[buffer->head] = c; 66 | buffer->head = i; 67 | // Overflow? => Discard old bytes 68 | if (i == buffer->tail) { 69 | buffer->tail = (unsigned int)(buffer->tail + 1) % SERIAL_BUFFER_SIZE; // Move tail to discard old bytes 70 | rx_bufferS1_overflow = 1; // Buffer overflow flag 71 | } 72 | } 73 | 74 | ISR(USART1_RX_vect) 75 | { 76 | if (bit_is_clear(UCSR1A, UPE1)) { 77 | unsigned char c = UDR1; 78 | store_char(c, &rx_bufferS1); 79 | } else { 80 | unsigned char c = UDR1; 81 | }; 82 | } 83 | 84 | int Serial1_available(void) 85 | { 86 | return (unsigned int)(SERIAL_BUFFER_SIZE + rx_bufferS1.head - rx_bufferS1.tail) % SERIAL_BUFFER_SIZE; 87 | } 88 | 89 | unsigned char Serial1_read(void) 90 | { 91 | // if the head isn't ahead of the tail, we don't have any characters 92 | if (rx_bufferS1.head == rx_bufferS1.tail) { 93 | return -1; 94 | } else { 95 | unsigned char c = rx_bufferS1.buffer[rx_bufferS1.tail]; 96 | rx_bufferS1.tail = (unsigned int)(rx_bufferS1.tail + 1) % SERIAL_BUFFER_SIZE; 97 | return c; 98 | } 99 | } 100 | 101 | // Write to serial1 (without interrupts) 102 | void Serial1_write(uint8_t c) 103 | { 104 | while (!((UCSR1A) & (1 <> 8; 139 | UBRR1L = baud_setting; 140 | 141 | //transmitting = false; 142 | UCSR1B = (1< 0) { 248 | //Serial.print("B:"); 249 | //Serial.println(Serial1_available()); 250 | // We rotate the Buffer (we could implement a ring buffer in future) 251 | for (i=7;i>0;i--){ 252 | UDPBuffer[i] = UDPBuffer[i-1]; 253 | } 254 | UDPBuffer[0] = Serial1_read(); 255 | #ifdef DEBUG3 256 | Serial.print(UDPBuffer[0]); 257 | #endif 258 | // We look for an OSC message start like /x/ 259 | if ((UDPBuffer[0] == '/')&&(UDPBuffer[2] == '/')){ 260 | if (readStatus == 0){ 261 | page = UDPBuffer[1] - '0'; // Convert page to int 262 | readStatus = 1; 263 | touchMessage = 0; 264 | //Serial.print("$"); 265 | #ifdef DEBUG3 266 | Serial.println(); 267 | #endif 268 | } 269 | else{ 270 | readStatus = 1; 271 | Serial.println("!ERR:osc"); 272 | } 273 | return; 274 | } else if (readStatus==1){ // looking for the message type 275 | // Fadder /1/fadder1 ,f xxxx 276 | if ((UDPBuffer[3] == 'd')&&(UDPBuffer[2] == 'e')&&(UDPBuffer[1] == 'r')){ 277 | readStatus=2; // Message type detected 278 | readCounter=11; // Bytes to read the parameter 279 | readNumParams=1; // 1 parameters 280 | switch (UDPBuffer[0]){ // fadder number 281 | case '1': 282 | commandType = 1; 283 | #ifdef DEBUG2 284 | Serial.print("$FAD1$"); 285 | #endif 286 | break; 287 | case '2': 288 | commandType = 2; 289 | #ifdef DEBUG2 290 | Serial.print("$FAD2$"); 291 | #endif 292 | break; 293 | case '3': 294 | commandType = 3; 295 | #ifdef DEBUG2 296 | Serial.print("$FAD3$"); 297 | #endif 298 | break; 299 | case '4': 300 | commandType = 4; 301 | #ifdef DEBUG2 302 | Serial.print("$FAD4$"); 303 | #endif 304 | break; 305 | } 306 | return; 307 | } // end fadder 308 | // XY message 309 | if ((UDPBuffer[2] == 'x')&&(UDPBuffer[1] == 'y')){ 310 | readStatus=2; // Message type detected 311 | readCounter=14; // Bytes to read the parameters 312 | readNumParams=2; // 2 parameters 313 | switch (UDPBuffer[0]){ // xy number 314 | case '1': 315 | commandType = 11; 316 | #ifdef DEBUG2 317 | Serial.print("$XY1:"); 318 | #endif 319 | break; 320 | case '2': 321 | commandType = 12; 322 | #ifdef DEBUG2 323 | Serial.print("$XY2:"); 324 | #endif 325 | break; 326 | default: 327 | commandType = 11; 328 | #ifdef DEBUG2 329 | Serial.print("$XY:"); 330 | #endif 331 | break; 332 | } 333 | return; 334 | } // End XY message 335 | // Push message 336 | if ((UDPBuffer[3] == 'u')&&(UDPBuffer[2] == 's')&&(UDPBuffer[1] == 'h')){ 337 | readStatus=2; // Message type detected 338 | readCounter=10; // Bytes to read the parameter 339 | readNumParams=1; // 1 parameters 340 | switch (UDPBuffer[0]){ // push number 341 | case '1': 342 | commandType = 21; 343 | #ifdef DEBUG2 344 | Serial.print("$P1:"); 345 | #endif 346 | break; 347 | case '2': 348 | commandType = 22; 349 | #ifdef DEBUG2 350 | Serial.print("$P2:"); 351 | #endif 352 | break; 353 | case '3': 354 | commandType = 23; 355 | #ifdef DEBUG2 356 | Serial.print("$P3:"); 357 | #endif 358 | break; 359 | case '4': 360 | commandType = 24; 361 | #ifdef DEBUG2 362 | Serial.print("$P4:"); 363 | #endif 364 | break; 365 | } 366 | return; 367 | } // end push 368 | // Toggle message 369 | if ((UDPBuffer[3] == 'g')&&(UDPBuffer[2] == 'l')&&(UDPBuffer[1] == 'e')){ 370 | readStatus=2; // Message type detected 371 | readCounter=10; // Bytes to read the parameter 372 | readNumParams=1; // 1 parameters 373 | switch (UDPBuffer[0]){ // push number 374 | case '1': 375 | commandType = 31; 376 | #ifdef DEBUG2 377 | Serial.print("$T1:"); 378 | #endif 379 | break; 380 | case '2': 381 | commandType = 32; 382 | #ifdef DEBUG2 383 | Serial.print("$T2:"); 384 | #endif 385 | break; 386 | case '3': 387 | commandType = 33; 388 | #ifdef DEBUG2 389 | Serial.print("$T3:"); 390 | #endif 391 | break; 392 | case '4': 393 | commandType = 34; 394 | #ifdef DEBUG2 395 | Serial.print("$T4:"); 396 | #endif 397 | break; 398 | } 399 | return; 400 | } // end toggle 401 | } else if (readStatus==2){ 402 | if ((UDPBuffer[1] == '/')&&(UDPBuffer[0] == 'z')){ // Touch up message? (/z) [only on page1] 403 | if ((page == 1)&&(commandType<=2)){ // Touchup message only on Fadder1 and Fadder2 404 | touchMessage = 1; 405 | } 406 | else{ 407 | touchMessage = 0; 408 | readStatus = 0; //Finish 409 | } 410 | } 411 | readCounter--; // Reading counter until we reach the Parameter position 412 | if (readCounter<=0){ 413 | readStatus=3; 414 | value = extractParamFloat1(); 415 | readStatus=0; 416 | if ((value<0.0)||(value>1.0)) 417 | { 418 | Serial.println("!ERR:f1!"); 419 | return; 420 | } 421 | if (readNumParams==2){ 422 | value2 = extractParamFloat2(); 423 | if ((value2<0.0)||(value2>1.0)) 424 | { 425 | Serial.println("!ERR:OSCf2!"); 426 | return; 427 | } 428 | } 429 | newMessage=1; 430 | //Serial.println(value); 431 | switch (commandType){ 432 | case 1: 433 | fadder1 = value; 434 | if ((touchMessage)&&(value==0)){ 435 | fadder1=0.5; 436 | MsgSend("/1/fader1\0\0\0,f\0\0\0\0\0\0",20,0.5); 437 | } 438 | #ifdef DEBUG 439 | Serial.print("$F1:"); 440 | Serial.println(fadder1); 441 | #endif 442 | break; 443 | case 2: 444 | fadder2 = value; 445 | if ((touchMessage)&&(value==0)){ 446 | fadder2=0.5; 447 | MsgSend("/1/fader2\0\0\0,f\0\0\0\0\0\0",20,0.5); 448 | } 449 | #ifdef DEBUG 450 | Serial.print("$F2:"); 451 | Serial.println(fadder2); 452 | #endif 453 | break; 454 | case 3: 455 | fadder3 = value; 456 | #ifdef DEBUG 457 | Serial.print("$F3:"); 458 | Serial.println(fadder3); 459 | #endif 460 | break; 461 | case 4: 462 | fadder4 = value; 463 | #ifdef DEBUG 464 | Serial.print("$F4:"); 465 | Serial.println(fadder4); 466 | #endif 467 | break; 468 | case 11: 469 | xy1_x = value; 470 | xy1_y = value2; 471 | #ifdef DEBUG 472 | Serial.print("$XY1:"); 473 | Serial.print(xy1_x); 474 | Serial.print(","); 475 | Serial.println(xy1_y); 476 | #endif 477 | break; 478 | case 12: 479 | xy2_x = value; 480 | xy2_y = value2; 481 | #ifdef DEBUG 482 | Serial.print("$XY2:"); 483 | Serial.print(xy2_x); 484 | Serial.print(","); 485 | Serial.println(xy2_y); 486 | #endif 487 | break; 488 | case 21: 489 | if (value==0) 490 | push1 = 0; 491 | else 492 | push1 = 1; 493 | #ifdef DEBUG 494 | Serial.print("$P1:"); 495 | Serial.println((int)push1); 496 | #endif 497 | break; 498 | case 22: 499 | if (value==0) 500 | push2 = 0; 501 | else 502 | push2 = 1; 503 | #ifdef DEBUG 504 | Serial.print("$P2:"); 505 | Serial.println((int)push2); 506 | #endif 507 | break; 508 | case 23: 509 | if (value==0) 510 | push3 = 0; 511 | else 512 | push3 = 1; 513 | #ifdef DEBUG 514 | Serial.print("$P3:"); 515 | Serial.println((int)push3); 516 | #endif 517 | break; 518 | case 24: 519 | if (value==0) 520 | push4 = 0; 521 | else 522 | push4 = 1; 523 | #ifdef DEBUG 524 | Serial.print("$P4:"); 525 | Serial.println((int)push4); 526 | #endif 527 | break; 528 | case 31: 529 | if (value==0) 530 | toggle1 = 0; 531 | else 532 | toggle1 = 1; 533 | #ifdef DEBUG 534 | Serial.print("$T1:"); 535 | Serial.println((int)toggle1); 536 | #endif 537 | break; 538 | case 32: 539 | if (value==0) 540 | toggle2 = 0; 541 | else 542 | toggle2 = 1; 543 | #ifdef DEBUG 544 | Serial.print("$T2:"); 545 | Serial.println((int)toggle2); 546 | #endif 547 | break; 548 | case 33: 549 | if (value==0) 550 | toggle3 = 0; 551 | else 552 | toggle3 = 1; 553 | #ifdef DEBUG 554 | Serial.print("$T3:"); 555 | Serial.println((int)toggle3); 556 | #endif 557 | break; 558 | case 34: 559 | if (value==0) 560 | toggle4 = 0; 561 | else 562 | toggle4 = 1; 563 | #ifdef DEBUG 564 | Serial.print("$T4:"); 565 | Serial.println((int)toggle4); 566 | #endif 567 | break; 568 | } // switch 569 | } // if (Read_counter<=0) 570 | } // if (read_status==2) 571 | } // end Serial.available() 572 | } 573 | 574 | // make one instance for the user to use 575 | JJROBOTS_OSC_Class OSC; -------------------------------------------------------------------------------- /libraries/JJROBOTS_OSC/JJROBOTS_OSC.h: -------------------------------------------------------------------------------- 1 | #ifndef JJROBOTS_OSC_h 2 | #define JJROBOTS_OSC_h 3 | 4 | #include "Arduino.h" 5 | 6 | int Serial1_available(void); 7 | unsigned char Serial1_read(void); 8 | void Serial1_write(uint8_t c); 9 | void Serial1_print(const char str[]); 10 | void Serial1_println(const char str[]); 11 | void Serial1_flush(); 12 | 13 | class JJROBOTS_OSC_Class 14 | { 15 | private: 16 | // UPD input buffer 17 | char UDPBuffer[8]; 18 | // OSC message read variables 19 | unsigned char readStatus; 20 | unsigned char readCounter; 21 | unsigned char readNumParams; 22 | unsigned char commandType; 23 | unsigned char touchMessage; 24 | 25 | //char OSC_led1[21]="/1/led1\x00,f\x00\x00\x00\x00\x00\x00"; // Message for LED1 26 | //char OSC_led1[21] = {'/','1','/','l','e','d','1','\0',',','f','\0','\0','\0','\0','\0','\0'}; 27 | 28 | //char OSC_fadder1[21]="/2/fadder1\x00\x00,f\x00\x00\x00\x00\x00\x00"; // Message for fadder1 page 2 29 | 30 | float extractParamFloat1(); 31 | float extractParamFloat2(); 32 | 33 | public: 34 | // Controls 35 | uint8_t page; 36 | uint8_t newMessage; 37 | float fadder1; 38 | float fadder2; 39 | float fadder3; 40 | float fadder4; 41 | float xy1_x; 42 | float xy1_y; 43 | float xy2_x; 44 | float xy2_y; 45 | uint8_t push1; 46 | uint8_t push2; 47 | uint8_t push3; 48 | uint8_t push4; 49 | uint8_t toggle1; 50 | uint8_t toggle2; 51 | uint8_t toggle3; 52 | uint8_t toggle4; 53 | 54 | JJROBOTS_OSC_Class(); 55 | void MsgSend(char *c,unsigned char msgSize, float p); 56 | void MsgRead(); 57 | void MsgRead2(); 58 | void ParseMsg(); 59 | 60 | }; 61 | 62 | extern JJROBOTS_OSC_Class OSC; 63 | 64 | #endif -------------------------------------------------------------------------------- /libraries/JJROBOTS_OSC/examples/JJROBOTS_OSC_sample.ino: -------------------------------------------------------------------------------- 1 | /* 2 | Example of JJROBTS_OSC library. 3 | Code by Jose Julio and Juan Pedro . JJROBOTS.COM 4 | */ 5 | 6 | #include 7 | 8 | void setup() 9 | { 10 | Serial.begin(115200); // For debug output 11 | } 12 | void loop() 13 | { 14 | OSC.MsgRead(); 15 | if (OSC.newMessage) 16 | { 17 | Serial.print("Message received: "); 18 | } 19 | } 20 | -------------------------------------------------------------------------------- /libraries/JJROBOTS_OSC/keywords.txt: -------------------------------------------------------------------------------- 1 | OSC KEYWORD1 2 | MsgRead KEYWORD2 3 | MsgSend KEYWORD2 4 | -------------------------------------------------------------------------------- /libraries/MPU6050/Examples/MPU6050_DMP6/MPU6050_DMP6.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0) 2 | // 6/21/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added seamless Fastwire support 7 | // - added note about gyro calibration 8 | // 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error 9 | // 2012-06-20 - improved FIFO overflow handling and simplified read process 10 | // 2012-06-19 - completely rearranged DMP initialization code and simplification 11 | // 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly 12 | // 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING 13 | // 2012-06-05 - add gravity-compensated initial reference frame acceleration output 14 | // - add 3D math helper file to DMP6 example sketch 15 | // - add Euler output and Yaw/Pitch/Roll output formats 16 | // 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee) 17 | // 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250 18 | // 2012-05-30 - basic DMP initialization working 19 | 20 | /* ============================================ 21 | I2Cdev device library code is placed under the MIT license 22 | Copyright (c) 2012 Jeff Rowberg 23 | 24 | Permission is hereby granted, free of charge, to any person obtaining a copy 25 | of this software and associated documentation files (the "Software"), to deal 26 | in the Software without restriction, including without limitation the rights 27 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28 | copies of the Software, and to permit persons to whom the Software is 29 | furnished to do so, subject to the following conditions: 30 | 31 | The above copyright notice and this permission notice shall be included in 32 | all copies or substantial portions of the Software. 33 | 34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40 | THE SOFTWARE. 41 | =============================================== 42 | */ 43 | 44 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 45 | // for both classes must be in the include path of your project 46 | #include "I2Cdev.h" 47 | 48 | #include "MPU6050_6Axis_MotionApps20.h" 49 | //#include "MPU6050.h" // not necessary if using MotionApps include file 50 | 51 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 52 | // is used in I2Cdev.h 53 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 54 | #include "Wire.h" 55 | #endif 56 | 57 | // class default I2C address is 0x68 58 | // specific I2C addresses may be passed as a parameter here 59 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 60 | // AD0 high = 0x69 61 | MPU6050 mpu; 62 | //MPU6050 mpu(0x69); // <-- use for AD0 high 63 | 64 | /* ========================================================================= 65 | NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch 66 | depends on the MPU-6050's INT pin being connected to the Arduino's 67 | external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is 68 | digital I/O pin 2. 69 | * ========================================================================= */ 70 | 71 | /* ========================================================================= 72 | NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error 73 | when using Serial.write(buf, len). The Teapot output uses this method. 74 | The solution requires a modification to the Arduino USBAPI.h file, which 75 | is fortunately simple, but annoying. This will be fixed in the next IDE 76 | release. For more info, see these links: 77 | 78 | http://arduino.cc/forum/index.php/topic,109987.0.html 79 | http://code.google.com/p/arduino/issues/detail?id=958 80 | * ========================================================================= */ 81 | 82 | 83 | 84 | // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual 85 | // quaternion components in a [w, x, y, z] format (not best for parsing 86 | // on a remote host such as Processing or something though) 87 | //#define OUTPUT_READABLE_QUATERNION 88 | 89 | // uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles 90 | // (in degrees) calculated from the quaternions coming from the FIFO. 91 | // Note that Euler angles suffer from gimbal lock (for more info, see 92 | // http://en.wikipedia.org/wiki/Gimbal_lock) 93 | //#define OUTPUT_READABLE_EULER 94 | 95 | // uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/ 96 | // pitch/roll angles (in degrees) calculated from the quaternions coming 97 | // from the FIFO. Note this also requires gravity vector calculations. 98 | // Also note that yaw/pitch/roll angles suffer from gimbal lock (for 99 | // more info, see: http://en.wikipedia.org/wiki/Gimbal_lock) 100 | #define OUTPUT_READABLE_YAWPITCHROLL 101 | 102 | // uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration 103 | // components with gravity removed. This acceleration reference frame is 104 | // not compensated for orientation, so +X is always +X according to the 105 | // sensor, just without the effects of gravity. If you want acceleration 106 | // compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead. 107 | //#define OUTPUT_READABLE_REALACCEL 108 | 109 | // uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration 110 | // components with gravity removed and adjusted for the world frame of 111 | // reference (yaw is relative to initial orientation, since no magnetometer 112 | // is present in this case). Could be quite handy in some cases. 113 | //#define OUTPUT_READABLE_WORLDACCEL 114 | 115 | // uncomment "OUTPUT_TEAPOT" if you want output that matches the 116 | // format used for the InvenSense teapot demo 117 | //#define OUTPUT_TEAPOT 118 | 119 | 120 | 121 | #define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6) 122 | bool blinkState = false; 123 | 124 | // MPU control/status vars 125 | bool dmpReady = false; // set true if DMP init was successful 126 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 127 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 128 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 129 | uint16_t fifoCount; // count of all bytes currently in FIFO 130 | uint8_t fifoBuffer[64]; // FIFO storage buffer 131 | 132 | // orientation/motion vars 133 | Quaternion q; // [w, x, y, z] quaternion container 134 | VectorInt16 aa; // [x, y, z] accel sensor measurements 135 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 136 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 137 | VectorFloat gravity; // [x, y, z] gravity vector 138 | float euler[3]; // [psi, theta, phi] Euler angle container 139 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 140 | 141 | // packet structure for InvenSense teapot demo 142 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 143 | 144 | 145 | 146 | // ================================================================ 147 | // === INTERRUPT DETECTION ROUTINE === 148 | // ================================================================ 149 | 150 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 151 | void dmpDataReady() { 152 | mpuInterrupt = true; 153 | } 154 | 155 | 156 | 157 | // ================================================================ 158 | // === INITIAL SETUP === 159 | // ================================================================ 160 | 161 | void setup() { 162 | // join I2C bus (I2Cdev library doesn't do this automatically) 163 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 164 | Wire.begin(); 165 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz) 166 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 167 | Fastwire::setup(400, true); 168 | #endif 169 | 170 | // initialize serial communication 171 | // (115200 chosen because it is required for Teapot Demo output, but it's 172 | // really up to you depending on your project) 173 | Serial.begin(115200); 174 | while (!Serial); // wait for Leonardo enumeration, others continue immediately 175 | 176 | // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3v or Ardunio 177 | // Pro Mini running at 3.3v, cannot handle this baud rate reliably due to 178 | // the baud timing being too misaligned with processor ticks. You must use 179 | // 38400 or slower in these cases, or use some kind of external separate 180 | // crystal solution for the UART timer. 181 | 182 | // initialize device 183 | Serial.println(F("Initializing I2C devices...")); 184 | mpu.initialize(); 185 | 186 | // verify connection 187 | Serial.println(F("Testing device connections...")); 188 | Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); 189 | 190 | // wait for ready 191 | Serial.println(F("\nSend any character to begin DMP programming and demo: ")); 192 | while (Serial.available() && Serial.read()); // empty buffer 193 | while (!Serial.available()); // wait for data 194 | while (Serial.available() && Serial.read()); // empty buffer again 195 | 196 | // load and configure the DMP 197 | Serial.println(F("Initializing DMP...")); 198 | devStatus = mpu.dmpInitialize(); 199 | 200 | // supply your own gyro offsets here, scaled for min sensitivity 201 | mpu.setXGyroOffset(220); 202 | mpu.setYGyroOffset(76); 203 | mpu.setZGyroOffset(-85); 204 | mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 205 | 206 | // make sure it worked (returns 0 if so) 207 | if (devStatus == 0) { 208 | // turn on the DMP, now that it's ready 209 | Serial.println(F("Enabling DMP...")); 210 | mpu.setDMPEnabled(true); 211 | 212 | // enable Arduino interrupt detection 213 | Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)...")); 214 | attachInterrupt(0, dmpDataReady, RISING); 215 | mpuIntStatus = mpu.getIntStatus(); 216 | 217 | // set our DMP Ready flag so the main loop() function knows it's okay to use it 218 | Serial.println(F("DMP ready! Waiting for first interrupt...")); 219 | dmpReady = true; 220 | 221 | // get expected DMP packet size for later comparison 222 | packetSize = mpu.dmpGetFIFOPacketSize(); 223 | } else { 224 | // ERROR! 225 | // 1 = initial memory load failed 226 | // 2 = DMP configuration updates failed 227 | // (if it's going to break, usually the code will be 1) 228 | Serial.print(F("DMP Initialization failed (code ")); 229 | Serial.print(devStatus); 230 | Serial.println(F(")")); 231 | } 232 | 233 | // configure LED for output 234 | pinMode(LED_PIN, OUTPUT); 235 | } 236 | 237 | 238 | 239 | // ================================================================ 240 | // === MAIN PROGRAM LOOP === 241 | // ================================================================ 242 | 243 | void loop() { 244 | // if programming failed, don't try to do anything 245 | if (!dmpReady) return; 246 | 247 | // wait for MPU interrupt or extra packet(s) available 248 | while (!mpuInterrupt && fifoCount < packetSize) { 249 | // other program behavior stuff here 250 | // . 251 | // . 252 | // . 253 | // if you are really paranoid you can frequently test in between other 254 | // stuff to see if mpuInterrupt is true, and if so, "break;" from the 255 | // while() loop to immediately process the MPU data 256 | // . 257 | // . 258 | // . 259 | } 260 | 261 | // reset interrupt flag and get INT_STATUS byte 262 | mpuInterrupt = false; 263 | mpuIntStatus = mpu.getIntStatus(); 264 | 265 | // get current FIFO count 266 | fifoCount = mpu.getFIFOCount(); 267 | 268 | // check for overflow (this should never happen unless our code is too inefficient) 269 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 270 | // reset so we can continue cleanly 271 | mpu.resetFIFO(); 272 | Serial.println(F("FIFO overflow!")); 273 | 274 | // otherwise, check for DMP data ready interrupt (this should happen frequently) 275 | } else if (mpuIntStatus & 0x02) { 276 | // wait for correct available data length, should be a VERY short wait 277 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 278 | 279 | // read a packet from FIFO 280 | mpu.getFIFOBytes(fifoBuffer, packetSize); 281 | 282 | // track FIFO count here in case there is > 1 packet available 283 | // (this lets us immediately read more without waiting for an interrupt) 284 | fifoCount -= packetSize; 285 | 286 | #ifdef OUTPUT_READABLE_QUATERNION 287 | // display quaternion values in easy matrix form: w x y z 288 | mpu.dmpGetQuaternion(&q, fifoBuffer); 289 | Serial.print("quat\t"); 290 | Serial.print(q.w); 291 | Serial.print("\t"); 292 | Serial.print(q.x); 293 | Serial.print("\t"); 294 | Serial.print(q.y); 295 | Serial.print("\t"); 296 | Serial.println(q.z); 297 | #endif 298 | 299 | #ifdef OUTPUT_READABLE_EULER 300 | // display Euler angles in degrees 301 | mpu.dmpGetQuaternion(&q, fifoBuffer); 302 | mpu.dmpGetEuler(euler, &q); 303 | Serial.print("euler\t"); 304 | Serial.print(euler[0] * 180/M_PI); 305 | Serial.print("\t"); 306 | Serial.print(euler[1] * 180/M_PI); 307 | Serial.print("\t"); 308 | Serial.println(euler[2] * 180/M_PI); 309 | #endif 310 | 311 | #ifdef OUTPUT_READABLE_YAWPITCHROLL 312 | // display Euler angles in degrees 313 | mpu.dmpGetQuaternion(&q, fifoBuffer); 314 | mpu.dmpGetGravity(&gravity, &q); 315 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 316 | Serial.print("ypr\t"); 317 | Serial.print(ypr[0] * 180/M_PI); 318 | Serial.print("\t"); 319 | Serial.print(ypr[1] * 180/M_PI); 320 | Serial.print("\t"); 321 | Serial.println(ypr[2] * 180/M_PI); 322 | #endif 323 | 324 | #ifdef OUTPUT_READABLE_REALACCEL 325 | // display real acceleration, adjusted to remove gravity 326 | mpu.dmpGetQuaternion(&q, fifoBuffer); 327 | mpu.dmpGetAccel(&aa, fifoBuffer); 328 | mpu.dmpGetGravity(&gravity, &q); 329 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 330 | Serial.print("areal\t"); 331 | Serial.print(aaReal.x); 332 | Serial.print("\t"); 333 | Serial.print(aaReal.y); 334 | Serial.print("\t"); 335 | Serial.println(aaReal.z); 336 | #endif 337 | 338 | #ifdef OUTPUT_READABLE_WORLDACCEL 339 | // display initial world-frame acceleration, adjusted to remove gravity 340 | // and rotated based on known orientation from quaternion 341 | mpu.dmpGetQuaternion(&q, fifoBuffer); 342 | mpu.dmpGetAccel(&aa, fifoBuffer); 343 | mpu.dmpGetGravity(&gravity, &q); 344 | mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); 345 | mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q); 346 | Serial.print("aworld\t"); 347 | Serial.print(aaWorld.x); 348 | Serial.print("\t"); 349 | Serial.print(aaWorld.y); 350 | Serial.print("\t"); 351 | Serial.println(aaWorld.z); 352 | #endif 353 | 354 | #ifdef OUTPUT_TEAPOT 355 | // display quaternion values in InvenSense Teapot demo format: 356 | teapotPacket[2] = fifoBuffer[0]; 357 | teapotPacket[3] = fifoBuffer[1]; 358 | teapotPacket[4] = fifoBuffer[4]; 359 | teapotPacket[5] = fifoBuffer[5]; 360 | teapotPacket[6] = fifoBuffer[8]; 361 | teapotPacket[7] = fifoBuffer[9]; 362 | teapotPacket[8] = fifoBuffer[12]; 363 | teapotPacket[9] = fifoBuffer[13]; 364 | Serial.write(teapotPacket, 14); 365 | teapotPacket[11]++; // packetCount, loops at 0xFF on purpose 366 | #endif 367 | 368 | // blink LED to indicate activity 369 | blinkState = !blinkState; 370 | digitalWrite(LED_PIN, blinkState); 371 | } 372 | } 373 | -------------------------------------------------------------------------------- /libraries/MPU6050/Examples/MPU6050_DMP6/Processing/MPUTeapot.pde: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Processing sketch for MPU6050 DMP output 2 | // 6/20/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-20 - initial release 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | import processing.serial.*; 33 | import processing.opengl.*; 34 | import toxi.geom.*; 35 | import toxi.processing.*; 36 | 37 | // NOTE: requires ToxicLibs to be installed in order to run properly. 38 | // 1. Download from http://toxiclibs.org/downloads 39 | // 2. Extract into [userdir]/Processing/libraries 40 | // (location may be different on Mac/Linux) 41 | // 3. Run and bask in awesomeness 42 | 43 | ToxiclibsSupport gfx; 44 | 45 | Serial port; // The serial port 46 | char[] teapotPacket = new char[14]; // InvenSense Teapot packet 47 | int serialCount = 0; // current packet byte position 48 | int aligned = 0; 49 | int interval = 0; 50 | 51 | float[] q = new float[4]; 52 | Quaternion quat = new Quaternion(1, 0, 0, 0); 53 | 54 | float[] gravity = new float[3]; 55 | float[] euler = new float[3]; 56 | float[] ypr = new float[3]; 57 | 58 | void setup() { 59 | // 300px square viewport using OpenGL rendering 60 | size(300, 300, OPENGL); 61 | gfx = new ToxiclibsSupport(this); 62 | 63 | // setup lights and antialiasing 64 | lights(); 65 | smooth(); 66 | 67 | // display serial port list for debugging/clarity 68 | println(Serial.list()); 69 | 70 | // get the first available port (use EITHER this OR the specific port code below) 71 | String portName = Serial.list()[0]; 72 | 73 | // get a specific serial port (use EITHER this OR the first-available code above) 74 | //String portName = "COM4"; 75 | 76 | // open the serial port 77 | port = new Serial(this, portName, 115200); 78 | 79 | // send single character to trigger DMP init/start 80 | // (expected by MPU6050_DMP6 example Arduino sketch) 81 | port.write('r'); 82 | } 83 | 84 | void draw() { 85 | if (millis() - interval > 1000) { 86 | // resend single character to trigger DMP init/start 87 | // in case the MPU is halted/reset while applet is running 88 | port.write('r'); 89 | interval = millis(); 90 | } 91 | 92 | // black background 93 | background(0); 94 | 95 | // translate everything to the middle of the viewport 96 | pushMatrix(); 97 | translate(width / 2, height / 2); 98 | 99 | // 3-step rotation from yaw/pitch/roll angles (gimbal lock!) 100 | // ...and other weirdness I haven't figured out yet 101 | //rotateY(-ypr[0]); 102 | //rotateZ(-ypr[1]); 103 | //rotateX(-ypr[2]); 104 | 105 | // toxiclibs direct angle/axis rotation from quaternion (NO gimbal lock!) 106 | // (axis order [1, 3, 2] and inversion [-1, +1, +1] is a consequence of 107 | // different coordinate system orientation assumptions between Processing 108 | // and InvenSense DMP) 109 | float[] axis = quat.toAxisAngle(); 110 | rotate(axis[0], -axis[1], axis[3], axis[2]); 111 | 112 | // draw main body in red 113 | fill(255, 0, 0, 200); 114 | box(10, 10, 200); 115 | 116 | // draw front-facing tip in blue 117 | fill(0, 0, 255, 200); 118 | pushMatrix(); 119 | translate(0, 0, -120); 120 | rotateX(PI/2); 121 | drawCylinder(0, 20, 20, 8); 122 | popMatrix(); 123 | 124 | // draw wings and tail fin in green 125 | fill(0, 255, 0, 200); 126 | beginShape(TRIANGLES); 127 | vertex(-100, 2, 30); vertex(0, 2, -80); vertex(100, 2, 30); // wing top layer 128 | vertex(-100, -2, 30); vertex(0, -2, -80); vertex(100, -2, 30); // wing bottom layer 129 | vertex(-2, 0, 98); vertex(-2, -30, 98); vertex(-2, 0, 70); // tail left layer 130 | vertex( 2, 0, 98); vertex( 2, -30, 98); vertex( 2, 0, 70); // tail right layer 131 | endShape(); 132 | beginShape(QUADS); 133 | vertex(-100, 2, 30); vertex(-100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); 134 | vertex( 100, 2, 30); vertex( 100, -2, 30); vertex( 0, -2, -80); vertex( 0, 2, -80); 135 | vertex(-100, 2, 30); vertex(-100, -2, 30); vertex(100, -2, 30); vertex(100, 2, 30); 136 | vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, -30, 98); vertex(-2, -30, 98); 137 | vertex(-2, 0, 98); vertex(2, 0, 98); vertex(2, 0, 70); vertex(-2, 0, 70); 138 | vertex(-2, -30, 98); vertex(2, -30, 98); vertex(2, 0, 70); vertex(-2, 0, 70); 139 | endShape(); 140 | 141 | popMatrix(); 142 | } 143 | 144 | void serialEvent(Serial port) { 145 | interval = millis(); 146 | while (port.available() > 0) { 147 | int ch = port.read(); 148 | print((char)ch); 149 | if (aligned < 4) { 150 | // make sure we are properly aligned on a 14-byte packet 151 | if (serialCount == 0) { 152 | if (ch == '$') aligned++; else aligned = 0; 153 | } else if (serialCount == 1) { 154 | if (ch == 2) aligned++; else aligned = 0; 155 | } else if (serialCount == 12) { 156 | if (ch == '\r') aligned++; else aligned = 0; 157 | } else if (serialCount == 13) { 158 | if (ch == '\n') aligned++; else aligned = 0; 159 | } 160 | //println(ch + " " + aligned + " " + serialCount); 161 | serialCount++; 162 | if (serialCount == 14) serialCount = 0; 163 | } else { 164 | if (serialCount > 0 || ch == '$') { 165 | teapotPacket[serialCount++] = (char)ch; 166 | if (serialCount == 14) { 167 | serialCount = 0; // restart packet byte position 168 | 169 | // get quaternion from data packet 170 | q[0] = ((teapotPacket[2] << 8) | teapotPacket[3]) / 16384.0f; 171 | q[1] = ((teapotPacket[4] << 8) | teapotPacket[5]) / 16384.0f; 172 | q[2] = ((teapotPacket[6] << 8) | teapotPacket[7]) / 16384.0f; 173 | q[3] = ((teapotPacket[8] << 8) | teapotPacket[9]) / 16384.0f; 174 | for (int i = 0; i < 4; i++) if (q[i] >= 2) q[i] = -4 + q[i]; 175 | 176 | // set our toxilibs quaternion to new data 177 | quat.set(q[0], q[1], q[2], q[3]); 178 | 179 | /* 180 | // below calculations unnecessary for orientation only using toxilibs 181 | 182 | // calculate gravity vector 183 | gravity[0] = 2 * (q[1]*q[3] - q[0]*q[2]); 184 | gravity[1] = 2 * (q[0]*q[1] + q[2]*q[3]); 185 | gravity[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]; 186 | 187 | // calculate Euler angles 188 | euler[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); 189 | euler[1] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]); 190 | euler[2] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1); 191 | 192 | // calculate yaw/pitch/roll angles 193 | ypr[0] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1); 194 | ypr[1] = atan(gravity[0] / sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2])); 195 | ypr[2] = atan(gravity[1] / sqrt(gravity[0]*gravity[0] + gravity[2]*gravity[2])); 196 | 197 | // output various components for debugging 198 | //println("q:\t" + round(q[0]*100.0f)/100.0f + "\t" + round(q[1]*100.0f)/100.0f + "\t" + round(q[2]*100.0f)/100.0f + "\t" + round(q[3]*100.0f)/100.0f); 199 | //println("euler:\t" + euler[0]*180.0f/PI + "\t" + euler[1]*180.0f/PI + "\t" + euler[2]*180.0f/PI); 200 | //println("ypr:\t" + ypr[0]*180.0f/PI + "\t" + ypr[1]*180.0f/PI + "\t" + ypr[2]*180.0f/PI); 201 | */ 202 | } 203 | } 204 | } 205 | } 206 | } 207 | 208 | void drawCylinder(float topRadius, float bottomRadius, float tall, int sides) { 209 | float angle = 0; 210 | float angleIncrement = TWO_PI / sides; 211 | beginShape(QUAD_STRIP); 212 | for (int i = 0; i < sides + 1; ++i) { 213 | vertex(topRadius*cos(angle), 0, topRadius*sin(angle)); 214 | vertex(bottomRadius*cos(angle), tall, bottomRadius*sin(angle)); 215 | angle += angleIncrement; 216 | } 217 | endShape(); 218 | 219 | // If it is not a cone, draw the circular top cap 220 | if (topRadius != 0) { 221 | angle = 0; 222 | beginShape(TRIANGLE_FAN); 223 | 224 | // Center point 225 | vertex(0, 0, 0); 226 | for (int i = 0; i < sides + 1; i++) { 227 | vertex(topRadius * cos(angle), 0, topRadius * sin(angle)); 228 | angle += angleIncrement; 229 | } 230 | endShape(); 231 | } 232 | 233 | // If it is not a cone, draw the circular bottom cap 234 | if (bottomRadius != 0) { 235 | angle = 0; 236 | beginShape(TRIANGLE_FAN); 237 | 238 | // Center point 239 | vertex(0, tall, 0); 240 | for (int i = 0; i < sides + 1; i++) { 241 | vertex(bottomRadius * cos(angle), tall, bottomRadius * sin(angle)); 242 | angle += angleIncrement; 243 | } 244 | endShape(); 245 | } 246 | } 247 | -------------------------------------------------------------------------------- /libraries/MPU6050/Examples/MPU6050_raw/MPU6050_raw.ino: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class 2 | // 10/7/2011 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2013-05-08 - added multiple output formats 7 | // - added seamless Fastwire support 8 | // 2011-10-07 - initial release 9 | 10 | /* ============================================ 11 | I2Cdev device library code is placed under the MIT license 12 | Copyright (c) 2011 Jeff Rowberg 13 | 14 | Permission is hereby granted, free of charge, to any person obtaining a copy 15 | of this software and associated documentation files (the "Software"), to deal 16 | in the Software without restriction, including without limitation the rights 17 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 18 | copies of the Software, and to permit persons to whom the Software is 19 | furnished to do so, subject to the following conditions: 20 | 21 | The above copyright notice and this permission notice shall be included in 22 | all copies or substantial portions of the Software. 23 | 24 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 25 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 26 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 27 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 28 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 29 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 30 | THE SOFTWARE. 31 | =============================================== 32 | */ 33 | 34 | // I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files 35 | // for both classes must be in the include path of your project 36 | #include "I2Cdev.h" 37 | #include "MPU6050.h" 38 | 39 | // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation 40 | // is used in I2Cdev.h 41 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 42 | #include "Wire.h" 43 | #endif 44 | 45 | // class default I2C address is 0x68 46 | // specific I2C addresses may be passed as a parameter here 47 | // AD0 low = 0x68 (default for InvenSense evaluation board) 48 | // AD0 high = 0x69 49 | MPU6050 accelgyro; 50 | //MPU6050 accelgyro(0x69); // <-- use for AD0 high 51 | 52 | int16_t ax, ay, az; 53 | int16_t gx, gy, gz; 54 | 55 | 56 | 57 | // uncomment "OUTPUT_READABLE_ACCELGYRO" if you want to see a tab-separated 58 | // list of the accel X/Y/Z and then gyro X/Y/Z values in decimal. Easy to read, 59 | // not so easy to parse, and slow(er) over UART. 60 | #define OUTPUT_READABLE_ACCELGYRO 61 | 62 | // uncomment "OUTPUT_BINARY_ACCELGYRO" to send all 6 axes of data as 16-bit 63 | // binary, one right after the other. This is very fast (as fast as possible 64 | // without compression or data loss), and easy to parse, but impossible to read 65 | // for a human. 66 | //#define OUTPUT_BINARY_ACCELGYRO 67 | 68 | 69 | #define LED_PIN 13 70 | bool blinkState = false; 71 | 72 | void setup() { 73 | // join I2C bus (I2Cdev library doesn't do this automatically) 74 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 75 | Wire.begin(); 76 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 77 | Fastwire::setup(400, true); 78 | #endif 79 | 80 | // initialize serial communication 81 | // (38400 chosen because it works as well at 8MHz as it does at 16MHz, but 82 | // it's really up to you depending on your project) 83 | Serial.begin(38400); 84 | 85 | // initialize device 86 | Serial.println("Initializing I2C devices..."); 87 | accelgyro.initialize(); 88 | 89 | // verify connection 90 | Serial.println("Testing device connections..."); 91 | Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed"); 92 | 93 | // use the code below to change accel/gyro offset values 94 | /* 95 | Serial.println("Updating internal sensor offsets..."); 96 | // -76 -2359 1688 0 0 0 97 | Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 98 | Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 99 | Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 100 | Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 101 | Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 102 | Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 103 | Serial.print("\n"); 104 | accelgyro.setXGyroOffset(220); 105 | accelgyro.setYGyroOffset(76); 106 | accelgyro.setZGyroOffset(-85); 107 | Serial.print(accelgyro.getXAccelOffset()); Serial.print("\t"); // -76 108 | Serial.print(accelgyro.getYAccelOffset()); Serial.print("\t"); // -2359 109 | Serial.print(accelgyro.getZAccelOffset()); Serial.print("\t"); // 1688 110 | Serial.print(accelgyro.getXGyroOffset()); Serial.print("\t"); // 0 111 | Serial.print(accelgyro.getYGyroOffset()); Serial.print("\t"); // 0 112 | Serial.print(accelgyro.getZGyroOffset()); Serial.print("\t"); // 0 113 | Serial.print("\n"); 114 | */ 115 | 116 | // configure Arduino LED for 117 | pinMode(LED_PIN, OUTPUT); 118 | } 119 | 120 | void loop() { 121 | // read raw accel/gyro measurements from device 122 | accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); 123 | 124 | // these methods (and a few others) are also available 125 | //accelgyro.getAcceleration(&ax, &ay, &az); 126 | //accelgyro.getRotation(&gx, &gy, &gz); 127 | 128 | #ifdef OUTPUT_READABLE_ACCELGYRO 129 | // display tab-separated accel/gyro x/y/z values 130 | Serial.print("a/g:\t"); 131 | Serial.print(ax); Serial.print("\t"); 132 | Serial.print(ay); Serial.print("\t"); 133 | Serial.print(az); Serial.print("\t"); 134 | Serial.print(gx); Serial.print("\t"); 135 | Serial.print(gy); Serial.print("\t"); 136 | Serial.println(gz); 137 | #endif 138 | 139 | #ifdef OUTPUT_BINARY_ACCELGYRO 140 | Serial.write((uint8_t)(ax >> 8)); Serial.write((uint8_t)(ax & 0xFF)); 141 | Serial.write((uint8_t)(ay >> 8)); Serial.write((uint8_t)(ay & 0xFF)); 142 | Serial.write((uint8_t)(az >> 8)); Serial.write((uint8_t)(az & 0xFF)); 143 | Serial.write((uint8_t)(gx >> 8)); Serial.write((uint8_t)(gx & 0xFF)); 144 | Serial.write((uint8_t)(gy >> 8)); Serial.write((uint8_t)(gy & 0xFF)); 145 | Serial.write((uint8_t)(gz >> 8)); Serial.write((uint8_t)(gz & 0xFF)); 146 | #endif 147 | 148 | // blink LED to indicate activity 149 | blinkState = !blinkState; 150 | digitalWrite(LED_PIN, blinkState); 151 | } 152 | -------------------------------------------------------------------------------- /libraries/MPU6050/JJ_MPU6050_DMP_6Axis.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Updated by Jose Julio 09/17/2013 7 | // This code is based on MPU6050_6Axis_MotionApps20.h file 8 | // - Reduced FIFO packet size (18 bytes 9 | // - Optimized DMP initialization (less memory) 10 | 11 | /* ============================================ 12 | I2Cdev device library code is placed under the MIT license 13 | Copyright (c) 2012 Jeff Rowberg 14 | 15 | Permission is hereby granted, free of charge, to any person obtaining a copy 16 | of this software and associated documentation files (the "Software"), to deal 17 | in the Software without restriction, including without limitation the rights 18 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 19 | copies of the Software, and to permit persons to whom the Software is 20 | furnished to do so, subject to the following conditions: 21 | 22 | The above copyright notice and this permission notice shall be included in 23 | all copies or substantial portions of the Software. 24 | 25 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 26 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 27 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 28 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 29 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 30 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 31 | THE SOFTWARE. 32 | =============================================== 33 | */ 34 | 35 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 36 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 37 | 38 | #include "I2Cdev.h" 39 | #include "helper_3dmath.h" 40 | 41 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 42 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 43 | 44 | #include "MPU6050.h" 45 | 46 | // Tom Carpenter's conditional PROGMEM code 47 | // http://forum.arduino.cc/index.php?topic=129407.0 48 | #ifndef __arm__ 49 | #include 50 | #else 51 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 52 | #ifndef __PGMSPACE_H_ 53 | #define __PGMSPACE_H_ 1 54 | #include 55 | 56 | #define PROGMEM 57 | #define PGM_P const char * 58 | #define PSTR(str) (str) 59 | #define F(x) x 60 | 61 | typedef void prog_void; 62 | typedef char prog_char; 63 | typedef unsigned char prog_uchar; 64 | typedef int8_t prog_int8_t; 65 | typedef uint8_t prog_uint8_t; 66 | typedef int16_t prog_int16_t; 67 | typedef uint16_t prog_uint16_t; 68 | typedef int32_t prog_int32_t; 69 | typedef uint32_t prog_uint32_t; 70 | 71 | #define strcpy_P(dest, src) strcpy((dest), (src)) 72 | #define strcat_P(dest, src) strcat((dest), (src)) 73 | #define strcmp_P(a, b) strcmp((a), (b)) 74 | 75 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 76 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 77 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 78 | #define pgm_read_float(addr) (*(const float *)(addr)) 79 | 80 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 81 | #define pgm_read_word_near(addr) pgm_read_word(addr) 82 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 83 | #define pgm_read_float_near(addr) pgm_read_float(addr) 84 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 85 | #define pgm_read_word_far(addr) pgm_read_word(addr) 86 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 87 | #define pgm_read_float_far(addr) pgm_read_float(addr) 88 | #endif 89 | #endif 90 | 91 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 92 | * unavailable, unless you happen to be amazing as decompiling binary by 93 | * hand (in which case, please contact me, and I'm totally serious). 94 | * 95 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 96 | * DMP reverse-engineering he did to help make this bit of wizardry 97 | * possible. 98 | */ 99 | 100 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 101 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 102 | // after moving string constants to flash memory storage using the F() 103 | // compiler macro (Arduino IDE 1.0+ required). 104 | 105 | //#define DEBUG 106 | #ifdef DEBUG 107 | #define DEBUG_PRINT(x) Serial.print(x) 108 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 109 | #define DEBUG_PRINTLN(x) Serial.println(x) 110 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 111 | #else 112 | #define DEBUG_PRINT(x) 113 | #define DEBUG_PRINTF(x, y) 114 | #define DEBUG_PRINTLN(x) 115 | #define DEBUG_PRINTLNF(x, y) 116 | #endif 117 | 118 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 119 | #define MPU6050_DMP_CONFIG_SIZE 111 // 174 //192 // dmpConfig[] 120 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 121 | 122 | /* ================================================================================================ * 123 | | 18-byte FIFO packet structure: | 124 | | | 125 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][FOOTER] | 126 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 | 127 | | | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | //0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | //0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | //0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | //0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | //0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | //0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | //0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | //0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | //0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | //0x04, 0x02, 0x03, 0x98, 0x98, 0x98, // CFG_MOTION_BIAS inv_turn_off bias correction 296 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 297 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 298 | // SPECIAL 0x01 = enable interrupts 299 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 300 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 301 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 302 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 303 | //0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 304 | //0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 305 | //0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 306 | 0x02, 0x16, 0x02, 0x00, 0x00 // D_0_22 inv_set_fifo_rate // Original 0x01 307 | 308 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 309 | 310 | // It is important to make sure the host processor can keep up with reading and processing 311 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 312 | }; 313 | 314 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 315 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 316 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 317 | 0x01, 0x6A, 0x02, 0x06, 0x00, 318 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 319 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 320 | 0x01, 0x62, 0x02, 0x00, 0x00, 321 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 322 | }; 323 | 324 | uint8_t MPU6050::dmpInitialize() { 325 | // reset device 326 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 327 | reset(); 328 | delay(30); // wait after reset 329 | 330 | // enable sleep mode and wake cycle 331 | /*Serial.println(F("Enabling sleep mode...")); 332 | setSleepEnabled(true); 333 | Serial.println(F("Enabling wake cycle...")); 334 | setWakeCycleEnabled(true);*/ 335 | 336 | // disable sleep mode 337 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 338 | setSleepEnabled(false); 339 | 340 | // get MPU hardware revision 341 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 342 | setMemoryBank(0x10, true, true); 343 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 344 | setMemoryStartAddress(0x06); 345 | DEBUG_PRINTLN(F("Checking hardware revision...")); 346 | uint8_t hwRevision = readMemoryByte(); 347 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 348 | DEBUG_PRINTLNF(hwRevision, HEX); 349 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 350 | setMemoryBank(0, false, false); 351 | 352 | // check OTP bank valid 353 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 354 | uint8_t otpValid = getOTPBankValid(); 355 | DEBUG_PRINT(F("OTP bank is ")); 356 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 357 | 358 | // get X/Y/Z gyro offsets 359 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 360 | int8_t xgOffsetTC = getXGyroOffsetTC(); 361 | int8_t ygOffsetTC = getYGyroOffsetTC(); 362 | int8_t zgOffsetTC = getZGyroOffsetTC(); 363 | DEBUG_PRINT(F("X gyro offset = ")); 364 | DEBUG_PRINTLN(xgOffset); 365 | DEBUG_PRINT(F("Y gyro offset = ")); 366 | DEBUG_PRINTLN(ygOffset); 367 | DEBUG_PRINT(F("Z gyro offset = ")); 368 | DEBUG_PRINTLN(zgOffset); 369 | 370 | // setup weird slave stuff (?) 371 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 372 | setSlaveAddress(0, 0x7F); 373 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 374 | setI2CMasterModeEnabled(false); 375 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 376 | setSlaveAddress(0, 0x68); 377 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 378 | resetI2CMaster(); 379 | delay(20); 380 | 381 | // load DMP code into memory banks 382 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 383 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 384 | DEBUG_PRINTLN(F(" bytes)")); 385 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 386 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 387 | 388 | // write DMP configuration 389 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 390 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 391 | DEBUG_PRINTLN(F(" bytes in config def)")); 392 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 393 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 394 | 395 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 396 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 397 | 398 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 399 | setIntEnabled(0x12); 400 | 401 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 402 | setRate(4); // 1khz / (1 + 4) = 200 Hz 403 | 404 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 405 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 406 | 407 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 408 | setDLPFMode(MPU6050_DLPF_BW_42); 409 | 410 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 411 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 412 | 413 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 414 | setDMPConfig1(0x03); 415 | setDMPConfig2(0x00); 416 | 417 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 418 | setOTPBankValid(false); 419 | 420 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 421 | setXGyroOffsetTC(xgOffsetTC); 422 | setYGyroOffsetTC(ygOffsetTC); 423 | setZGyroOffsetTC(zgOffsetTC); 424 | 425 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 426 | //setXGyroOffset(0); 427 | //setYGyroOffset(0); 428 | //setZGyroOffset(0); 429 | 430 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 431 | uint8_t dmpUpdate[16], j; 432 | uint16_t pos = 0; 433 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 434 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 435 | 436 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 437 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 438 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 439 | 440 | DEBUG_PRINTLN(F("Resetting FIFO...")); 441 | resetFIFO(); 442 | 443 | DEBUG_PRINTLN(F("Reading FIFO count...")); 444 | uint16_t fifoCount = getFIFOCount(); 445 | uint8_t fifoBuffer[128]; 446 | 447 | DEBUG_PRINT(F("Current FIFO count=")); 448 | DEBUG_PRINTLN(fifoCount); 449 | getFIFOBytes(fifoBuffer, fifoCount); 450 | 451 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 452 | setMotionDetectionThreshold(2); 453 | 454 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 455 | setZeroMotionDetectionThreshold(156); 456 | 457 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 458 | setMotionDetectionDuration(80); 459 | 460 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 461 | setZeroMotionDetectionDuration(0); 462 | 463 | DEBUG_PRINTLN(F("Resetting FIFO...")); 464 | resetFIFO(); 465 | 466 | DEBUG_PRINTLN(F("Enabling FIFO...")); 467 | setFIFOEnabled(true); 468 | 469 | DEBUG_PRINTLN(F("Enabling DMP...")); 470 | setDMPEnabled(true); 471 | 472 | DEBUG_PRINTLN(F("Resetting DMP...")); 473 | resetDMP(); 474 | 475 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 476 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 477 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 478 | 479 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 480 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 481 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 482 | 483 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 484 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 485 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 486 | 487 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 488 | while ((fifoCount = getFIFOCount()) < 3); 489 | 490 | DEBUG_PRINT(F("Current FIFO count=")); 491 | DEBUG_PRINTLN(fifoCount); 492 | DEBUG_PRINTLN(F("Reading FIFO data...")); 493 | getFIFOBytes(fifoBuffer, fifoCount); 494 | 495 | DEBUG_PRINTLN(F("Reading interrupt status...")); 496 | uint8_t mpuIntStatus = getIntStatus(); 497 | 498 | DEBUG_PRINT(F("Current interrupt status=")); 499 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 500 | 501 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 502 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 503 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 504 | 505 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 506 | while ((fifoCount = getFIFOCount()) < 3); 507 | 508 | DEBUG_PRINT(F("Current FIFO count=")); 509 | DEBUG_PRINTLN(fifoCount); 510 | 511 | DEBUG_PRINTLN(F("Reading FIFO data...")); 512 | getFIFOBytes(fifoBuffer, fifoCount); 513 | 514 | DEBUG_PRINTLN(F("Reading interrupt status...")); 515 | mpuIntStatus = getIntStatus(); 516 | 517 | DEBUG_PRINT(F("Current interrupt status=")); 518 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 519 | 520 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 521 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 522 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 523 | 524 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 525 | 526 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 527 | setDMPEnabled(false); 528 | 529 | DEBUG_PRINTLN(F("Setting up internal 18-byte (default) DMP packet buffer...")); 530 | dmpPacketSize = 18; // original 42 bytes; 531 | 532 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 533 | resetFIFO(); 534 | getIntStatus(); 535 | } else { 536 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 537 | return 2; // configuration block loading failed 538 | } 539 | } else { 540 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 541 | return 1; // main binary block loading failed 542 | } 543 | return 0; // success 544 | } 545 | 546 | bool MPU6050::dmpPacketAvailable() { 547 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 548 | } 549 | 550 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 551 | // uint8_t MPU6050::dmpGetFIFORate(); 552 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 553 | // uint8_t MPU6050::dmpGetSampleFrequency(); 554 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 555 | 556 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 557 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 558 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 559 | 560 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 561 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 562 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 563 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 564 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 572 | 573 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 574 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 575 | if (packet == 0) packet = dmpPacketBuffer; 576 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 577 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 578 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 579 | return 0; 580 | } 581 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 582 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 583 | if (packet == 0) packet = dmpPacketBuffer; 584 | data[0] = (packet[28] << 8) + packet[29]; 585 | data[1] = (packet[32] << 8) + packet[33]; 586 | data[2] = (packet[36] << 8) + packet[37]; 587 | return 0; 588 | } 589 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 590 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 591 | if (packet == 0) packet = dmpPacketBuffer; 592 | v -> x = (packet[28] << 8) + packet[29]; 593 | v -> y = (packet[32] << 8) + packet[33]; 594 | v -> z = (packet[36] << 8) + packet[37]; 595 | return 0; 596 | } 597 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 598 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 599 | if (packet == 0) packet = dmpPacketBuffer; 600 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 601 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 602 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 603 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 604 | return 0; 605 | } 606 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 607 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 608 | if (packet == 0) packet = dmpPacketBuffer; 609 | data[0] = ((packet[0] << 8) + packet[1]); 610 | data[1] = ((packet[4] << 8) + packet[5]); 611 | data[2] = ((packet[8] << 8) + packet[9]); 612 | data[3] = ((packet[12] << 8) + packet[13]); 613 | return 0; 614 | } 615 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 616 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 617 | int16_t qI[4]; 618 | uint8_t status = dmpGetQuaternion(qI, packet); 619 | if (status == 0) { 620 | q -> w = (float)qI[0] / 16384.0f; 621 | q -> x = (float)qI[1] / 16384.0f; 622 | q -> y = (float)qI[2] / 16384.0f; 623 | q -> z = (float)qI[3] / 16384.0f; 624 | return 0; 625 | } 626 | return status; // int16 return value, indicates error if this line is reached 627 | } 628 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 629 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 630 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 631 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 632 | if (packet == 0) packet = dmpPacketBuffer; 633 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 634 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 635 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 636 | return 0; 637 | } 638 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 639 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 640 | if (packet == 0) packet = dmpPacketBuffer; 641 | data[0] = (packet[16] << 8) + packet[17]; 642 | data[1] = (packet[20] << 8) + packet[21]; 643 | data[2] = (packet[24] << 8) + packet[25]; 644 | return 0; 645 | } 646 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 647 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 648 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 649 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 650 | v -> x = vRaw -> x - gravity -> x*8192; 651 | v -> y = vRaw -> y - gravity -> y*8192; 652 | v -> z = vRaw -> z - gravity -> z*8192; 653 | return 0; 654 | } 655 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 656 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 657 | // rotate measured 3D acceleration vector into original state 658 | // frame of reference based on orientation quaternion 659 | memcpy(v, vReal, sizeof(VectorInt16)); 660 | v -> rotate(q); 661 | return 0; 662 | } 663 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 664 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 665 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 666 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 667 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 668 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 669 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 670 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 671 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 672 | return 0; 673 | } 674 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 675 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 676 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 677 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 678 | 679 | 680 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 681 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi 682 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 683 | data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi 684 | return 0; 685 | } 686 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 687 | // yaw: (about Z axis) 688 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 689 | // pitch: (nose up/down, about Y axis) 690 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 691 | // roll: (tilt left/right, about X axis) 692 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 693 | return 0; 694 | } 695 | 696 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 697 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 698 | 699 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 700 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 701 | if (dmpData[k] < 0x10) Serial.print("0"); 702 | Serial.print(dmpData[k], HEX); 703 | Serial.print(" "); 704 | } 705 | Serial.print("\n");*/ 706 | //Serial.println((uint16_t)dmpPacketBuffer); 707 | return 0; 708 | } 709 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 710 | uint8_t status; 711 | uint8_t buf[dmpPacketSize]; 712 | for (uint8_t i = 0; i < numPackets; i++) { 713 | // read packet from FIFO 714 | getFIFOBytes(buf, dmpPacketSize); 715 | 716 | // process packet 717 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 718 | 719 | // increment external process count variable, if supplied 720 | if (processed != 0) *processed++; 721 | } 722 | return 0; 723 | } 724 | 725 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 726 | 727 | // uint8_t MPU6050::dmpInitFIFOParam(); 728 | // uint8_t MPU6050::dmpCloseFIFO(); 729 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 730 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 731 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 732 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 733 | // void MPU6050::dmpOverrideQuaternion(long *q); 734 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 735 | return dmpPacketSize; 736 | } 737 | 738 | 739 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 740 | -------------------------------------------------------------------------------- /libraries/MPU6050/MPU6050_6Axis_MotionApps20.h: -------------------------------------------------------------------------------- 1 | // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation 2 | // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) 3 | // 5/20/2013 by Jeff Rowberg 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 5 | // 6 | // Changelog: 7 | // ... - ongoing debug release 8 | 9 | /* ============================================ 10 | I2Cdev device library code is placed under the MIT license 11 | Copyright (c) 2012 Jeff Rowberg 12 | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy 14 | of this software and associated documentation files (the "Software"), to deal 15 | in the Software without restriction, including without limitation the rights 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 17 | copies of the Software, and to permit persons to whom the Software is 18 | furnished to do so, subject to the following conditions: 19 | 20 | The above copyright notice and this permission notice shall be included in 21 | all copies or substantial portions of the Software. 22 | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 29 | THE SOFTWARE. 30 | =============================================== 31 | */ 32 | 33 | #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ 34 | #define _MPU6050_6AXIS_MOTIONAPPS20_H_ 35 | 36 | #include "I2Cdev.h" 37 | #include "helper_3dmath.h" 38 | 39 | // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board 40 | #define MPU6050_INCLUDE_DMP_MOTIONAPPS20 41 | 42 | #include "MPU6050.h" 43 | 44 | // Tom Carpenter's conditional PROGMEM code 45 | // http://forum.arduino.cc/index.php?topic=129407.0 46 | #ifndef __arm__ 47 | #include 48 | #else 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen 50 | #ifndef __PGMSPACE_H_ 51 | #define __PGMSPACE_H_ 1 52 | #include 53 | 54 | #define PROGMEM 55 | #define PGM_P const char * 56 | #define PSTR(str) (str) 57 | #define F(x) x 58 | 59 | typedef void prog_void; 60 | typedef char prog_char; 61 | typedef unsigned char prog_uchar; 62 | typedef int8_t prog_int8_t; 63 | typedef uint8_t prog_uint8_t; 64 | typedef int16_t prog_int16_t; 65 | typedef uint16_t prog_uint16_t; 66 | typedef int32_t prog_int32_t; 67 | typedef uint32_t prog_uint32_t; 68 | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) 70 | #define strcat_P(dest, src) strcat((dest), (src)) 71 | #define strcmp_P(a, b) strcmp((a), (b)) 72 | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) 76 | #define pgm_read_float(addr) (*(const float *)(addr)) 77 | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) 86 | #endif 87 | #endif 88 | 89 | /* Source is from the InvenSense MotionApps v2 demo code. Original source is 90 | * unavailable, unless you happen to be amazing as decompiling binary by 91 | * hand (in which case, please contact me, and I'm totally serious). 92 | * 93 | * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the 94 | * DMP reverse-engineering he did to help make this bit of wizardry 95 | * possible. 96 | */ 97 | 98 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 99 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 100 | // after moving string constants to flash memory storage using the F() 101 | // compiler macro (Arduino IDE 1.0+ required). 102 | 103 | //#define DEBUG 104 | #ifdef DEBUG 105 | #define DEBUG_PRINT(x) Serial.print(x) 106 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) 107 | #define DEBUG_PRINTLN(x) Serial.println(x) 108 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) 109 | #else 110 | #define DEBUG_PRINT(x) 111 | #define DEBUG_PRINTF(x, y) 112 | #define DEBUG_PRINTLN(x) 113 | #define DEBUG_PRINTLNF(x, y) 114 | #endif 115 | 116 | #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 117 | #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 118 | #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 119 | 120 | /* ================================================================================================ * 121 | | Default MotionApps v2.0 42-byte FIFO packet structure: | 122 | | | 123 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 124 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 125 | | | 126 | | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 127 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 128 | * ================================================================================================ */ 129 | 130 | // this block of memory gets written to the MPU on start-up, and it seems 131 | // to be volatile memory, so it has to be done each time (it only takes ~1 132 | // second though) 133 | const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 134 | // bank 0, 256 bytes 135 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 136 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 137 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 138 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 139 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 140 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 141 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 142 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 143 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 144 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 145 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 146 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 147 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 148 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 149 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 150 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 151 | 152 | // bank 1, 256 bytes 153 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 154 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 155 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 156 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 157 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 158 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 159 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 160 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 161 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 163 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 164 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 166 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 167 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 168 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 169 | 170 | // bank 2, 256 bytes 171 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 172 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 174 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 175 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 176 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 177 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 178 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 179 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 180 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 181 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 182 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 183 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 184 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 185 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 186 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 187 | 188 | // bank 3, 256 bytes 189 | 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 190 | 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 191 | 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 192 | 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 193 | 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 194 | 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 195 | 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 196 | 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 197 | 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 198 | 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 199 | 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 200 | 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 201 | 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 202 | 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 203 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 204 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 205 | 206 | // bank 4, 256 bytes 207 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 208 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 209 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 210 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 211 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 212 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 213 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 214 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 215 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 216 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 217 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 218 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 219 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 220 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 221 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 222 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 223 | 224 | // bank 5, 256 bytes 225 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 226 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 227 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 228 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 229 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 230 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 231 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 232 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 233 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 234 | 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 235 | 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 236 | 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 237 | 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 238 | 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 239 | 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 240 | 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 241 | 242 | // bank 6, 256 bytes 243 | 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 244 | 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 245 | 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 246 | 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 247 | 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 248 | 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 249 | 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 250 | 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 251 | 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 252 | 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 253 | 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 254 | 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 255 | 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 256 | 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 257 | 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 258 | 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 259 | 260 | // bank 7, 138 bytes (remainder) 261 | 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 262 | 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 263 | 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 264 | 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 265 | 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 266 | 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 267 | 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 268 | 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 269 | 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 270 | }; 271 | 272 | // thanks to Noah Zerkin for piecing this stuff together! 273 | const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 274 | // BANK OFFSET LENGTH [DATA] 275 | 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 276 | 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 277 | 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 278 | 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 279 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 280 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 281 | 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 282 | 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 283 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 284 | 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 285 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 286 | 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 287 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 288 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 289 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 290 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 291 | 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 292 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 293 | 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 294 | 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 295 | 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 296 | 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 297 | // SPECIAL 0x01 = enable interrupts 298 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 299 | 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 300 | 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 301 | 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 302 | 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 303 | 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 304 | 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 305 | 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate 306 | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 310 | 311 | // It is important to make sure the host processor can keep up with reading and processing 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 313 | }; 314 | 315 | const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 316 | 0x01, 0xB2, 0x02, 0xFF, 0xFF, 317 | 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 318 | 0x01, 0x6A, 0x02, 0x06, 0x00, 319 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 320 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 321 | 0x01, 0x62, 0x02, 0x00, 0x00, 322 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 323 | }; 324 | 325 | uint8_t MPU6050::dmpInitialize() { 326 | // reset device 327 | DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 328 | reset(); 329 | delay(30); // wait after reset 330 | 331 | // enable sleep mode and wake cycle 332 | /*Serial.println(F("Enabling sleep mode...")); 333 | setSleepEnabled(true); 334 | Serial.println(F("Enabling wake cycle...")); 335 | setWakeCycleEnabled(true);*/ 336 | 337 | // disable sleep mode 338 | DEBUG_PRINTLN(F("Disabling sleep mode...")); 339 | setSleepEnabled(false); 340 | 341 | // get MPU hardware revision 342 | DEBUG_PRINTLN(F("Selecting user bank 16...")); 343 | setMemoryBank(0x10, true, true); 344 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); 345 | setMemoryStartAddress(0x06); 346 | DEBUG_PRINTLN(F("Checking hardware revision...")); 347 | uint8_t hwRevision = readMemoryByte(); 348 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); 349 | DEBUG_PRINTLNF(hwRevision, HEX); 350 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 351 | setMemoryBank(0, false, false); 352 | 353 | // check OTP bank valid 354 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 355 | uint8_t otpValid = getOTPBankValid(); 356 | DEBUG_PRINT(F("OTP bank is ")); 357 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 358 | 359 | // get X/Y/Z gyro offsets 360 | DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 361 | int8_t xgOffsetTC = getXGyroOffsetTC(); 362 | int8_t ygOffsetTC = getYGyroOffsetTC(); 363 | int8_t zgOffsetTC = getZGyroOffsetTC(); 364 | DEBUG_PRINT(F("X gyro offset = ")); 365 | DEBUG_PRINTLN(xgOffset); 366 | DEBUG_PRINT(F("Y gyro offset = ")); 367 | DEBUG_PRINTLN(ygOffset); 368 | DEBUG_PRINT(F("Z gyro offset = ")); 369 | DEBUG_PRINTLN(zgOffset); 370 | 371 | // setup weird slave stuff (?) 372 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 373 | setSlaveAddress(0, 0x7F); 374 | DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 375 | setI2CMasterModeEnabled(false); 376 | DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 377 | setSlaveAddress(0, 0x68); 378 | DEBUG_PRINTLN(F("Resetting I2C Master control...")); 379 | resetI2CMaster(); 380 | delay(20); 381 | 382 | // load DMP code into memory banks 383 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 384 | DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); 385 | DEBUG_PRINTLN(F(" bytes)")); 386 | if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { 387 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 388 | 389 | // write DMP configuration 390 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 391 | DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 392 | DEBUG_PRINTLN(F(" bytes in config def)")); 393 | if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 394 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 395 | 396 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 397 | setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 398 | 399 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 400 | setIntEnabled(0x12); 401 | 402 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 403 | setRate(4); // 1khz / (1 + 4) = 200 Hz 404 | 405 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 406 | setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 407 | 408 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 409 | setDLPFMode(MPU6050_DLPF_BW_42); 410 | 411 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 412 | setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 413 | 414 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 415 | setDMPConfig1(0x03); 416 | setDMPConfig2(0x00); 417 | 418 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 419 | setOTPBankValid(false); 420 | 421 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 422 | setXGyroOffsetTC(xgOffsetTC); 423 | setYGyroOffsetTC(ygOffsetTC); 424 | setZGyroOffsetTC(zgOffsetTC); 425 | 426 | //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 427 | //setXGyroOffset(0); 428 | //setYGyroOffset(0); 429 | //setZGyroOffset(0); 430 | 431 | DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 432 | uint8_t dmpUpdate[16], j; 433 | uint16_t pos = 0; 434 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 435 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 436 | 437 | DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 438 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 439 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 440 | 441 | DEBUG_PRINTLN(F("Resetting FIFO...")); 442 | resetFIFO(); 443 | 444 | DEBUG_PRINTLN(F("Reading FIFO count...")); 445 | uint16_t fifoCount = getFIFOCount(); 446 | uint8_t fifoBuffer[128]; 447 | 448 | DEBUG_PRINT(F("Current FIFO count=")); 449 | DEBUG_PRINTLN(fifoCount); 450 | getFIFOBytes(fifoBuffer, fifoCount); 451 | 452 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 453 | setMotionDetectionThreshold(2); 454 | 455 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 456 | setZeroMotionDetectionThreshold(156); 457 | 458 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 459 | setMotionDetectionDuration(80); 460 | 461 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 462 | setZeroMotionDetectionDuration(0); 463 | 464 | DEBUG_PRINTLN(F("Resetting FIFO...")); 465 | resetFIFO(); 466 | 467 | DEBUG_PRINTLN(F("Enabling FIFO...")); 468 | setFIFOEnabled(true); 469 | 470 | DEBUG_PRINTLN(F("Enabling DMP...")); 471 | setDMPEnabled(true); 472 | 473 | DEBUG_PRINTLN(F("Resetting DMP...")); 474 | resetDMP(); 475 | 476 | DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 477 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 478 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 479 | 480 | DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 481 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 482 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 483 | 484 | DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 485 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 486 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 487 | 488 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 489 | while ((fifoCount = getFIFOCount()) < 3); 490 | 491 | DEBUG_PRINT(F("Current FIFO count=")); 492 | DEBUG_PRINTLN(fifoCount); 493 | DEBUG_PRINTLN(F("Reading FIFO data...")); 494 | getFIFOBytes(fifoBuffer, fifoCount); 495 | 496 | DEBUG_PRINTLN(F("Reading interrupt status...")); 497 | uint8_t mpuIntStatus = getIntStatus(); 498 | 499 | DEBUG_PRINT(F("Current interrupt status=")); 500 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 501 | 502 | DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 503 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 504 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 505 | 506 | DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 507 | while ((fifoCount = getFIFOCount()) < 3); 508 | 509 | DEBUG_PRINT(F("Current FIFO count=")); 510 | DEBUG_PRINTLN(fifoCount); 511 | 512 | DEBUG_PRINTLN(F("Reading FIFO data...")); 513 | getFIFOBytes(fifoBuffer, fifoCount); 514 | 515 | DEBUG_PRINTLN(F("Reading interrupt status...")); 516 | mpuIntStatus = getIntStatus(); 517 | 518 | DEBUG_PRINT(F("Current interrupt status=")); 519 | DEBUG_PRINTLNF(mpuIntStatus, HEX); 520 | 521 | DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 522 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 523 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 524 | 525 | DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 526 | 527 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 528 | setDMPEnabled(false); 529 | 530 | DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); 531 | dmpPacketSize = 42; 532 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 533 | return 3; // TODO: proper error code for no memory 534 | }*/ 535 | 536 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 537 | resetFIFO(); 538 | getIntStatus(); 539 | } else { 540 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 541 | return 2; // configuration block loading failed 542 | } 543 | } else { 544 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 545 | return 1; // main binary block loading failed 546 | } 547 | return 0; // success 548 | } 549 | 550 | bool MPU6050::dmpPacketAvailable() { 551 | return getFIFOCount() >= dmpGetFIFOPacketSize(); 552 | } 553 | 554 | // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 555 | // uint8_t MPU6050::dmpGetFIFORate(); 556 | // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 557 | // uint8_t MPU6050::dmpGetSampleFrequency(); 558 | // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 559 | 560 | //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 561 | //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 562 | //uint8_t MPU6050::dmpRunFIFORateProcesses(); 563 | 564 | // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 565 | // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 566 | // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 567 | // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 568 | // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 569 | // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 570 | // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 571 | // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 572 | // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 573 | // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 574 | // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 575 | // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 576 | 577 | uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 578 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 579 | if (packet == 0) packet = dmpPacketBuffer; 580 | data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 581 | data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 582 | data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 583 | return 0; 584 | } 585 | uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 586 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 587 | if (packet == 0) packet = dmpPacketBuffer; 588 | data[0] = (packet[28] << 8) + packet[29]; 589 | data[1] = (packet[32] << 8) + packet[33]; 590 | data[2] = (packet[36] << 8) + packet[37]; 591 | return 0; 592 | } 593 | uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 594 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 595 | if (packet == 0) packet = dmpPacketBuffer; 596 | v -> x = (packet[28] << 8) + packet[29]; 597 | v -> y = (packet[32] << 8) + packet[33]; 598 | v -> z = (packet[36] << 8) + packet[37]; 599 | return 0; 600 | } 601 | uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 602 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 603 | if (packet == 0) packet = dmpPacketBuffer; 604 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 605 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 606 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 607 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 608 | return 0; 609 | } 610 | uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 611 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 612 | if (packet == 0) packet = dmpPacketBuffer; 613 | data[0] = ((packet[0] << 8) + packet[1]); 614 | data[1] = ((packet[4] << 8) + packet[5]); 615 | data[2] = ((packet[8] << 8) + packet[9]); 616 | data[3] = ((packet[12] << 8) + packet[13]); 617 | return 0; 618 | } 619 | uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 620 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 621 | int16_t qI[4]; 622 | uint8_t status = dmpGetQuaternion(qI, packet); 623 | if (status == 0) { 624 | q -> w = (float)qI[0] / 16384.0f; 625 | q -> x = (float)qI[1] / 16384.0f; 626 | q -> y = (float)qI[2] / 16384.0f; 627 | q -> z = (float)qI[3] / 16384.0f; 628 | return 0; 629 | } 630 | return status; // int16 return value, indicates error if this line is reached 631 | } 632 | // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 633 | // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 634 | uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 635 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 636 | if (packet == 0) packet = dmpPacketBuffer; 637 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 638 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 639 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 640 | return 0; 641 | } 642 | uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 643 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 644 | if (packet == 0) packet = dmpPacketBuffer; 645 | data[0] = (packet[16] << 8) + packet[17]; 646 | data[1] = (packet[20] << 8) + packet[21]; 647 | data[2] = (packet[24] << 8) + packet[25]; 648 | return 0; 649 | } 650 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 651 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 652 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 653 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 654 | v -> x = vRaw -> x - gravity -> x*8192; 655 | v -> y = vRaw -> y - gravity -> y*8192; 656 | v -> z = vRaw -> z - gravity -> z*8192; 657 | return 0; 658 | } 659 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 660 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 661 | // rotate measured 3D acceleration vector into original state 662 | // frame of reference based on orientation quaternion 663 | memcpy(v, vReal, sizeof(VectorInt16)); 664 | v -> rotate(q); 665 | return 0; 666 | } 667 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 668 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 669 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 670 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 671 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 672 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 673 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 674 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 675 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 676 | return 0; 677 | } 678 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 679 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 680 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 681 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 682 | 683 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 684 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); // psi 685 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 686 | data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1); // phi 687 | return 0; 688 | } 689 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 690 | // yaw: (about Z axis) 691 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 692 | // pitch: (nose up/down, about Y axis) 693 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 694 | // roll: (tilt left/right, about X axis) 695 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 696 | return 0; 697 | } 698 | 699 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 700 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 701 | 702 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 703 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 704 | if (dmpData[k] < 0x10) Serial.print("0"); 705 | Serial.print(dmpData[k], HEX); 706 | Serial.print(" "); 707 | } 708 | Serial.print("\n");*/ 709 | //Serial.println((uint16_t)dmpPacketBuffer); 710 | return 0; 711 | } 712 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 713 | uint8_t status; 714 | uint8_t buf[dmpPacketSize]; 715 | for (uint8_t i = 0; i < numPackets; i++) { 716 | // read packet from FIFO 717 | getFIFOBytes(buf, dmpPacketSize); 718 | 719 | // process packet 720 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 721 | 722 | // increment external process count variable, if supplied 723 | if (processed != 0) *processed++; 724 | } 725 | return 0; 726 | } 727 | 728 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 729 | 730 | // uint8_t MPU6050::dmpInitFIFOParam(); 731 | // uint8_t MPU6050::dmpCloseFIFO(); 732 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 733 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 734 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 735 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 736 | // void MPU6050::dmpOverrideQuaternion(long *q); 737 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 738 | return dmpPacketSize; 739 | } 740 | 741 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 742 | -------------------------------------------------------------------------------- /libraries/MPU6050/helper_3dmath.h: -------------------------------------------------------------------------------- 1 | // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper 2 | // 6/5/2012 by Jeff Rowberg 3 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib 4 | // 5 | // Changelog: 6 | // 2012-06-05 - add 3D math helper file to DMP6 example sketch 7 | 8 | /* ============================================ 9 | I2Cdev device library code is placed under the MIT license 10 | Copyright (c) 2012 Jeff Rowberg 11 | 12 | Permission is hereby granted, free of charge, to any person obtaining a copy 13 | of this software and associated documentation files (the "Software"), to deal 14 | in the Software without restriction, including without limitation the rights 15 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 16 | copies of the Software, and to permit persons to whom the Software is 17 | furnished to do so, subject to the following conditions: 18 | 19 | The above copyright notice and this permission notice shall be included in 20 | all copies or substantial portions of the Software. 21 | 22 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 23 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 24 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 25 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 26 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 27 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 28 | THE SOFTWARE. 29 | =============================================== 30 | */ 31 | 32 | #ifndef _HELPER_3DMATH_H_ 33 | #define _HELPER_3DMATH_H_ 34 | 35 | class Quaternion { 36 | public: 37 | float w; 38 | float x; 39 | float y; 40 | float z; 41 | 42 | Quaternion() { 43 | w = 1.0f; 44 | x = 0.0f; 45 | y = 0.0f; 46 | z = 0.0f; 47 | } 48 | 49 | Quaternion(float nw, float nx, float ny, float nz) { 50 | w = nw; 51 | x = nx; 52 | y = ny; 53 | z = nz; 54 | } 55 | 56 | Quaternion getProduct(Quaternion q) { 57 | // Quaternion multiplication is defined by: 58 | // (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2) 59 | // (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2) 60 | // (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2) 61 | // (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2 62 | return Quaternion( 63 | w*q.w - x*q.x - y*q.y - z*q.z, // new w 64 | w*q.x + x*q.w + y*q.z - z*q.y, // new x 65 | w*q.y - x*q.z + y*q.w + z*q.x, // new y 66 | w*q.z + x*q.y - y*q.x + z*q.w); // new z 67 | } 68 | 69 | Quaternion getConjugate() { 70 | return Quaternion(w, -x, -y, -z); 71 | } 72 | 73 | float getMagnitude() { 74 | return sqrt(w*w + x*x + y*y + z*z); 75 | } 76 | 77 | void normalize() { 78 | float m = getMagnitude(); 79 | w /= m; 80 | x /= m; 81 | y /= m; 82 | z /= m; 83 | } 84 | 85 | Quaternion getNormalized() { 86 | Quaternion r(w, x, y, z); 87 | r.normalize(); 88 | return r; 89 | } 90 | }; 91 | 92 | class VectorInt16 { 93 | public: 94 | int16_t x; 95 | int16_t y; 96 | int16_t z; 97 | 98 | VectorInt16() { 99 | x = 0; 100 | y = 0; 101 | z = 0; 102 | } 103 | 104 | VectorInt16(int16_t nx, int16_t ny, int16_t nz) { 105 | x = nx; 106 | y = ny; 107 | z = nz; 108 | } 109 | 110 | float getMagnitude() { 111 | return sqrt(x*x + y*y + z*z); 112 | } 113 | 114 | void normalize() { 115 | float m = getMagnitude(); 116 | x /= m; 117 | y /= m; 118 | z /= m; 119 | } 120 | 121 | VectorInt16 getNormalized() { 122 | VectorInt16 r(x, y, z); 123 | r.normalize(); 124 | return r; 125 | } 126 | 127 | void rotate(Quaternion *q) { 128 | // http://www.cprogramming.com/tutorial/3d/quaternions.html 129 | // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm 130 | // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation 131 | // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1 132 | 133 | // P_out = q * P_in * conj(q) 134 | // - P_out is the output vector 135 | // - q is the orientation quaternion 136 | // - P_in is the input vector (a*aReal) 137 | // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z]) 138 | Quaternion p(0, x, y, z); 139 | 140 | // quaternion multiplication: q * p, stored back in p 141 | p = q -> getProduct(p); 142 | 143 | // quaternion multiplication: p * conj(q), stored back in p 144 | p = p.getProduct(q -> getConjugate()); 145 | 146 | // p quaternion is now [0, x', y', z'] 147 | x = p.x; 148 | y = p.y; 149 | z = p.z; 150 | } 151 | 152 | VectorInt16 getRotated(Quaternion *q) { 153 | VectorInt16 r(x, y, z); 154 | r.rotate(q); 155 | return r; 156 | } 157 | }; 158 | 159 | class VectorFloat { 160 | public: 161 | float x; 162 | float y; 163 | float z; 164 | 165 | VectorFloat() { 166 | x = 0; 167 | y = 0; 168 | z = 0; 169 | } 170 | 171 | VectorFloat(float nx, float ny, float nz) { 172 | x = nx; 173 | y = ny; 174 | z = nz; 175 | } 176 | 177 | float getMagnitude() { 178 | return sqrt(x*x + y*y + z*z); 179 | } 180 | 181 | void normalize() { 182 | float m = getMagnitude(); 183 | x /= m; 184 | y /= m; 185 | z /= m; 186 | } 187 | 188 | VectorFloat getNormalized() { 189 | VectorFloat r(x, y, z); 190 | r.normalize(); 191 | return r; 192 | } 193 | 194 | void rotate(Quaternion *q) { 195 | Quaternion p(0, x, y, z); 196 | 197 | // quaternion multiplication: q * p, stored back in p 198 | p = q -> getProduct(p); 199 | 200 | // quaternion multiplication: p * conj(q), stored back in p 201 | p = p.getProduct(q -> getConjugate()); 202 | 203 | // p quaternion is now [0, x', y', z'] 204 | x = p.x; 205 | y = p.y; 206 | z = p.z; 207 | } 208 | 209 | VectorFloat getRotated(Quaternion *q) { 210 | VectorFloat r(x, y, z); 211 | r.rotate(q); 212 | return r; 213 | } 214 | }; 215 | 216 | #endif /* _HELPER_3DMATH_H_ */ --------------------------------------------------------------------------------