├── BalancingRotaped ├── CAD │ └── Roteped.stp └── Code │ ├── IMU │ ├── IMU.ino │ ├── MPU6050_6Axis_MotionApps20.h │ └── readangle.ino │ └── Rotaped │ ├── ODriveInit.ino │ ├── Rotaped.ino │ └── thresholdSticks.ino ├── LICENSE └── README.md /BalancingRotaped/Code/IMU/IMU.ino: -------------------------------------------------------------------------------- 1 | #include "I2Cdev.h" 2 | #include "MPU6050_6Axis_MotionApps20.h" 3 | #include "Wire.h" 4 | 5 | // class default I2C address is 0x68 6 | // specific I2C addresses may be passed as a parameter here 7 | // AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board) 8 | // AD0 high = 0x69 9 | MPU6050 mpu; 10 | //MPU6050 mpu(0x69); // <-- use for AD0 high 11 | 12 | #define LED_PIN 13 // (Arduino is 11, Teensy is 11, Teensy++ is 6) 13 | bool blinkState = false; 14 | 15 | // MPU control/status vars 16 | bool dmpReady = false; // set true if DMP init was successful 17 | uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 18 | uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 19 | uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 20 | uint16_t fifoCount; // count of all bytes currently in FIFO 21 | uint8_t fifoBuffer[64]; // FIFO storage buffer 22 | 23 | // orientation/motion vars 24 | Quaternion q; // [w, x, y, z] quaternion container 25 | VectorInt16 aa; // [x, y, z] accel sensor measurements 26 | VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements 27 | VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements 28 | VectorFloat gravity; // [x, y, z] gravity vector 29 | float euler[3]; // [psi, theta, phi] Euler angle container 30 | float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector 31 | 32 | 33 | // packet structure for InvenSense teapot demo 34 | uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; 35 | 36 | unsigned long currentMillis; 37 | 38 | long previousMillis = 0; // set up timers 39 | long interval = 20; // time constant for timers 40 | unsigned long count; 41 | 42 | int IMUdataReady = 0; 43 | 44 | volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high 45 | 46 | // ****************** SETUP ****************************** 47 | 48 | void setup() { 49 | // join I2C bus (I2Cdev library doesn't do this automatically) 50 | #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE 51 | Wire.begin(); 52 | TWBR = 24; // 400kHz I2C clock (200kHz if CPU is 8MHz). Comment this line if having compilation difficulties with TWBR. 53 | #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE 54 | Fastwire::setup(400, true); 55 | #endif 56 | 57 | // initialize serial communication 58 | Serial.begin(115200); 59 | 60 | // initialize device 61 | mpu.initialize(); 62 | devStatus = mpu.dmpInitialize(); 63 | 64 | // supply your own gyro offsets here, scaled for min sensitivity 65 | mpu.setXGyroOffset(113); 66 | mpu.setYGyroOffset(-22); 67 | mpu.setZGyroOffset(10); 68 | mpu.setXAccelOffset(-1315); 69 | mpu.setYAccelOffset(-544); 70 | mpu.setZAccelOffset(1119); 71 | 72 | // make sure it worked (returns 0 if so) 73 | if (devStatus == 0) { 74 | // turn on the DMP, now that it's ready 75 | mpu.setDMPEnabled(true); 76 | 77 | // enable Arduino interrupt detection 78 | attachInterrupt(0, dmpDataReady, RISING); 79 | mpuIntStatus = mpu.getIntStatus(); 80 | 81 | // get expected DMP packet size for later comparison 82 | packetSize = mpu.dmpGetFIFOPacketSize(); 83 | } else { 84 | // ERROR! 85 | // 1 = initial memory load failed 86 | // 2 = DMP configuration updates failed 87 | // (if it's going to break, usually the code will be 1) 88 | Serial.print(F("DMP Initialization failed (code ")); 89 | Serial.print(devStatus); 90 | Serial.println(F(")")); 91 | } 92 | 93 | // configure LED for output 94 | pinMode(LED_PIN, OUTPUT); 95 | 96 | delay (1500); 97 | } 98 | 99 | // ********************* MAIN LOOP ******************************* 100 | 101 | void loop() { 102 | 103 | currentMillis = millis(); 104 | if (currentMillis - previousMillis >= interval) { // start timed event 105 | previousMillis = currentMillis; 106 | 107 | if (IMUdataReady == 1) { 108 | readAngles(); 109 | } 110 | 111 | 112 | Serial.print(ypr[1] * 180/M_PI); 113 | Serial.print('\n'); 114 | 115 | 116 | 117 | } // end of timed event 118 | 119 | } // end of main loop 120 | -------------------------------------------------------------------------------- /BalancingRotaped/Code/IMU/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, 0x07 // 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::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { 651 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) 652 | if (packet == 0) packet = dmpPacketBuffer; 653 | v -> x = (packet[16] << 8) + packet[17]; 654 | v -> y = (packet[20] << 8) + packet[21]; 655 | v -> z = (packet[24] << 8) + packet[25]; 656 | return 0; 657 | } 658 | // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 659 | // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 660 | uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 661 | // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 662 | v -> x = vRaw -> x - gravity -> x*8192; 663 | v -> y = vRaw -> y - gravity -> y*8192; 664 | v -> z = vRaw -> z - gravity -> z*8192; 665 | return 0; 666 | } 667 | // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 668 | uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 669 | // rotate measured 3D acceleration vector into original state 670 | // frame of reference based on orientation quaternion 671 | memcpy(v, vReal, sizeof(VectorInt16)); 672 | v -> rotate(q); 673 | return 0; 674 | } 675 | // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 676 | // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 677 | // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 678 | // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 679 | // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 680 | uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 681 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 682 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 683 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 684 | return 0; 685 | } 686 | // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 687 | // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 688 | // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 689 | // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 690 | 691 | uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 692 | 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 693 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 694 | 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 695 | return 0; 696 | } 697 | uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 698 | // yaw: (about Z axis) 699 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 700 | // pitch: (nose up/down, about Y axis) 701 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 702 | // roll: (tilt left/right, about X axis) 703 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 704 | return 0; 705 | } 706 | 707 | // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 708 | // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 709 | 710 | uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 711 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 712 | if (dmpData[k] < 0x10) Serial.print("0"); 713 | Serial.print(dmpData[k], HEX); 714 | Serial.print(" "); 715 | } 716 | Serial.print("\n");*/ 717 | //Serial.println((uint16_t)dmpPacketBuffer); 718 | return 0; 719 | } 720 | uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 721 | uint8_t status; 722 | uint8_t buf[dmpPacketSize]; 723 | for (uint8_t i = 0; i < numPackets; i++) { 724 | // read packet from FIFO 725 | getFIFOBytes(buf, dmpPacketSize); 726 | 727 | // process packet 728 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 729 | 730 | // increment external process count variable, if supplied 731 | if (processed != 0) *processed++; 732 | } 733 | return 0; 734 | } 735 | 736 | // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 737 | 738 | // uint8_t MPU6050::dmpInitFIFOParam(); 739 | // uint8_t MPU6050::dmpCloseFIFO(); 740 | // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 741 | // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 742 | // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 743 | // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 744 | // void MPU6050::dmpOverrideQuaternion(long *q); 745 | uint16_t MPU6050::dmpGetFIFOPacketSize() { 746 | return dmpPacketSize; 747 | } 748 | 749 | #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ 750 | -------------------------------------------------------------------------------- /BalancingRotaped/Code/IMU/readangle.ino: -------------------------------------------------------------------------------- 1 | // IMU interrupt service routine 2 | 3 | void dmpDataReady() { 4 | IMUdataReady = 1; 5 | } 6 | 7 | 8 | // function that actually read the angle when the flag is set by the ISR 9 | 10 | void readAngles() { 11 | 12 | mpuIntStatus = mpu.getIntStatus(); 13 | fifoCount = mpu.getFIFOCount(); 14 | 15 | if ((mpuIntStatus & 0x10) || fifoCount == 1024) { 16 | // reset so we can continue cleanly 17 | mpu.resetFIFO(); 18 | Serial.println(F("FIFO overflow!")); 19 | } 20 | 21 | else if (mpuIntStatus & 0x02) { 22 | // wait for correct available data length, should be a VERY short wait 23 | while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount(); 24 | 25 | // read a packet from FIFO 26 | mpu.getFIFOBytes(fifoBuffer, packetSize); 27 | 28 | // track FIFO count here in case there is > 1 packet available 29 | // (this lets us immediately read more without waiting for an interrupt) 30 | fifoCount -= packetSize; 31 | 32 | mpu.dmpGetQuaternion(&q, fifoBuffer); 33 | mpu.dmpGetGravity(&gravity, &q); 34 | mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); 35 | 36 | // blink LED to indicate activity 37 | blinkState = !blinkState; 38 | digitalWrite(LED_PIN, blinkState); 39 | 40 | IMUdataReady = 0; 41 | 42 | count = count + 1; 43 | } 44 | } 45 | 46 | -------------------------------------------------------------------------------- /BalancingRotaped/Code/Rotaped/ODriveInit.ino: -------------------------------------------------------------------------------- 1 | // init hips 2 | 3 | void OdriveInit1() { 4 | 5 | //Serial2 << "w axis" << axis << ".controller.config.vel_limit " << 60000.0f << '\n'; 6 | //Serial2 << "w axis" << axis << ".motor.config.current_lim " << 20.0f << '\n'; 7 | //Serial2 << "w axis" << axis << ".motor.config.calibration_current " << 10.0f << '\n'; 8 | 9 | // *** current limit and other parameers are set on the ODrive using the Odrive tool as above. *** 10 | 11 | // init ODrive axis 12 | 13 | //axis 0 14 | 15 | requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; 16 | odrive1.run_state(0, requested_state, true); 17 | 18 | requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; 19 | odrive1.run_state(0, requested_state, true); 20 | 21 | requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; 22 | odrive1.run_state(0, requested_state, false); // don't wait 23 | 24 | //axis 1 25 | 26 | requested_state = ODriveArduino::AXIS_STATE_MOTOR_CALIBRATION; 27 | odrive1.run_state(1, requested_state, true); 28 | 29 | requested_state = ODriveArduino::AXIS_STATE_ENCODER_OFFSET_CALIBRATION; 30 | odrive1.run_state(1, requested_state, true); 31 | 32 | requested_state = ODriveArduino::AXIS_STATE_CLOSED_LOOP_CONTROL; 33 | odrive1.run_state(1, requested_state, false); // don't wait 34 | 35 | delay(500); 36 | 37 | 38 | 39 | } 40 | -------------------------------------------------------------------------------- /BalancingRotaped/Code/Rotaped/Rotaped.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | // Radio 4 | #include 5 | #include 6 | #include 7 | 8 | RF24 radio(7, 8); // CE, CSN 9 | const byte addresses[][6] = {"00001", "00002"}; 10 | 11 | //ODrive 12 | #include 13 | 14 | //ODrive Objects 15 | ODriveArduino odrive1(Serial2); 16 | 17 | //**************remote control**************** 18 | struct RECEIVE_DATA_STRUCTURE{ 19 | //put your variable definitions here for the data you want to send 20 | //THIS MUST BE EXACTLY THE SAME ON THE OTHER ARDUINO 21 | int16_t menuDown; 22 | int16_t Select; 23 | int16_t menuUp; 24 | int16_t toggleBottom; 25 | int16_t toggleTop; 26 | int16_t toggle1; 27 | int16_t toggle2; 28 | int16_t mode; 29 | int16_t RLR; 30 | int16_t RFB; 31 | int16_t RT; 32 | int16_t LLR; 33 | int16_t LFB; 34 | int16_t LT; 35 | }; 36 | 37 | RECEIVE_DATA_STRUCTURE mydata_remote; 38 | 39 | float RLR = 0; 40 | float RFB = 0; 41 | float RT = 0; 42 | float LLR = 0; 43 | float LFB = 0; 44 | float LT = 0; 45 | 46 | unsigned long currentMillis; 47 | unsigned long previousMillis; 48 | 49 | int requested_state; 50 | 51 | // Balancing PID 52 | 53 | double Pk1 = 4500; 54 | double Ik1 = 5500; 55 | double Dk1 = 38; 56 | 57 | double SetpointAccum; 58 | double Output1a; 59 | 60 | double Setpoint1, Input1, Output1; // PID variables 61 | PID PID1(&Input1, &Output1, &Setpoint1, Pk1, Ik1 , Dk1, DIRECT); // PID Setup 62 | 63 | float pot; 64 | float IMUroll; 65 | float IMUpitch; 66 | 67 | int sw1; // Odrive init 68 | float pot2; // setpoint trim for single balancing wheel 69 | 70 | 71 | void setup() { 72 | // put your setup code here, to run once: 73 | 74 | pinMode(5, INPUT_PULLUP); // ODrive init 75 | 76 | PID1.SetMode(AUTOMATIC); 77 | PID1.SetOutputLimits(-150000, 150000); 78 | PID1.SetSampleTime(10); 79 | 80 | pinMode(A0, INPUT); // setpoint trim 81 | 82 | radio.begin(); 83 | radio.openWritingPipe(addresses[0]); // 00002 84 | radio.openReadingPipe(1, addresses[1]); // 00001 85 | radio.setPALevel(RF24_PA_MIN); 86 | 87 | radio.startListening(); 88 | 89 | Serial2.begin(115200); // ODrive 90 | Serial3.begin(115200); // read IMU data 91 | Serial.begin(115200); // debug 92 | } 93 | 94 | void loop() { 95 | 96 | currentMillis = millis(); 97 | 98 | if (currentMillis - previousMillis >= 10) { // start timed loop 99 | previousMillis = currentMillis; 100 | 101 | // check for radio data 102 | if (radio.available()) { 103 | radio.read(&mydata_remote, sizeof(RECEIVE_DATA_STRUCTURE)); 104 | } 105 | 106 | else{ Serial.println("no data");} 107 | 108 | // threshold remote data 109 | // some are reversed based on stick wiring in remote 110 | RFB = (thresholdStick(mydata_remote.RFB))*-1; 111 | RLR = thresholdStick(mydata_remote.RLR); 112 | RT = thresholdStick(mydata_remote.RT); 113 | LFB = (thresholdStick(mydata_remote.LFB))*-1; 114 | LLR = thresholdStick(mydata_remote.LLR); 115 | LT = thresholdStick(mydata_remote.LT); 116 | 117 | RFB = RFB/25; // scale driving stick 118 | 119 | IMUroll = Serial3.parseFloat(); // read IMU roll 120 | if (Serial3.read() == '\n') { // end of IMU data 121 | } 122 | 123 | pot2 = analogRead(A0); // setpoint trim 124 | pot2 = (pot2 - 512)/100; 125 | 126 | sw1 = digitalRead(5); // init ODrive 127 | if (sw1 == 0) { 128 | OdriveInit1(); 129 | } 130 | 131 | Setpoint1 = pot2+RFB; // PID calcs 132 | Setpoint1 = constrain(Setpoint1,-8,8); // do not lean further than 8 degrees 133 | Input1 = IMUroll; 134 | PID1.Compute(); 135 | 136 | LT = LT * 100; // steering 137 | 138 | odrive1.SetVelocity(0, (Output1*-1)+LT); // drive motors 139 | odrive1.SetVelocity(1, (Output1)+LT); 140 | 141 | 142 | 143 | } // end of 10ms loop 144 | 145 | 146 | 147 | } 148 | -------------------------------------------------------------------------------- /BalancingRotaped/Code/Rotaped/thresholdSticks.ino: -------------------------------------------------------------------------------- 1 | int thresholdStick (int pos) { 2 | 3 | // get zero centre position 4 | pos = pos - 512; 5 | 6 | // threshold value for control sticks 7 | if (pos > 50) { 8 | pos = pos -50; 9 | } 10 | else if (pos < -50) { 11 | pos = pos +50; 12 | } 13 | else { 14 | pos = 0; 15 | } 16 | 17 | return pos; 18 | } 19 | 20 | 21 | 22 | // motion filter to filter motions and compliance 23 | 24 | float filter(float prevValue, float currentValue, int filter) { 25 | float lengthFiltered = (prevValue + (currentValue * filter)) / (filter + 1); 26 | return lengthFiltered; 27 | } 28 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 James Bruton 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Rotaped 2 | --------------------------------------------------------------------------------